Vec3.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_VEC3_H
#define MOMENTUM_VEC3_H
 
#include "momentum_namespace.h"
#include "momentum_export.h"
 
 
#include <agx/PushDisableWarnings.h> // Disabling warnings. Include agx/PopDisableWarnings.h below!
#include <iosfwd>
#include <agx/PopDisableWarnings.h> // End of disabled warnings.
 
#include <agx/agx.h>
#include <agx/Vec3.h>
#include <sstream>
#include "momentum_math.h"
 
 
namespace MOMENTUM_NAMESPACE
{
 
  class Vec3
  {
    public:
      typedef double Type;
 
 
#ifndef SWIG
      operator agx::Vec3() const { return agx::Vec3(x(), y(), z()); }
      operator agx::Vec3f() const { return agx::Vec3f((agx::Real32)x(), (agx::Real32)y(), (agx::Real32)z()); }
 
      Vec3(const agx::Vec3& copy)
      {
        set(copy.x(), copy.y(), copy.z());
      }
 
      Vec3(const agx::Vec3f& copy)
      {
        set(copy.x(), copy.y(), copy.z());
      }
 
#endif
 
      Vec3(const Vec3& copy );
 
      // Copy constructor with a new length
      Vec3(const Vec3& copy, double length);
 
      // Copy constructor with one modified element, for x, y and z setters.
      Vec3(const Vec3& copy, double value, int i);
 
 
      static Vec3 random(double min = double(0), double max = double(1));
 
      static Vec3 random(const Vec3& min, const Vec3& max);
 
      Vec3();
 
      explicit Vec3( double r );
 
      // Initialize 3d vector from \p x \p y \z values
      Vec3( double x, double y, double z );
 
      explicit Vec3(const double v[3] );
 
      bool operator == ( const Vec3& v ) const;
 
      bool operator != ( const Vec3& v ) const;
 
      static Vec3 componentMin(const Vec3& v1, const Vec3& v2);
 
      static Vec3 componentMax(const Vec3& v1, const Vec3& v2);
 
      double minComponent() const;
 
      double maxComponent() const;
 
      int minElement() const;
 
      int maxElement() const;
 
      void clamp(const Vec3& min, const Vec3& max);
 
      bool equalsZero() const;
 
      void set( double x, double y, double z );
 
      void set( double value );
 
      void set( const Vec3& rhs );
 
#ifndef SWIG
      double& operator [] ( int i );
 
      const double operator [] (int i) const;
#endif
 
      double x() const;
 
      double y() const;
 
      double z() const;
 
      void setX( double value );
 
      void setY(double value);
 
      void setZ(double value);
 
      bool isValid() const;
 
      bool isNaN() const;
 
      bool isFinite() const; // Non NAN, not infinite.
 
      double operator * ( const Vec3& rhs ) const;
 
      const Vec3 operator ^ ( const Vec3& rhs ) const;
 
      const Vec3 cross(const Vec3& rhs) const;
 
 
      static Vec3 mul( const Vec3& lhs, const Vec3& rhs );
 
      static Vec3 div( const Vec3& lhs, const Vec3& rhs );
 
      const Vec3 operator * ( double rhs ) const;
 
      Vec3& operator *= ( double rhs );
 
      const Vec3 operator / ( double rhs ) const;
 
      Vec3& operator /= ( double rhs );
 
      const Vec3 operator + ( const Vec3& rhs ) const;
 
      Vec3& operator += ( const Vec3& rhs );
 
      const Vec3 operator - ( const Vec3& rhs ) const;
 
      Vec3& operator -= ( const Vec3& rhs );
 
      const Vec3 operator + ( const double rhs ) const;
 
      Vec3& operator += ( const double rhs );
 
      const Vec3 operator - ( const double rhs ) const;
 
      Vec3& operator -= ( const double rhs );
 
      const Vec3 operator - () const;
 
      double length() const;
 
      double length2() const;
 
      double distance2(const Vec3& v2) const;
 
      double distance(const Vec3& v2) const;
 
      double normalize();
 
      double setLength(double l);
 
      Vec3 normal() const;
 
      static Vec3 X_AXIS();
 
      static Vec3 Y_AXIS();
 
      static Vec3 Z_AXIS();
 
      static Vec3 AXIS(int i);
 
    std::string __str__() const;
 
    protected:
      double m_data[4];
  };    // end of class Vec3_template
 
 
#ifndef SWIG
  // Implementation
 
  inline Vec3::Vec3(const Vec3& 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 ] = double(0);
  }
 
 
  inline Vec3::Vec3(const Vec3& copy, double length)
  {
    m_data[0] = copy.m_data[0];
    m_data[1] = copy.m_data[1];
    m_data[2] = copy.m_data[2];
    m_data[3] = double(0);
 
    setLength(length);
  }
 
 
  inline Vec3::Vec3(const Vec3& copy, double value, int i)
  {
    m_data[0] = copy.m_data[0];
    m_data[1] = copy.m_data[1];
    m_data[2] = copy.m_data[2];
    m_data[3] = double(0);
 
    m_data[i] = value;
  }
 
 
  inline Vec3::Vec3()
  {
    m_data[0] = double();
    m_data[1] = double();
    m_data[2] = double();
    m_data[3] = double(0);
  }
 
 
  inline Vec3::Vec3( double r )
  {
    m_data[0] = m_data[1] = m_data[2] = r;
    m_data[3] = double(0);
  }
 
 
  inline Vec3::Vec3( double x, double y, double z )
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = double(0);
  }
 
 
  inline Vec3::Vec3(const double v[3] )
  {
    m_data[0] = v[0];
    m_data[1] = v[1];
    m_data[2] = v[2];
    m_data[3] = double(0);
  }
 
 
 
  inline bool Vec3::operator == ( const Vec3& v ) const
  {
    return m_data[0] == v.m_data[0] && m_data[1] == v.m_data[1] && m_data[2] == v.m_data[2];
  }
 
 
  inline bool Vec3::operator != ( const Vec3& v ) const
  {
    return m_data[0] != v.m_data[0] || m_data[1] != v.m_data[1] || m_data[2] != v.m_data[2];
  }
 
 
 
  inline Vec3 Vec3::componentMin(const Vec3& v1, const Vec3& v2)
  {
    return Vec3( std::min( v1[0], v2[0] ), std::min( v1[1], v2[1] ), std::min( v1[2], v2[2] ) );
  }
 
 
  inline Vec3 Vec3::componentMax(const Vec3& v1, const Vec3& v2)
  {
    return Vec3( std::max( v1[0], v2[0] ), std::max( v1[1], v2[1] ), std::max( v1[2], v2[2] ) );
  }
 
 
  inline double Vec3::minComponent() const
  {
    return std::min(std::min(m_data[0], m_data[1]), m_data[2]);
  }
 
 
  inline double Vec3::maxComponent() const
  {
    return std::max(std::max(m_data[0], m_data[1]), m_data[2]);
  }
 
 
  inline int Vec3::minElement() const
  {
    double m = std::numeric_limits<double>::infinity();
    int idx = 0;
    for(int i = 0; i < 3; i++) {
      double a = agx::absolute(m_data[i]);
      if ( a < m) {
        idx = i;
        m = a;
      }
    }
    return idx;
  }
 
 
  inline int Vec3::maxElement() const
  {
    double m = 0;
    int idx = 0;
    for(int i = 0; i < 3; i++) {
      double a = agx::absolute(m_data[i]);
      if ( a > m) {
        idx = i;
        m = a;
      }
    }
    return idx;
  }
 
 
  inline void Vec3::clamp(const Vec3& min, const Vec3& 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] );
  }
 
 
  inline bool Vec3::equalsZero() const
  {
    return (agx::equalsZero(m_data[0]) && agx::equalsZero(m_data[1]) && agx::equalsZero(m_data[2]));
  }
 
 
 
  inline void Vec3::set( double x, double y, double z )
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
  }
 
 
  inline void Vec3::set( double value )
  {
    m_data[0] = m_data[1] = m_data[2] = value;
  }
 
 
  inline void Vec3::set( const Vec3& rhs )
  {
    m_data[0] = rhs.m_data[0];
    m_data[1] = rhs.m_data[1];
    m_data[2] = rhs.m_data[2];
  }
 
 
  inline double& Vec3::operator [] ( int i )
  {
    return m_data[i];
  }
 
 
  inline const double  Vec3::operator [] ( int i ) const
  {
    return m_data[i];
  }
 
 
  inline double Vec3::x() const
  {
    return m_data[0];
  }
 
 
  inline double Vec3::y() const
  {
    return m_data[1];
  }
 
 
  inline double Vec3::z() const
  {
    return m_data[2];
  }
 
 
  inline void Vec3::setX(double value)
  {
    m_data[0] = value;
  }
 
 
  inline void Vec3::setY(double value)
  {
    m_data[1] = value;
  }
 
 
  inline void Vec3::setZ(double value)
  {
    m_data[2] = value;
  }
 
 
  inline bool Vec3::isValid() const
  {
    return !isNaN();
  }
 
 
  inline bool Vec3::isNaN() const
  {
    return agx::isNaN( m_data[0] ) || agx::isNaN( m_data[1] ) || agx::isNaN( m_data[2] );
  }
 
 
  inline bool Vec3::isFinite() const
  {
    return agx::isFinite( m_data[0] ) && agx::isFinite( m_data[1] ) && agx::isFinite( m_data[2] );
  }
 
 
  inline double Vec3::operator * ( const Vec3& rhs ) const
  {
    return double(m_data[0] * rhs.m_data[0] + m_data[1] * rhs.m_data[1] + m_data[2] * rhs.m_data[2]);
  }
 
 
  inline const Vec3 Vec3::operator ^ ( const Vec3& rhs ) const
  {
    return Vec3( m_data[1] * rhs.m_data[2] - m_data[2] * rhs.m_data[1],
                  m_data[2] * rhs.m_data[0] - m_data[0] * rhs.m_data[2] ,
                  m_data[0] * rhs.m_data[1] - m_data[1] * rhs.m_data[0] );
  }
 
 
  inline const Vec3 Vec3::cross(const Vec3& rhs) const
  {
    return Vec3(m_data[1] * rhs.m_data[2] - m_data[2] * rhs.m_data[1],
      m_data[2] * rhs.m_data[0] - m_data[0] * rhs.m_data[2],
      m_data[0] * rhs.m_data[1] - m_data[1] * rhs.m_data[0]);
  }
 
 
  inline Vec3 Vec3::mul( const Vec3& lhs, const Vec3& rhs )
  {    return Vec3( lhs[0] * rhs[0],
                  lhs[1] * rhs[1],
                  lhs[2] * rhs[2] );
  }
 
 
  inline Vec3 Vec3::div( const Vec3& lhs, const Vec3& rhs )
  {
    return Vec3( lhs[0] / rhs[0],
                  lhs[1] / rhs[1],
                  lhs[2] / rhs[2] );
  }
 
 
 
  inline const Vec3 Vec3::operator * ( double rhs ) const
  {
    return Vec3( double(m_data[0] * rhs), double(m_data[1] * rhs), double(m_data[2] * rhs) );
  }
 
 
 
  inline Vec3& Vec3::operator *= ( double rhs )
  {
    m_data[0] = double(m_data[0] * rhs);
    m_data[1] = double(m_data[1] * rhs);
    m_data[2] = double(m_data[2] * rhs);
    return *this;
  }
 
 
  inline const Vec3 Vec3::operator / ( double rhs ) const
  {
    return Vec3( double(m_data[0] / rhs), double(m_data[1] / rhs), double(m_data[2] / rhs) );
  }
 
 
  inline Vec3& Vec3::operator /= ( double rhs )
  {
    m_data[0] = double(m_data[0] / rhs);
    m_data[1] = double(m_data[1] / rhs);
    m_data[2] = double(m_data[2] / rhs);
    return *this;
  }
 
 
 
  inline const Vec3 Vec3::operator + ( const Vec3& rhs ) const
  {
    return Vec3( m_data[0] + rhs[0], m_data[1] + rhs[1], m_data[2] + rhs[2] );
  }
 
 
  inline Vec3& Vec3::operator += ( const Vec3& rhs )
  {
    m_data[0] += rhs.m_data[0];
    m_data[1] += rhs.m_data[1];
    m_data[2] += rhs.m_data[2];
    return *this;
  }
 
 
  inline const Vec3 Vec3::operator - ( const Vec3& rhs ) const
  {
    return Vec3( m_data[0] - rhs[0], m_data[1] - rhs[1], m_data[2] - rhs[2] );
  }
 
 
  inline Vec3& Vec3::operator -= ( const Vec3& rhs )
  {
    m_data[0] -= rhs.m_data[0];
    m_data[1] -= rhs.m_data[1];
    m_data[2] -= rhs.m_data[2];
    return *this;
  }
 
 
  inline const Vec3 Vec3::operator + ( const double rhs ) const
  {
    return Vec3( m_data[0] + rhs, m_data[1] + rhs, m_data[2] + rhs );
  }
 
 
  inline Vec3& Vec3::operator += ( const double rhs )
  {
    m_data[0] += rhs;
    m_data[1] += rhs;
    m_data[2] += rhs;
    return *this;
  }
 
 
  inline const Vec3  Vec3::operator - ( const double rhs ) const
  {
    return Vec3( m_data[0] - rhs, m_data[1] - rhs, m_data[2] - rhs );
  }
 
 
  inline Vec3& Vec3::operator -= ( const double rhs )
  {
    m_data[0] -= rhs;
    m_data[1] -= rhs;
    m_data[2] -= rhs;
    return *this;
  }
 
 
  inline const Vec3 Vec3::operator - () const
  {
    return Vec3 ( -m_data[0], -m_data[1], -m_data[2] );
  }
 
 
  inline double Vec3::length() const
  {
    return std::sqrt( (m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2]) );
  }
 
 
  inline double Vec3::length2() const
  {
    return m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2];
  }
 
 
  inline double  Vec3::distance(const Vec3& v2) const
  {
    return std::sqrt(distance2(v2));
  }
 
 
  inline double  Vec3::normalize()
  {
    return setLength(1.0);
  }
 
  inline double Vec3::setLength(double newLength)
  {
    double norm = Vec3::length();
 
    if (norm > 0.0) {
      double inv = newLength / 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);
 
    }
    return(norm);
  }
 
 
  inline Vec3  Vec3::normal() const
  {
    Vec3 normal = *this;
    normal.normalize();
    return normal;
  }
 
 
 
  inline Vec3 Vec3::X_AXIS()
  {
    return Vec3(1, 0, 0);
  }
 
 
  inline Vec3 Vec3::Y_AXIS()
  {
    return Vec3(0, 1, 0);
  }
 
 
  inline Vec3 Vec3::Z_AXIS()
  {
    return Vec3(0, 0, 1);
  }
 
 
  inline Vec3 Vec3::AXIS(int i)
  {
    Vec3 tmp;
    tmp[i] = double(1);
    return tmp;
  }
 
 
  inline Vec3 Vec3::random(double min, double max)
  {
    return Vec3(agx::random(min, max), agx::random(min, max), agx::random(min, max));
  }
 
 
  inline Vec3 Vec3::random(const Vec3& min, const Vec3& max)
  {
    return Vec3(agx::random(min[0], max[0]), agx::random(min[1], max[1]), agx::random(min[2], max[2]));
  }
#endif
 
 
  inline bool equivalent( const Vec3& a, const Vec3& 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);
  }
 
 
  inline Vec3 absolute( const Vec3& a)
  {
    return Vec3(std::abs(a[0]),
                    std::abs(a[1]),
                    std::abs(a[2]));
  }
 
 
  inline Vec3 asin( const Vec3& a)
  {
    return Vec3(std::asin(a[0]),
                    std::asin(a[1]),
                    std::asin(a[2]));
  }
 
 
  inline Vec3 sinc( const Vec3& a)
  {
    return Vec3(agx::sinc(a[0]),
                    agx::sinc(a[1]),
                    agx::sinc(a[2]));
  }
 
  inline Vec3 operator* ( double val, const Vec3& vec )
  {
    return vec * val;
  }
 
 
  inline Vec3 max( const Vec3& lhs, const Vec3& rhs )
  {
    return Vec3::componentMax(lhs, rhs);
  }
 
 
  inline Vec3 min( const Vec3& lhs, const Vec3& rhs )
  {
    return Vec3::componentMin(lhs, rhs);
  }
 
#ifndef SWIG
 
 
  inline std::ostream& operator << ( std::ostream& output, const Vec3& v )
  {
    output << v[0] << " " << v[1] << " " << v[2];
    return output;
  }
 
 
  inline std::string Vec3::__str__() const
  {
    std::ostringstream stream;
    stream << *this;
    return stream.str();
  }
 
 
  inline double Vec3::distance2(const Vec3& v2) const
  {
    Vec3 diff(m_data[0] - v2.m_data[0], m_data[1] - v2.m_data[1], m_data[2] - v2.m_data[2]);
    return diff.length2();
  }
#endif // "#ifndef SWIG"
 
 
  inline Vec3 clamp( const Vec3& vec, const Vec3& minimum, const Vec3& maximum )
  {
    Vec3 result = vec;
    result.clamp(minimum, maximum);
    return result;
  }
 
}    // end of namespace Momentum
 
 
#endif