CVector3d.h

Go to the documentation of this file.

//==============================================================================
/*
    Software License Agreement (BSD License)
    Copyright (c) 2003-2016, CHAI3D.
    (www.chai3d.org)

    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:

    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above
    copyright notice, this list of conditions and the following
    disclaimer in the documentation and/or other materials provided
    with the distribution.

    * Neither the name of CHAI3D nor the names of its contributors may
    be used to endorse or promote products derived from this software
    without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    POSSIBILITY OF SUCH DAMAGE. 

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

//------------------------------------------------------------------------------
#ifndef CVector3dH
#define CVector3dH
//------------------------------------------------------------------------------
#include "system/CString.h"
#include "system/CGlobals.h"
#include "math/CConstants.h"
//------------------------------------------------------------------------------
#include <locale>
#include <ostream>
#include <cmath>
#include <string>
#include <sstream>
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
namespace chai3d {
//------------------------------------------------------------------------------

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

//==============================================================================
//==============================================================================
struct cVector3d
{
    //--------------------------------------------------------------------------
    // CONSTRUCTOR & DESTRUCTOR:
    //--------------------------------------------------------------------------

public:

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


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    cVector3d(const double a_x, const double a_y, const double a_z)
    { 
        (*this)(0) = a_x;
        (*this)(1) = a_y;
        (*this)(2) = a_z;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    cVector3d (const cVector3d &a_vector)
    {
        (*this)(0) = a_vector(0);
        (*this)(1) = a_vector(1);
        (*this)(2) = a_vector(2);
    }


#ifdef C_USE_EIGEN

    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    cVector3d (const Eigen::Vector3d &a_vector)
    {
        (*this)(0) = a_vector(0);
        (*this)(1) = a_vector(1);
        (*this)(2) = a_vector(2);
    }

#endif


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    cVector3d(const char* a_string)
    {
        set(a_string); 
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    cVector3d(const std::string& a_string)
    {
        set(a_string);
    }


    //--------------------------------------------------------------------------
    // PUBLIC METHODS
    //--------------------------------------------------------------------------

#ifdef C_USE_EIGEN

    Eigen::Vector3d eigen()
    {
        return (Eigen::Vector3d((*this)(0), (*this)(1), (*this)(2)));
    }

#endif

    inline double x() const
    { 
        return((*this)(0)); 
    }
    
    inline double y() const
    { 
        return((*this)(1)); 
    }
    
    inline double z() const
    {
        return((*this)(2)); 
    }
    
    inline void x(const double a_value)
    { 
        (*this)(0) = a_value; 
    }
    
    inline void y(const double a_value)
    { 
        (*this)(1) = a_value; 
    }
    
    inline void z(const double a_value)
    { 
        (*this)(2) = a_value; 
    }

    inline void zero()
    {
        (*this)(0) = 0.0;
        (*this)(1) = 0.0;
        (*this)(2) = 0.0;
    }


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


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void set(const double& a_x, const double& a_y, const double& a_z)
    {
        (*this)(0) = a_x;
        (*this)(1) = a_y;
        (*this)(2) = a_z;
    }


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

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

        // parse data
        double ax, ay, az;
        std::string str = a_initStr;
        std::locale loc;
        for (int j=0; j<(int)(str.length()); j++) if (!std::isdigit(str[j], loc) && str[j] != '.') str[j] = ' ';
        std::istringstream is(str);
        if (!(is >> ax && !is.fail())) return (false);
        if (!(is >> ay && !is.fail())) return (false);
        if (!(is >> az && !is.fail())) return (false);

        // copy the values we found
        (*this)(0) = ax;
        (*this)(1) = ay;
        (*this)(2) = az;

        // return result
        return (true);
    }


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


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void copyto(cVector3d& a_destination) const
    {
        a_destination(0) = (*this)(0);
        a_destination(1) = (*this)(1);
        a_destination(2) = (*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void copyfrom(const cVector3d &a_source)
    {
        (*this)(0) = a_source(0);
        (*this)(1) = a_source(1);
        (*this)(2) = a_source(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void add(const cVector3d& a_vector)
    {
        (*this)(0) = (*this)(0) + a_vector(0);
        (*this)(1) = (*this)(1) + a_vector(1);
        (*this)(2) = (*this)(2) + a_vector(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void add(const double& a_x,
                    const double& a_y,
                    const double& a_z)
    {
        (*this)(0) = (*this)(0) + a_x;
        (*this)(1) = (*this)(1) + a_y;
        (*this)(2) = (*this)(2) + a_z;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void addr(const cVector3d& a_vector,
                     cVector3d& a_result) const
    {
        a_result(0)  = (*this)(0) + a_vector(0);
        a_result(1)  = (*this)(1) + a_vector(1);
        a_result(2)  = (*this)(2) + a_vector(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void addr(const double& a_x,
                     const double& a_y,
                     const double& a_z,
                     cVector3d& a_result) const
    {
        a_result(0)  = (*this)(0) + a_x;
        a_result(1)  = (*this)(1) + a_y;
        a_result(2)  = (*this)(2) + a_z;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void sub(const cVector3d& a_vector)
    {
        (*this)(0) = (*this)(0) - a_vector(0);
        (*this)(1) = (*this)(1) - a_vector(1);
        (*this)(2) = (*this)(2) - a_vector(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void sub(const double& a_x,
                    const double& a_y,
                    const double& a_z)
    {
        (*this)(0) = (*this)(0) - a_x;
        (*this)(1) = (*this)(1) - a_y;
        (*this)(2) = (*this)(2) - a_z;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void subr(const cVector3d& a_vector,
                     cVector3d& a_result) const
    {
        a_result(0)  = (*this)(0) - a_vector(0);
        a_result(1)  = (*this)(1) - a_vector(1);
        a_result(2)  = (*this)(2) - a_vector(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void subr(const double& a_x,
                     const double& a_y,
                     const double& a_z,
                     cVector3d &a_result) const
    {
        a_result(0)  = (*this)(0) - a_x;
        a_result(1)  = (*this)(1) - a_y;
        a_result(2)  = (*this)(2) - a_z;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void mul(const double &a_scalar)
    {
        (*this)(0) = a_scalar * (*this)(0);
        (*this)(1) = a_scalar * (*this)(1);
        (*this)(2) = a_scalar * (*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void mul(const double &a_scalar0,
                    const double &a_scalar1,
                    const double &a_scalar2)
    {
        (*this)(0) = a_scalar0 * (*this)(0);
        (*this)(1) = a_scalar1 * (*this)(1);
        (*this)(2) = a_scalar2 * (*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void mulr(const double& a_scalar,
                     cVector3d& a_result) const
    {
        a_result(0)  = a_scalar * (*this)(0);
        a_result(1)  = a_scalar * (*this)(1);
        a_result(2)  = a_scalar * (*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void mulr(const double &a_scalar0,
                     const double &a_scalar1,
                     const double &a_scalar2,
                     cVector3d& a_result) const
    {
        a_result(0)  = a_scalar0 * (*this)(0);
        a_result(1)  = a_scalar1 * (*this)(1);
        a_result(2)  = a_scalar2 * (*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void mulElement(const cVector3d& a_vector)
    {
        (*this)(0)*=a_vector(0);
        (*this)(1)*=a_vector(1);
        (*this)(2)*=a_vector(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void mulElementr(const cVector3d& a_vector, 
                            cVector3d& a_result) const
    {
        a_result(0)  = (*this)(0)*a_vector(0);
        a_result(1)  = (*this)(1)*a_vector(1);
        a_result(2)  = (*this)(2)*a_vector(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void div(const double& a_scalar)
    {
        double factor = 1.0 / a_scalar;
        (*this)(0) = (*this)(0) * factor;
        (*this)(1) = (*this)(1) * factor;
        (*this)(2) = (*this)(2) * factor;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void divr(const double& a_scalar, 
                     cVector3d& a_result) const
    {
        double factor = 1.0 / a_scalar;
        a_result(0)  = (*this)(0) * factor;
        a_result(1)  = (*this)(1) * factor;
        a_result(2)  = (*this)(2) * factor;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void negate()
    {
        (*this)(0) = -(*this)(0);
        (*this)(1) = -(*this)(1);
        (*this)(2) = -(*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void negater(cVector3d& a_result) const
    {
        a_result(0)  = -(*this)(0);
        a_result(1)  = -(*this)(1);
        a_result(2)  = -(*this)(2);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void cross(const cVector3d& a_vector)
    {
        // compute cross product
        double a =  ((*this)(1) * a_vector(2) ) - ((*this)(2) * a_vector(1) );
        double b = -((*this)(0) * a_vector(2) ) + ((*this)(2) * a_vector(0) );
        double c =  ((*this)(0) * a_vector(1) ) - ((*this)(1) * a_vector(0) );

        // store result in current vector
        (*this)(0) = a;
        (*this)(1) = b;
        (*this)(2) = c;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void crossr(const cVector3d& a_vector, 
                       cVector3d& a_result) const
    {
        a_result(0)  =  ((*this)(1) * a_vector(2) ) - ((*this)(2) * a_vector(1) );
        a_result(1)  = -((*this)(0) * a_vector(2) ) + ((*this)(2) * a_vector(0) );
        a_result(2)  =  ((*this)(0) * a_vector(1) ) - ((*this)(1) * a_vector(0) );
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline double dot(const cVector3d& a_vector) const
    {
        return(((*this)(0) * a_vector(0) ) + ((*this)(1) * a_vector(1) ) + ((*this)(2) * a_vector(2) ));
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline double length() const
    {
        return(sqrt(((*this)(0) * (*this)(0)) + ((*this)(1) * (*this)(1)) + ((*this)(2) * (*this)(2))));
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline double lengthsq() const
    {
        return(((*this)(0) * (*this)(0)) + ((*this)(1) * (*this)(1)) + ((*this)(2) * (*this)(2)));
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void normalize()
    {
        // compute length of vector
        double len = sqrt(((*this)(0) * (*this)(0)) + ((*this)(1) * (*this)(1)) + ((*this)(2) * (*this)(2)));
        if (len == 0.0) { return; }
        double factor = 1.0 / len;

        // divide current vector by its length
        (*this)(0) = (*this)(0) * factor;
        (*this)(1) = (*this)(1) * factor;
        (*this)(2) = (*this)(2) * factor;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void normalizer(cVector3d& a_result) const
    {
        // compute length of vector
        double len = sqrt(((*this)(0) * (*this)(0)) + ((*this)(1) * (*this)(1)) + ((*this)(2) * (*this)(2)));
        double factor = 1.0 / len;

        // divide current vector by its length
        a_result(0)  = (*this)(0) * factor;
        a_result(1)  = (*this)(1) * factor;
        a_result(2)  = (*this)(2) * factor;
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline void clamp(const double& a_maxLength)
    {
        double len = sqrt(((*this)(0) * (*this)(0)) + ((*this)(1) * (*this)(1)) + ((*this)(2) * (*this)(2)));
        if (a_maxLength == 0)
        {
            (*this)(0) = 0.0;
            (*this)(1) = 0.0;
            (*this)(2) = 0.0;
        }
        else if (len > a_maxLength)
        {
            double factor = a_maxLength / len;
            (*this)(0) = (*this)(0) * factor;
            (*this)(1) = (*this)(1) * factor;
            (*this)(2) = (*this)(2) * factor;
        }
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline double distance(const cVector3d& a_vector) const
    {
        // compute distance between both points
        double dx = (*this)(0) - a_vector(0) ;
        double dy = (*this)(1) - a_vector(1) ;
        double dz = (*this)(2) - a_vector(2) ;

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


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline double distancesq(const cVector3d& a_vector) const
    {
        // compute distance for each element
        double dx = (*this)(0) - a_vector(0) ;
        double dy = (*this)(1) - a_vector(1) ;
        double dz = (*this)(2) - a_vector(2) ;

        // return squared distance
        return(dx*dx + dy*dy + dz*dz);
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline bool equals(const cVector3d& a_vector, 
                       const double a_epsilon = 0.0) const
    {
        // accelerated path for exact equality
        if (a_epsilon == 0.0)
        {
            if ( ((*this)(0) == a_vector(0) ) && ((*this)(1) == a_vector(1) ) && ((*this)(2) == a_vector(2) ) )
            {
                return (true);
            }
            else
            {
                return (false);
            }
        }

        if ((fabs(a_vector(0) - (*this)(0)) < a_epsilon) &&
            (fabs(a_vector(1) - (*this)(1)) < a_epsilon) &&
            (fabs(a_vector(2) - (*this)(2)) < a_epsilon))
        {
            return (true);
        }
        else
        {
            return (false);
        }
    }


    //--------------------------------------------------------------------------
    //--------------------------------------------------------------------------
    inline std::string str(const unsigned int a_precision = 2) const
    {
        std::string result;
        result = (cStr((*this)(0), a_precision) + ", " +
                  cStr((*this)(1), a_precision) + ", " +
                  cStr((*this)(2), a_precision));
        return (result);
    }


    //--------------------------------------------------------------------------
    // OPERATORS:
    //--------------------------------------------------------------------------

public:

    inline void operator/= (const double& a_val)
    {
        double factor = 1.0 / a_val;
        (*this)(0) *= factor;
        (*this)(1) *= factor;
        (*this)(2) *= factor;
    }


    inline void operator*= (const double& a_val)
    {
        (*this)(0) *= a_val;
        (*this)(1) *= a_val;
        (*this)(2) *= a_val;
    }


    inline void operator+= (const cVector3d& a_input)
    {
        (*this)(0) += a_input(0);
        (*this)(1) += a_input(1);
        (*this)(2) += a_input(2);
    }


    inline void operator-= (const cVector3d& a_input)
    {
        (*this)(0) -= a_input(0);
        (*this)(1) -= a_input(1);
        (*this)(2) -= a_input(2);
    }


    inline double& operator() (const int a_index)
    {
        return m_data[a_index];
    }


    inline const double& operator() (const int a_index) const
    {
        return m_data[a_index];
    }


    //--------------------------------------------------------------------------
    // PRIVATE MEMBERS
    //--------------------------------------------------------------------------
    
private:

    double m_data[3];
};


//==============================================================================
// OPERATORS:
//==============================================================================


inline cVector3d operator*(const cVector3d& a_vector, const double a_scale)
{
    return (cVector3d(a_vector(0) * a_scale,
                      a_vector(1) * a_scale,
                      a_vector(2) * a_scale));
}


inline cVector3d operator*(const double a_scale, const cVector3d& a_vector)
{
    return (cVector3d(a_vector(0) * a_scale,
                      a_vector(1) * a_scale,
                      a_vector(2) * a_scale));
}


inline cVector3d operator/(const cVector3d& a_vector, const double a_scale)
{
    return (cVector3d(a_vector(0) / a_scale,
                      a_vector(1) / a_scale,
                      a_vector(2) / a_scale));
}


inline cVector3d operator+(const cVector3d& a_vector0, const cVector3d& a_vector1)
{
    return cVector3d(a_vector0(0) + a_vector1(0), 
                     a_vector0(1) + a_vector1(1), 
                     a_vector0(2) + a_vector1(2));
}


inline cVector3d operator-(const cVector3d& a_vector0, const cVector3d& a_vector1)
{
    return cVector3d(a_vector0(0) - a_vector1(0), 
                     a_vector0(1) - a_vector1(1), 
                     a_vector0(2) - a_vector1(2));
}


inline cVector3d operator-(const cVector3d& a_vector0)
{
    return cVector3d(-a_vector0(0), 
                     -a_vector0(1), 
                     -a_vector0(2));
}


inline double operator*(const cVector3d& a_vector0, const cVector3d& a_vector1)
{
    return (a_vector0(0) * a_vector1(0) + 
            a_vector0(1) * a_vector1(1) + 
            a_vector0(2) * a_vector1(2));
}


static inline std::ostream &operator << (std::ostream &a_os, cVector3d const& a_vector)
{
    a_os << a_vector(0)  << ", " << a_vector(1)  << ", " << a_vector(2) ;
    return (a_os);
}


//------------------------------------------------------------------------------
} // namespace chai3d
//------------------------------------------------------------------------------

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