Vec4.h

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material without having a written signed agreement with Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#ifndef MOMENTUM_VEC4_H
#define MOMENTUM_VEC4_H
 
#include "momentum_namespace.h"
#include "momentum_math.h"
 
#include <iosfwd>
 
#include <agx/agx.h>
#include "Vec3.h"
 
 
namespace MOMENTUM_NAMESPACE
{
  class Vec4
  {
    public:
      typedef double Type;
 
    public:
 
      Vec4(const Vec4& copy );
 
      Vec4();
 
      explicit Vec4( double r );
 
      Vec4( double x, double y, double z, double w );
 
      explicit Vec4(const double v[4] );
 
      Vec4( const Vec3& v3, double w );
 
      static Vec4 random(double min, double max);
 
      static Vec4 random(const Vec4& min, const Vec4& max);
 
      bool operator == ( const Vec4& v ) const;
 
      bool operator != ( const Vec4& v ) const;
 
      static Vec4 componentMin(const Vec4& v1, const Vec4& v2);
 
      static Vec4 componentMax(const Vec4& v1, const Vec4& v2);
 
      double minComponent() const;
 
      double maxComponent() const;
 
      int minElement() const;
 
      int maxElement() const;
 
      void clamp(const Vec4& min, const Vec4& max);
 
      bool equalsZero() const;
 
      void set( double x, double y, double z, double w );
 
      void set( double value );
 
      void set( const Vec4& rhs );
 
#ifndef SWIG
      double& operator [] ( int i );
 
      const double& operator [] ( int i ) const;
#endif
      double x() const;
 
      double y() const;
 
      double z() const;
 
      double w() const;
 
      void setX(double value);
 
      void setY(double value);
 
      void setZ(double value);
 
      void setW(double value);
 
      bool isValid() const;
 
      bool isNaN() const;
 
      bool isFinite() const;
 
      double operator * ( const Vec4& rhs ) const;
 
      const Vec4 operator | ( const Vec4& rhs ) const;
 
      const Vec4 operator * ( double rhs ) const;
 
      Vec4& operator *= ( double rhs );
 
      const Vec4 operator / ( double rhs ) const;
 
      Vec4& operator /= ( double rhs );
 
      const Vec4 operator + ( const Vec4& rhs ) const;
 
      Vec4& operator += ( const Vec4& rhs );
 
      const Vec4 operator - ( const Vec4& rhs ) const;
 
      Vec4& operator -= ( const Vec4& rhs );
 
      const Vec4 operator + ( const double& rhs ) const;
 
      Vec4& operator += ( const double& rhs );
 
      const Vec4 operator - ( const double& rhs ) const;
 
      Vec4& operator -= ( const double& rhs );
 
      const Vec4 operator - () const;
 
      double length() const;
 
      double length2() const;
 
      double distance2(const Vec4& v2) const;
 
      double distance(const Vec4& v2) const;
 
      double normalize();
 
      Vec3 asVec3() const;
 
      std::string __str__() const;
 
    private:
      double m_data[4];
  };    // end of class Vec4
 
 
#ifndef SWIG
  /* Implementation */
 
 
  inline Vec4::Vec4(const Vec4& copy )
  {
    m_data[ 0 ] = copy.m_data[ 0 ];
    m_data[ 1 ] = copy.m_data[ 1 ];
    m_data[ 2 ] = copy.m_data[ 2 ];
    m_data[ 3 ] = copy.m_data[ 3 ];
  }
 
 
  inline Vec4::Vec4()
  {
    m_data[0] = double();
    m_data[1] = double();
    m_data[2] = double();
    m_data[3] = double();
  }
 
 
  inline Vec4::Vec4( double r )
  {
    m_data[0] = m_data[1] = m_data[2] = m_data[3] = r;
  }
 
 
  inline Vec4::Vec4( double x, double y, double z, double w )
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = w;
  }
 
 
  inline Vec4::Vec4(const double v[4] )
  {
    m_data[0] = v[0];
    m_data[1] = v[1];
    m_data[2] = v[2];
    m_data[3] = v[3];
  }
 
 
  inline Vec4::Vec4( const Vec3& v3, double w )
  {
    m_data[0] = double(v3[0]);
    m_data[1] = double(v3[1]);
    m_data[2] = double(v3[2]);
    m_data[3] = w;
  }
 
 
 
  inline bool Vec4::operator == ( const Vec4& v ) const
  {
    return m_data[0] == v.m_data[0] && m_data[1] == v.m_data[1] && m_data[2] == v.m_data[2] && m_data[3] == v.m_data[3];
  }
 
 
  inline bool Vec4::operator != ( const Vec4& v ) const
  {
    return m_data[0] != v.m_data[0] || m_data[1] != v.m_data[1] || m_data[2] != v.m_data[2] || m_data[3] != v.m_data[3];
  }
 
 
 
  inline Vec4 Vec4::componentMin(const Vec4& v1, const Vec4& v2)
  {
    return Vec4(std::min(v1[0], v2[0]), std::min(v1[1], v2[1]), std::min(v1[2], v2[2]), std::min(v1[3], v2[3]));
  }
 
 
  inline Vec4 Vec4::componentMax(const Vec4& v1, const Vec4& v2)
  {
    return Vec4(std::max(v1[0], v2[0]), std::max(v1[1], v2[1]), std::max(v1[2], v2[2]), std::max(v1[3], v2[3]));
  }
 
 
  inline double Vec4::minComponent() const
  {
    return std::min(std::min(std::min(m_data[0], m_data[1]), m_data[2]), m_data[3]);
  }
 
 
  inline double Vec4::maxComponent() const
  {
    return std::max(std::max(std::max(m_data[0], m_data[1]), m_data[2]), m_data[3]);
  }
 
 
  inline int Vec4::minElement() const
  {
    double m = std::numeric_limits<double>::infinity();
    int idx = 0;
    for(int i = 0; i < 4; i++) {
      double a = agx::absolute(m_data[i]);
      if ( a < m) {
        idx = i;
        m = a;
      }
    }
    return idx;
  }
 
 
  inline int Vec4::maxElement() const
  {
    double m = 0;
    int idx = 0;
    for(int i = 0; i < 4; i++) {
      double a = agx::absolute(m_data[i]);
      if ( a > m) {
        idx = i;
        m = a;
      }
    }
    return idx;
  }
 
 
  inline void Vec4::clamp(const Vec4& min, const Vec4& max)
  {
    m_data[0] = agx::clamp( m_data[0], min.m_data[0], max.m_data[0] );
    m_data[1] = agx::clamp( m_data[1], min.m_data[1], max.m_data[1] );
    m_data[2] = agx::clamp( m_data[2], min.m_data[2], max.m_data[2] );
    m_data[3] = agx::clamp( m_data[3], min.m_data[3], max.m_data[3] );
  }
 
 
 
  inline bool Vec4::equalsZero() const
  {
    return (agx::equalsZero(m_data[0]) && agx::equalsZero(m_data[1]) && agx::equalsZero(m_data[2]) && agx::equalsZero(m_data[3]));
  }
 
 
 
 
  inline void Vec4::set( double x, double y, double z, double w )
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = w;
  }
 
 
  inline void Vec4::set( double value )
  {
    m_data[0] = m_data[1] = m_data[2] = m_data[3] = value;
  }
 
 
  inline void Vec4::set( const Vec4& rhs )
  {
    m_data[0] = rhs.m_data[0];
    m_data[1] = rhs.m_data[1];
    m_data[2] = rhs.m_data[2];
    m_data[3] = rhs.m_data[3];
  }
 
 
  inline double& Vec4::operator [] ( int i )
  {
    return m_data[i];
  }
 
 
  inline const double& Vec4::operator [] ( int i ) const
  {
    return m_data[i];
  }
 
 
  inline double Vec4::x() const
  {
    return m_data[0];
  }
 
 
  inline double Vec4::y() const
  {
    return m_data[1];
  }
 
  inline double Vec4::z() const
  {
    return m_data[2];
  }
 
 
  inline double Vec4::w() const
  {
    return m_data[3];
  }
 
 
  inline void Vec4::setX(double value)
  {
    m_data[0] = value;
  }
 
 
  inline void Vec4::setY(double value)
  {
    m_data[1] = value;
  }
 
 
  inline void Vec4::setZ(double value)
  {
    m_data[2] = value;
  }
 
 
  inline void Vec4::setW(double value)
  {
    m_data[3] = value;
  }
 
 
  inline bool Vec4::isValid() const
  {
    return !isNaN();
  }
 
 
  inline bool Vec4::isNaN() const
  {
    return agx::isNaN( m_data[0] ) || agx::isNaN( m_data[1] ) || agx::isNaN( m_data[2] ) || agx::isNaN( m_data[3] );
  }
 
 
  inline bool Vec4::isFinite() const
  {
    return agx::isFinite( m_data[0] ) && agx::isFinite( m_data[1] ) && agx::isFinite( m_data[2] ) && agx::isFinite( m_data[3] );
  }
 
 
  inline double Vec4::operator * ( const Vec4& rhs ) const
  {
    return m_data[0] * rhs.m_data[0] + m_data[1] * rhs.m_data[1] + m_data[2] * rhs.m_data[2] + m_data[3] * rhs.m_data[3];
  }
 
 
  inline const Vec4 Vec4::operator | ( const Vec4& rhs ) const
  {
    return Vec4( m_data[0] * rhs.m_data[0],
                  m_data[1] * rhs.m_data[1],
                  m_data[2] * rhs.m_data[2],
                  m_data[3] * rhs.m_data[3] );
  }
 
 
  inline const Vec4 Vec4::operator * ( double rhs ) const
  {
    return Vec4( m_data[0] * rhs, m_data[1] * rhs, m_data[2] * rhs, m_data[3] * rhs );
  }
 
 
 
  inline Vec4& Vec4::operator *= ( double rhs )
  {
    m_data[0] = m_data[0] * rhs;
    m_data[1] = m_data[1] * rhs;
    m_data[2] = m_data[2] * rhs;
    m_data[3] = m_data[3] * rhs;
    return *this;
  }
 
 
  inline const Vec4 Vec4::operator / ( double rhs ) const
  {
    return Vec4( m_data[0] / rhs, m_data[1] / rhs, m_data[2] / rhs, m_data[3] / rhs );
  }
 
 
  inline Vec4& Vec4::operator /= ( double rhs )
  {
    m_data[0] = m_data[0] / rhs;
    m_data[1] = m_data[1] / rhs;
    m_data[2] = m_data[2] / rhs;
    m_data[3] = m_data[3] / rhs;
    return *this;
  }
 
 
 
  inline const Vec4 Vec4::operator + ( const Vec4& rhs ) const
  {
    return Vec4( m_data[0] + rhs.m_data[0], m_data[1] + rhs.m_data[1], m_data[2] + rhs.m_data[2], m_data[3] + rhs.m_data[3] );
  }
 
 
  inline Vec4& Vec4::operator += ( const Vec4& rhs )
  {
    m_data[0] += rhs.m_data[0];
    m_data[1] += rhs.m_data[1];
    m_data[2] += rhs.m_data[2];
    m_data[3] += rhs.m_data[3];
    return *this;
  }
 
 
  inline const Vec4 Vec4::operator - ( const Vec4& rhs ) const
  {
    return Vec4( m_data[0] - rhs.m_data[0], m_data[1] - rhs.m_data[1], m_data[2] - rhs.m_data[2], m_data[3] - rhs.m_data[3] );
  }
 
 
  inline Vec4& Vec4::operator -= ( const Vec4& rhs )
  {
    m_data[0] -= rhs.m_data[0];
    m_data[1] -= rhs.m_data[1];
    m_data[2] -= rhs.m_data[2];
    m_data[3] -= rhs.m_data[3];
    return *this;
  }
 
 
  inline const Vec4 Vec4::operator + ( const double& rhs ) const
  {
    return Vec4( m_data[0] + rhs, m_data[1] + rhs, m_data[2] + rhs, m_data[3] + rhs );
  }
 
 
  inline Vec4& Vec4::operator += ( const double& rhs )
  {
    m_data[0] += rhs;
    m_data[1] += rhs;
    m_data[2] += rhs;
    m_data[3] += rhs;
    return *this;
  }
 
 
  inline const Vec4 Vec4::operator - ( const double& rhs ) const
  {
    return Vec4( m_data[0] - rhs, m_data[1] - rhs, m_data[2] - rhs, m_data[3] - rhs );
  }
 
 
  inline Vec4& Vec4::operator -= ( const double& rhs )
  {
    m_data[0] -= rhs;
    m_data[1] -= rhs;
    m_data[2] -= rhs;
    m_data[3] -= rhs;
    return *this;
  }
 
 
  inline const Vec4 Vec4::operator - () const
  {
    return Vec4 ( -m_data[0], -m_data[1], -m_data[2], -m_data[3] );
  }
 
 
  inline double Vec4::length() const
  {
    return std::sqrt( length2() );
  }
 
 
  inline double Vec4::length2() const
  {
    return double(m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2] + m_data[3] * m_data[3]);
  }
 
 
 
  inline double Vec4::distance(const Vec4& v2) const
  {
    return std::sqrt(distance2(v2));
  }
 
 
  inline double Vec4::normalize()
  {
    double norm = Vec4::length();
    if ( norm > 0.0 ) {
      double inv = 1.0 / norm;
      m_data[0] = double(m_data[0] * inv);
      m_data[1] = double(m_data[1] * inv);
      m_data[2] = double(m_data[2] * inv);
      m_data[3] = double(m_data[3] * inv);
    }
    return( norm );
  }
 
 
 
 
  inline Vec3 Vec4::asVec3() const
  {
    return Vec3( m_data[0], m_data[1], m_data[2] );
  }
 
 
  inline Vec4 Vec4::random(double min, double max)
  {
    return Vec4(agx::random(min, max), agx::random(min, max), agx::random(min, max), agx::random(min, max));
  }
 
 
  inline Vec4 Vec4::random(const Vec4& min, const Vec4& max)
  {
    return Vec4(agx::random(min[0], max[0]), agx::random(min[1], max[1]), agx::random(min[2], max[2]), agx::random(min[3], max[3]));
  }
 
 
  inline std::ostream& operator << ( std::ostream& output, const Vec4& v )
  {
    output << v[0] << " " << v[1] << " " << v[2] << " " << v[3];
    return output;
  }
 
 
  inline std::string Vec4::__str__() const
  {
    std::ostringstream stream;
    stream << *this;
    return stream.str();
  }
 
 
  inline double operator * ( const Vec3& lhs, const Vec4& rhs )
  {
    return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + rhs[3];
  }
 
  inline double operator * ( const Vec4& lhs, const Vec3& rhs )
  {
    return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3];
  }
 
  inline double Vec4::distance2(const Vec4& v2) const
  {
    Vec4 diff(m_data[0] - v2.m_data[0], m_data[1] - v2.m_data[1], m_data[2] - v2.m_data[2], m_data[3] - v2.m_data[3]);
    return diff.length2();
  }
#endif // "#ifndef SWIG"
 
 
  inline bool equivalent(const Vec4& a, const Vec4& b, double epsilon = double(MOMENTUM_EQUIVALENT_EPSILON))
  {
    return agx::equivalent(a[0], b[0], epsilon) &&
      agx::equivalent(a[1], b[1], epsilon) &&
      agx::equivalent(a[2], b[2], epsilon) &&
      agx::equivalent(a[3], b[3], epsilon);
  }
 
 
  inline Vec4 absolute(const Vec4& a)
  {
    return Vec4(std::abs(a[0]),
      std::abs(a[1]),
      std::abs(a[2]),
      std::abs(a[3]));
  }
 
 
  inline Vec4 clamp(const Vec4& vec, const Vec4& minimum, const Vec4& maximum)
  {
    Vec4 result = vec;
    result.clamp(minimum, maximum);
    return result;
  }
 
}    // end of namespace momentum
 
#endif