CMaths.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
    \version   3.2.0 $Rev: 2163 $
*/
//==============================================================================

//------------------------------------------------------------------------------
#ifndef CMathsH
#define CMathsH
//------------------------------------------------------------------------------
#include "math/CTransform.h"
#include <math.h>
//------------------------------------------------------------------------------

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

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

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------


//==============================================================================
//==============================================================================
inline bool cCheckBit(const unsigned int& a_value, const unsigned int& a_bitPosition)
{
    if ((a_value & (1<<a_bitPosition)) > 0)
    {
        return (true);
    }
    else
    {
        return (false);
    }
}


//==============================================================================
//==============================================================================
inline unsigned int cSetBit(const unsigned int& a_data, const unsigned int& a_bitPosition, const bool a_value)
{
    if (a_value)
    {
        return (a_data | (1<<a_bitPosition));
    }
    else
    {
        return (a_data & !(1<<a_bitPosition));
    }
}


//==============================================================================
//==============================================================================
inline bool cZero(const double& a_value)
{
    return ((a_value < C_TINY) && (a_value > -C_TINY));
}


//==============================================================================
//==============================================================================
template<class T> inline T cAbs(const T& a_value)
{
    return (a_value >= 0 ? a_value : -a_value);
}


//==============================================================================
//==============================================================================
template<class T> inline double cSign(const T& a_value)
{
    if (a_value < 0) 
    {
        return (-1);
    }
    else
    {
        return (1);
    }
}


//==============================================================================
//==============================================================================
template<class T> inline T cMax(const T& a_value1, 
                                const T& a_value2)
{
    return (a_value1 >= a_value2 ? a_value1 : a_value2);
}


//==============================================================================
//==============================================================================
template<class T> inline T cMin(const T& a_value1, 
                                const T& a_value2)
{
    return (a_value1 <= a_value2 ? a_value1 : a_value2);
}


//==============================================================================
//==============================================================================
template<class T> inline T cMax3(const T& a_value1,
                                 const T& a_value2,
                                 const T& a_value3)
{
    return (cMax(a_value1, cMax(a_value2, a_value3)));
}


//==============================================================================
//==============================================================================
template<class T> inline T cMin3(const T& a_value1, 
                                 const T& a_value2, 
                                 const T& a_value3)
{
    return (cMin(a_value1, cMin(a_value2, a_value3)));
}


//==============================================================================
//==============================================================================
template<class T> inline void cSwap(T& a_value1, 
                                    T& a_value2)
{
    T value = a_value1;
    a_value1 = a_value2;
    a_value2 = value;
}


//==============================================================================
//==============================================================================
template<class T> inline T cLerp(const double& a_level, 
                                 const T& a_value1, 
                                 const T& a_value2)
{
    return (a_value2 * a_level + a_value1 * (1 - a_level));
}


//==============================================================================
//==============================================================================
template<class T> inline T cClamp(const T& a_value, 
                                  const T& a_low, 
                                  const T& a_high)
{
    if      (a_value < a_low)  return a_low;
    else if (a_value > a_high) return a_high;
    else                       return a_value;
}


//==============================================================================
//==============================================================================
template<class T> inline T cClamp0(const T& a_value)
{
    return cMax<T>(0, a_value);
}


//==============================================================================
//==============================================================================
inline double cClamp01(const double& a_value)
{
    return (cClamp(a_value, 0.0, 1.0));
}


//==============================================================================
//==============================================================================
template<class T, class V> inline bool cContains(const T& a_value, 
                                                 const V& a_low, 
                                                 const V& a_high)
{
    return ((a_value >= a_low) && (a_value <= a_high));
}


//==============================================================================
//==============================================================================
inline double cSqr(const double& a_value)
{
    return(a_value*a_value);
}


//==============================================================================
//==============================================================================
inline double cSqrt(const double& a_value)
{
    return(sqrt(a_value));
}


//==============================================================================
//==============================================================================
inline double cCbrt(const double& a_value)
{
    return (pow(a_value, 1.0/3.0));
}


//==============================================================================
//==============================================================================
inline double cCosDeg(const double& a_angleDeg)
{
    return (cos(a_angleDeg * C_DEG2RAD));
}


//==============================================================================
//==============================================================================
inline double cSinDeg(const double& a_angleDeg)
{
    return (sin(a_angleDeg * C_DEG2RAD));
}


//==============================================================================
//==============================================================================
inline double cTanDeg(const double& a_angleDeg)
{
    return (tan(a_angleDeg * C_DEG2RAD));
}


//==============================================================================
//==============================================================================
inline double cCosRad(const double& a_angleRad)
{
    return (cos(a_angleRad));
}


//==============================================================================
//==============================================================================
inline double cSinRad(const double& a_angleRad)
{
    return (sin(a_angleRad));
}


//==============================================================================
//==============================================================================
inline double cTanRad(const double& a_angleRad)
{
    return (tan(a_angleRad));
}


//==============================================================================
//==============================================================================
inline double cDegToRad(const double& a_angleDeg)
{
    return (a_angleDeg * C_DEG2RAD);
}


//==============================================================================
//==============================================================================
inline double cRadToDeg(const double& a_angleRad)
{
    return (a_angleRad * C_RAD2DEG);
}


//==============================================================================
//==============================================================================
inline int cNumDigits(int a_value)
{
    int digits = 0;

    if (a_value == 0) { return (1); }

    double value = (double)(abs(a_value));
    while (value >= 1.0)
    {
        value /= 10;
        digits++;
    }
    return (digits);
}


//==============================================================================
//==============================================================================
inline cVector3d cAdd(const cVector3d& a_vector1,
                      const cVector3d& a_vector2)
{
    return cVector3d(
      a_vector1(0)+a_vector2(0),
      a_vector1(1)+a_vector2(1),
      a_vector1(2)+a_vector2(2));
}


//==============================================================================
//==============================================================================
inline cVector3d cAdd(const cVector3d& a_vector1,
                      const cVector3d& a_vector2,
                      const cVector3d& a_vector3)
{
    return cVector3d(
      a_vector1(0) +a_vector2(0) +a_vector3(0) ,
      a_vector1(1) +a_vector2(1) +a_vector3(1) ,
      a_vector1(2) +a_vector2(2) +a_vector3(2) );
}


//==============================================================================
//==============================================================================
inline cVector3d cSub(const cVector3d& a_vector1,
                      const cVector3d& a_vector2)
{
  return cVector3d(
    a_vector1(0) -a_vector2(0) ,
    a_vector1(1) -a_vector2(1) ,
    a_vector1(2) -a_vector2(2) );
}


//==============================================================================
//==============================================================================
inline cVector3d cNegate(const cVector3d& a_vector)
{
    return cVector3d(-a_vector(0),
                     -a_vector(1),
                     -a_vector(2));
}


//==============================================================================
//==============================================================================
inline cVector3d cMul(const double& a_value,
                      const cVector3d& a_vector)
{
    return cVector3d(a_vector(0) *a_value,
                     a_vector(1) *a_value,
                     a_vector(2) *a_value);
}


//==============================================================================
//==============================================================================
inline cVector3d cDiv(const double& a_value, 
                      const cVector3d& a_vector)
{
    double factor = 1.0 / a_value;
    return (cVector3d(factor * a_vector(0),
                      factor * a_vector(1),
                      factor * a_vector(2)));
}


//==============================================================================
//==============================================================================
inline cVector3d cCross(const cVector3d& a_vector1,
                        const cVector3d& a_vector2)
{
    cVector3d result;
    a_vector1.crossr(a_vector2, result);
    return (result);
}


//==============================================================================
//==============================================================================
inline double cDot(const cVector3d& a_vector1, 
                   const cVector3d& a_vector2)
{
    return(a_vector1.dot(a_vector2));
}


//==============================================================================
//==============================================================================
inline cVector3d cNormalize(const cVector3d& a_vector)
{
    cVector3d result;
    a_vector.normalizer(result);
    return (result);
}


//==============================================================================
//==============================================================================
inline double cDistance(const cVector3d& a_point1, 
                        const cVector3d& a_point2)
{
    return ( a_point1.distance(a_point2) );
}


//==============================================================================
//==============================================================================
inline double cDistanceSq(const cVector3d& a_point1, 
                          const cVector3d& a_point2)
{
    return ( a_point1.distancesq(a_point2) );
}


//==============================================================================
//==============================================================================
bool inline cEqualPoints(const cVector3d& a_point1,
                         const cVector3d& a_point2,
                         const double a_epsilon = C_SMALL)
{
    // accelerated path for exact equality
    if (a_epsilon == 0.0) 
    {
        if ((a_point1(0)  == a_point2(0)) && (a_point1(1)  == a_point2(1)) && (a_point1(2)  == a_point2(2)))
        {
            return (true);
        }
        else
        {
            return (false);
        }
    }

    if ((fabs(a_point1(0) - a_point2(0)) < a_epsilon) &&
        (fabs(a_point1(1) - a_point2(1)) < a_epsilon) &&
        (fabs(a_point1(2) - a_point2(2)) < a_epsilon))
    {
        return (true);
    }
    else
    {
        return (false);
    }
}


//==============================================================================
//==============================================================================
inline cMatrix3d cIdentity3d(void)
{
    cMatrix3d result;
    result.identity();
    return (result);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cAdd(const cMatrix3d& a_matrix1,
                      const cMatrix3d& a_matrix2)
{
    cMatrix3d result;
    a_matrix1.addr(a_matrix2, result);
    return (result);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cSub(const cMatrix3d& a_matrix1,
                      const cMatrix3d& a_matrix2)
{
    cMatrix3d result;
    a_matrix1.subr(a_matrix2, result);
    return (result);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cRotEulerRad(const double& a_angleRad1,
    const double& a_angleRad2,
    const double& a_angleRad3,
    const cEulerOrder a_eulerOrder,
    const bool a_useIntrinsicEulerModel = true)
{
    // create matrix
    cMatrix3d rot;
    if (a_useIntrinsicEulerModel)
        rot.setIntrinsicEulerRotationRad(a_angleRad1, a_angleRad2, a_angleRad3, a_eulerOrder);
    else
        rot.setExtrinsicEulerRotationRad(a_angleRad1, a_angleRad2, a_angleRad3, a_eulerOrder);

    // return result
    return (rot);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cRotEulerDeg(const double& a_angleDeg1,
    const double& a_angleDeg2,
    const double& a_angleDeg3,
    const cEulerOrder a_eulerOrder,
    const bool a_useIntrinsicEulerModel = true)
{
    // create matrix
    cMatrix3d rot;
    if (a_useIntrinsicEulerModel)
        rot.setExtrinsicEulerRotationDeg(a_angleDeg1, a_angleDeg2, a_angleDeg3, a_eulerOrder);
    else
        rot.setExtrinsicEulerRotationDeg(a_angleDeg1, a_angleDeg2, a_angleDeg3, a_eulerOrder);

    // return result
    return (rot);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cRotAxisAngleRad(const double& a_axisX,
    const double& a_axisY,
    const double& a_axisZ,
    const double& a_angleRad)
{
    // create matrix
    cMatrix3d rot;
    rot.setAxisAngleRotationRad(a_axisX,
                                a_axisY,
                                a_axisZ,
                                a_angleRad);

    // return result
    return (rot);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cRotAxisAngleDeg(const double& a_axisX,
    const double& a_axisY,
    const double& a_axisZ,
    const double& a_angleDeg)
{
    // create matrix
    cMatrix3d rot;
    rot.setAxisAngleRotationDeg(a_axisX,
                                a_axisY,
                                a_axisZ,
                                a_angleDeg);

    // return result
    return (rot);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cMul(const cMatrix3d& a_matrix1,
                      const cMatrix3d& a_matrix2)
{
    cMatrix3d result;
    a_matrix1.mulr(a_matrix2, result);
    return (result);
}


//==============================================================================
//==============================================================================
inline cVector3d cMul(const cMatrix3d& a_matrix,
                      const cVector3d& a_vector)
{
    cVector3d result;
    a_matrix.mulr(a_vector, result);
    return(result);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cTranspose(const cMatrix3d& a_matrix)
{
    cMatrix3d result;
    a_matrix.transr(result);
    return(result);
}


//==============================================================================
//==============================================================================
inline cMatrix3d cInverse(const cMatrix3d& a_matrix)
{
    cMatrix3d result;
    a_matrix.invertr(result);
    return(result);
}


//==============================================================================
//==============================================================================
inline double cAngle(const cVector3d& a_vector1,
                     const cVector3d& a_vector2)
{
    // compute length of vectors
    double n1 = a_vector1.length();
    double n2 = a_vector2.length();
    double val = n1 * n2;

    // check if lengths of vectors are not zero
    if (fabs(val) < C_SMALL)
    {
        return (0.0);
    }

    // compute angle
    double result = a_vector1.dot(a_vector2)/(val);
    if (result > 1.0) { result = 1.0; }
    else if (result < -1.0) { result = -1.0; }

    return(acos(result));
}


//==============================================================================
//==============================================================================
inline double cCosAngle(const cVector3d& a_vector1,
                        const cVector3d& a_vector2)
{
    // compute length of vectors
    double n1 = a_vector1.length();
    double n2 = a_vector2.length();
    double val = n1 * n2;

    // check if lengths of vectors are not zero
    if (fabs(val) < C_SMALL)
    {
        return (0.0);
    }

    // compute angle
    return(a_vector1.dot(a_vector2)/(val));
}


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

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