D:/CHAI3D/build-2-0-0/2.0.0/win32/src/math/CVector3d.h

Go to the documentation of this file.

//===========================================================================
/*
    This file is part of the CHAI 3D visualization and haptics libraries.
    Copyright (C) 2003-2009 by CHAI 3D. All rights reserved.

    This library is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License("GPL") version 2
    as published by the Free Software Foundation.

    For using the CHAI 3D libraries with software that can not be combined
    with the GNU GPL, and for taking advantage of the additional benefits
    of our support services, please contact CHAI 3D about acquiring a
    Professional Edition License.

    \author    <http://www.chai3d.org>
    \author    Francois Conti
    \author    Dan Morris
    \version   2.0.0 $Rev: 251 $
*/
//===========================================================================

//---------------------------------------------------------------------------
#ifndef CVector3dH
#define CVector3dH
//---------------------------------------------------------------------------
#include "math/CString.h"
#include "math/CConstants.h"
#include "extras/CGlobals.h"
#include <math.h>
#include <ostream>
//---------------------------------------------------------------------------

//===========================================================================
//===========================================================================

//===========================================================================
//===========================================================================
struct cVector3d
{
    public:
        //-----------------------------------------------------------------------
    // MEMBERS:
    //-----------------------------------------------------------------------

    double x;

    double y;

    double z;


    //-----------------------------------------------------------------------
    // CONSTRUCTOR & DESTRUCTOR:
    //-----------------------------------------------------------------------

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    cVector3d() {}

    cVector3d(const char* a_initstr) { set(a_initstr); }

    cVector3d(const string& a_initstr) { set(a_initstr); }

    cVector3d(const double a_x, const double a_y,
        const double a_z) : x(a_x), y(a_y), z(a_z) { }

    cVector3d(const cVector3d& a_source) : x(a_source.x), y(a_source.y), z(a_source.z) { }

    cVector3d(const double* a_in) : x(a_in[0]), y(a_in[1]), z(a_in[2]) { }

    cVector3d(const float* a_in) : x(a_in[0]), y(a_in[1]), z(a_in[2]) { }


    //-----------------------------------------------------------------------
    // OVERLOADED CAST OPERATORS:
    //-----------------------------------------------------------------------

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    double& operator[] (unsigned int index) { return (&x)[index]; }

    double operator[] (unsigned int index) const { return ((double*)(&x))[index]; }


    //-----------------------------------------------------------------------
    // OPERATION METHODS:
    //-----------------------------------------------------------------------

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void zero()
    {
                x = 0.0;
                y = 0.0;
                z = 0.0;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline double get(const unsigned int& a_component) const
    {
        return ((double*)(this))[a_component];
    }

        //-----------------------------------------------------------------------
        //-----------------------------------------------------------------------
    inline void operator/= (const double& a_val)
    {
                x /= a_val;
                y /= a_val;
                z /= a_val;
    }

        //-----------------------------------------------------------------------
        //-----------------------------------------------------------------------
    inline void operator*= (const double& a_val)
    {
                x *= a_val;
                y *= a_val;
                z *= a_val;
    }

        //-----------------------------------------------------------------------
        //-----------------------------------------------------------------------
    inline void operator+= (const cVector3d& a_input)
    {
                x += a_input.x;
                y += a_input.y;
                z += a_input.z;
    }

        //-----------------------------------------------------------------------
        //-----------------------------------------------------------------------
    inline void operator-= (const cVector3d& a_input)
    {
                x -= a_input.x;
                y -= a_input.y;
                z -= a_input.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void set(const double& a_x, const double& a_y, const double& a_z)
    {
      x = a_x;
      y = a_y;
      z = a_z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline bool set(const char* a_initStr)
    {
                if (a_initStr == 0) return false;

                double ax,ay,az;

                // Look for a valid-format string
                // Ignore leading whitespace and ('s
                const char* curpos = a_initStr;
                while(
                        (*curpos != '\0') &&
                        (*curpos == ' ' || *curpos == '\t' || *curpos == '(')
        )
                {
                        curpos++;
        }

                int result = sscanf(curpos,"%lf%*[ ,\t\n\r]%lf%*[ ,\t\n\r]%lf",&ax,&ay,&az);

                // Make sure the conversion worked
                if (result !=3) return false;

                // Copy the values we found
                x = ax; y = ay; z = az;
                return (true);
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline bool set(const string& a_initStr)
    {
                return ( set(a_initStr.c_str()) );
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void copyto(cVector3d& a_destination) const
    {
                a_destination.x = x;
                a_destination.y = y;
                a_destination.z = z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void copyfrom(const cVector3d &a_source)
    {
                x = a_source.x;
                y = a_source.y;
                z = a_source.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void add(const cVector3d& a_vector)
    {
                x = x + a_vector.x;
                y = y + a_vector.y;
                z = z + a_vector.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void add(const double& a_x, const double& a_y, const double& a_z)
    {
                x = x + a_x;
                y = y + a_y;
                z = z + a_z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void addr(const cVector3d& a_vector, cVector3d& a_result) const
    {
      a_result.x = x + a_vector.x;
      a_result.y = y + a_vector.y;
      a_result.z = z + a_vector.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void addr(const double& a_x, const double& a_y, const double& a_z, cVector3d& a_result) const
    {
                a_result.x = x + a_x;
                a_result.y = y + a_y;
                a_result.z = z + a_z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void sub(const cVector3d& a_vector)
    {
                x = x - a_vector.x;
                y = y - a_vector.y;
                z = z - a_vector.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void sub(const double& a_x, const double& a_y, const double& a_z)
    {
                x = x - a_x;
                y = y - a_y;
                z = z - a_z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void subr(const cVector3d& a_vector, cVector3d& a_result) const
    {
                a_result.x = x - a_vector.x;
                a_result.y = y - a_vector.y;
                a_result.z = z - a_vector.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void subr(const double& a_x, const double& a_y, const double& a_z,
                  cVector3d &a_result) const
    {
                a_result.x = x - a_x;
                a_result.y = y - a_y;
                a_result.z = z - a_z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void mul(const double &a_scalar)
    {
                x = a_scalar * x;
                y = a_scalar * y;
                z = a_scalar * z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void mulr(const double& a_scalar, cVector3d& a_result) const
    {
                a_result.x = a_scalar * x;
                a_result.y = a_scalar * y;
                a_result.z = a_scalar * z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void div(const double &a_scalar)
    {
                x = x / a_scalar;
                y = y / a_scalar;
                z = z / a_scalar;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void divr(const double& a_scalar, cVector3d& a_result) const
    {
                a_result.x = x / a_scalar;
                a_result.y = y / a_scalar;
                a_result.z = z / a_scalar;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void negate()
    {
                x = -x;
                y = -y;
                z = -z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void negater(cVector3d& a_result) const
    {
                a_result.x = -x;
                a_result.y = -y;
                a_result.z = -z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void cross(const cVector3d& a_vector)
    {
                // compute cross product
                double a =  (y * a_vector.z) - (z * a_vector.y);
                double b = -(x * a_vector.z) + (z * a_vector.x);
                double c =  (x * a_vector.y) - (y * a_vector.x);

                // store result in current vector
                x = a;
                y = b;
                z = c;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline cVector3d crossAndReturn(const cVector3d& a_vector) const
    {
                cVector3d r;
                crossr(a_vector,r);
                return (r);
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void crossr(const cVector3d& a_vector, cVector3d& a_result) const
    {
                a_result.x =  (y * a_vector.z) - (z * a_vector.y);
                a_result.y = -(x * a_vector.z) + (z * a_vector.x);
                a_result.z =  (x * a_vector.y) - (y * a_vector.x);
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline double dot(const cVector3d& a_vector) const
    {
                return((x * a_vector.x) + (y * a_vector.y) + (z * a_vector.z));
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void elementMul(const cVector3d& a_vector)
    {
                x*=a_vector.x;
                y*=a_vector.y;
                z*=a_vector.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void elementMulr(const cVector3d& a_vector, cVector3d& a_result) const
    {
                a_result.x = x*a_vector.x;
                a_result.y = y*a_vector.y;
                a_result.z = z*a_vector.z;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline double length() const
    {
                return(sqrt((x * x) + (y * y) + (z * z)));
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline double lengthsq(void) const
    {
                return((x * x) + (y * y) + (z * z));
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void normalize()
    {
                // compute length of vector
                double length = sqrt((x * x) + (y * y) + (z * z));

                // divide current vector by its length
                x = x / length;
                y = y / length;
                z = z / length;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void normalizer(cVector3d& a_result) const
    {
                // compute length of vector
                double length = sqrt((x * x) + (y * y) + (z * z));

                // divide current vector by its length
                a_result.x = x / length;
                a_result.y = y / length;
                a_result.z = z / length;
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline double distance(const cVector3d& a_vector) const
    {
                // compute distance between both points
                double dx = x - a_vector.x;
                double dy = y - a_vector.y;
                double dz = z - a_vector.z;

                // return result
                return(sqrt( dx * dx + dy * dy + dz * dz ));
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline double distancesq(const cVector3d& a_vector) const
    {
                // compute distance between both points
                double dx = x - a_vector.x;
                double dy = y - a_vector.y;
                double dz = z - a_vector.z;

                // return result
                return( dx * dx + dy * dy + dz * dz );
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline bool equals(const cVector3d& a_vector, const double epsilon=0.0) const
    {
                // Accelerated path for exact equality
                if (epsilon == 0.0)
                {
                        if ( (x == a_vector.x) && (y == a_vector.y) && (z == a_vector.z) )
                        {
                                return (true);
                        }
                        else
                        {
                                return (false);
                        }
                }

                if ((fabs(a_vector.x-x) < epsilon) &&
                        (fabs(a_vector.y-y) < epsilon) &&
                        (fabs(a_vector.z-z) < epsilon))
                {
                        return (true);
                }
                else
                {
                        return (false);
                }
        }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void str(string& a_string, const unsigned int a_precision=2) const
    {
                a_string.append("( ");
                cStr(a_string, x, a_precision);
                a_string.append(", ");
                cStr(a_string, y, a_precision);
                a_string.append(", ");
                cStr(a_string, z, a_precision);
                a_string.append(" )");
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline string str(const unsigned int a_precision=2) const
    {
                string a_string;
                str(a_string,a_precision);
                return (a_string);
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void print(const unsigned int a_precision=2) const
    {
        string s;
        str(s,a_precision);
        CHAI_DEBUG_PRINT("%s\n",s.c_str());
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void decompose(const cVector3d& a_input, cVector3d& a_parallel, cVector3d& a_perpendicular)
    {
                double scale = (this->dot(a_input) / (a_input.dot(a_input)));
                a_parallel = a_input;
                a_parallel.mul(scale);
                this->subr(a_parallel,a_perpendicular);
    }

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void slerp(double a_level, cVector3d const& a_vector1, cVector3d a_vector2)
    {
        // a_vector2 is passed in by value so that we may scale it
        double a_vec1lensq = a_vector1.lengthsq();
        double cosomega = a_vector1.dot(a_vector2)/(a_vec1lensq);
        if ((cosomega-1.0) > -1e-4 && (cosomega-1.0) < 1e-4) {
            // vectors are (almost) parallel
            // linearly interpolate
            *this = a_vector1;
            this->mul(1.0-a_level);
            a_vector2.mul(a_level);
            this->operator +=(a_vector2);
            this->mul(sqrt(a_vec1lensq/this->lengthsq()));
        } else {
            if (cosomega < 0.0) {
                cosomega = -cosomega;
                a_vector2.negate();
            }
            double ratio1, ratio2;
            if ((cosomega+1.0) > -1e-4 && (cosomega+1.0) < 1e-4) {
                // vectors are 180 degrees apart
                // there is no unique path between them
                if ((a_vector1.x < a_vector1.y) && (a_vector1.x < a_vector1.z)){
                    // x component is the smallest
                    a_vector2.x = 0;
                    a_vector2.y = -a_vector1.z;
                    a_vector2.z = a_vector1.y;
                } else if (a_vector1.y < a_vector1.z) {
                    // y component is the smallest
                    a_vector2.x = -a_vector1.z;
                    a_vector2.y = 0;
                    a_vector2.z = a_vector1.x;
                } else {
                    // z component is the smallest
                    a_vector2.x = -a_vector1.y;
                    a_vector2.y = a_vector1.x;
                    a_vector2.z = 0;
                }
                // scale it so it is the same length as before
                a_vector2.mul(sqrt(a_vec1lensq/a_vector2.lengthsq()));

                ratio1 = sin(CHAI_PI*(0.5-a_level));
                ratio2 = sin(CHAI_PI*a_level);
            } else {
                double omega = acos(cosomega);
                double sinomega = sin(omega);
                ratio1 = sin(omega*(1.0-a_level))/sinomega;
                ratio2 = sin(omega*a_level)/sinomega;
            }
            *this = a_vector1;
            this->mul(ratio1);
            a_vector2.mul(ratio2);
            this->add(a_vector2);
        }
    }
};


//===========================================================================
// Operators on cVector3d
//===========================================================================

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
inline cVector3d operator*(const cVector3d& v, const double a_input)
{
        return (cVector3d(v.x*a_input,v.y*a_input,v.z*a_input));
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
inline cVector3d operator/(const cVector3d& v, const double a_input)
{
        return (cVector3d(v.x/a_input,v.y/a_input,v.z/a_input));
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
inline cVector3d operator*(const double a_input, const cVector3d& v)
{
    return cVector3d(v.x*a_input,v.y*a_input,v.z*a_input);
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
inline cVector3d operator+(const cVector3d& v1, const cVector3d& v2)
{
    return cVector3d(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z);
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
inline cVector3d operator-(const cVector3d& v1, const cVector3d& v2)
{
    return cVector3d(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z);
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
inline double operator*(const cVector3d& v1, const cVector3d& v2)
{
    return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z;
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
static inline std::ostream &operator << (std::ostream &a_os, cVector3d const& a_vec)
{
    a_os << a_vec.x << ", " << a_vec.y << ", " << a_vec.z;
    return a_os;
}


//===========================================================================
//===========================================================================
struct cRay3d
{
    cVector3d m_origin;

    cVector3d m_direction;

    cRay3d();

    cRay3d(const cVector3d& a_origin, const cVector3d& a_direction) :
        m_origin(a_origin), m_direction(a_direction)
        {  }
};


//===========================================================================
//===========================================================================
struct cSegment3d
{
    cSegment3d(const cVector3d& a_start, const cVector3d& a_end) :
        m_start(a_start), m_end(a_end)
        { }

    cVector3d m_start;

    cVector3d m_end;

    double distanceSquaredToPoint(const cVector3d& a_point,
                                  double& a_t,
                                  cVector3d* a_closestPoint=0);
};



//---------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------

---

CHAI3D 2.0.0 documentation
Please address any questions to support@chai3d.org
(C) 2003-2009 - CHAI 3D
All Rights Reserved.