path: root/indra/llmath/v3dmath.h

/**
 * @file v3dmath.h
 * @brief High precision 3 dimensional vector.
 *
 * $LicenseInfo:firstyear=2000&license=viewerlgpl$
 * Second Life Viewer Source Code
 * Copyright (C) 2010, Linden Research, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation;
 * version 2.1 of the License only.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 * $/LicenseInfo$
 */

#ifndef LL_V3DMATH_H
#define LL_V3DMATH_H

#include "llerror.h"
#include "v3math.h"

class LLVector3d
{
    public:
        F64 mdV[3];

        const static LLVector3d zero;
        const static LLVector3d x_axis;
        const static LLVector3d y_axis;
        const static LLVector3d z_axis;
        const static LLVector3d x_axis_neg;
        const static LLVector3d y_axis_neg;
        const static LLVector3d z_axis_neg;

        inline LLVector3d();                            // Initializes LLVector3d to (0, 0, 0)
        inline LLVector3d(const F64 x, const F64 y, const F64 z);           // Initializes LLVector3d to (x. y, z)
        inline explicit LLVector3d(const F64 *vec);             // Initializes LLVector3d to (vec[0]. vec[1], vec[2])
        inline explicit LLVector3d(const LLVector3 &vec);
        explicit LLVector3d(const LLSD& sd)
        {
            setValue(sd);
        }

        void setValue(const LLSD& sd)
        {
            mdV[0] = sd[0].asReal();
            mdV[1] = sd[1].asReal();
            mdV[2] = sd[2].asReal();
        }

        LLSD getValue() const
        {
            LLSD ret;
            ret[0] = mdV[0];
            ret[1] = mdV[1];
            ret[2] = mdV[2];
            return ret;
        }

        inline BOOL isFinite() const;                                   // checks to see if all values of LLVector3d are finite
        BOOL        clamp(const F64 min, const F64 max);        // Clamps all values to (min,max), returns TRUE if data changed
        BOOL        abs();                      // sets all values to absolute value of original value (first octant), returns TRUE if changed

        inline const LLVector3d&    clear();        // Clears LLVector3d to (0, 0, 0, 1)
        inline const LLVector3d&    clearVec();     // deprecated
        inline const LLVector3d&    setZero();      // Zero LLVector3d to (0, 0, 0, 0)
        inline const LLVector3d&    zeroVec();      // deprecated
        inline const LLVector3d&    set(const F64 x, const F64 y, const F64 z); // Sets LLVector3d to (x, y, z, 1)
        inline const LLVector3d&    set(const LLVector3d &vec); // Sets LLVector3d to vec
        inline const LLVector3d&    set(const F64 *vec);        // Sets LLVector3d to vec
        inline const LLVector3d&    set(const LLVector3 &vec);
        inline const LLVector3d&    setVec(const F64 x, const F64 y, const F64 z);  // deprecated
        inline const LLVector3d&    setVec(const LLVector3d &vec);  // deprecated
        inline const LLVector3d&    setVec(const F64 *vec);         // deprecated
        inline const LLVector3d&    setVec(const LLVector3 &vec);   // deprecated

        F64     magVec() const;             // deprecated
        F64     magVecSquared() const;      // deprecated
        inline F64      normVec();                  // deprecated

        F64 length() const;         // Returns magnitude of LLVector3d
        F64 lengthSquared() const;  // Returns magnitude squared of LLVector3d
        inline F64 normalize();     // Normalizes and returns the magnitude of LLVector3d

        const LLVector3d&   rotVec(const F64 angle, const LLVector3d &vec); // Rotates about vec by angle radians
        const LLVector3d&   rotVec(const F64 angle, const F64 x, const F64 y, const F64 z);     // Rotates about x,y,z by angle radians
        const LLVector3d&   rotVec(const LLMatrix3 &mat);               // Rotates by LLMatrix4 mat
        const LLVector3d&   rotVec(const LLQuaternion &q);              // Rotates by LLQuaternion q

        BOOL isNull() const;            // Returns TRUE if vector has a _very_small_ length
        BOOL isExactlyZero() const      { return !mdV[VX] && !mdV[VY] && !mdV[VZ]; }

        const LLVector3d&   operator=(const LLVector4 &a);

        F64 operator[](int idx) const { return mdV[idx]; }
        F64 &operator[](int idx) { return mdV[idx]; }

        friend LLVector3d operator+(const LLVector3d& a, const LLVector3d& b);  // Return vector a + b
        friend LLVector3d operator-(const LLVector3d& a, const LLVector3d& b);  // Return vector a minus b
        friend F64 operator*(const LLVector3d& a, const LLVector3d& b);     // Return a dot b
        friend LLVector3d operator%(const LLVector3d& a, const LLVector3d& b);  // Return a cross b
        friend LLVector3d operator*(const LLVector3d& a, const F64 k);              // Return a times scaler k
        friend LLVector3d operator/(const LLVector3d& a, const F64 k);              // Return a divided by scaler k
        friend LLVector3d operator*(const F64 k, const LLVector3d& a);              // Return a times scaler k
        friend bool operator==(const LLVector3d& a, const LLVector3d& b);       // Return a == b
        friend bool operator!=(const LLVector3d& a, const LLVector3d& b);       // Return a != b

        friend const LLVector3d& operator+=(LLVector3d& a, const LLVector3d& b);    // Return vector a + b
        friend const LLVector3d& operator-=(LLVector3d& a, const LLVector3d& b);    // Return vector a minus b
        friend const LLVector3d& operator%=(LLVector3d& a, const LLVector3d& b);    // Return a cross b
        friend const LLVector3d& operator*=(LLVector3d& a, const F64 k);                // Return a times scaler k
        friend const LLVector3d& operator/=(LLVector3d& a, const F64 k);                // Return a divided by scaler k

        friend LLVector3d operator-(const LLVector3d& a);                   // Return vector -a

        friend std::ostream&     operator<<(std::ostream& s, const LLVector3d& a);      // Stream a

        static BOOL parseVector3d(const std::string& buf, LLVector3d* value);

};

typedef LLVector3d LLGlobalVec;

inline const LLVector3d &LLVector3d::set(const LLVector3 &vec)
{
    mdV[0] = vec.mV[0];
    mdV[1] = vec.mV[1];
    mdV[2] = vec.mV[2];
    return *this;
}

inline const LLVector3d &LLVector3d::setVec(const LLVector3 &vec)
{
    mdV[0] = vec.mV[0];
    mdV[1] = vec.mV[1];
    mdV[2] = vec.mV[2];
    return *this;
}


inline LLVector3d::LLVector3d(void)
{
    mdV[0] = 0.f;
    mdV[1] = 0.f;
    mdV[2] = 0.f;
}

inline LLVector3d::LLVector3d(const F64 x, const F64 y, const F64 z)
{
    mdV[VX] = x;
    mdV[VY] = y;
    mdV[VZ] = z;
}

inline LLVector3d::LLVector3d(const F64 *vec)
{
    mdV[VX] = vec[VX];
    mdV[VY] = vec[VY];
    mdV[VZ] = vec[VZ];
}

inline LLVector3d::LLVector3d(const LLVector3 &vec)
{
    mdV[VX] = vec.mV[VX];
    mdV[VY] = vec.mV[VY];
    mdV[VZ] = vec.mV[VZ];
}

/*
inline LLVector3d::LLVector3d(const LLVector3d &copy)
{
    mdV[VX] = copy.mdV[VX];
    mdV[VY] = copy.mdV[VY];
    mdV[VZ] = copy.mdV[VZ];
}
*/

// Destructors

// checker
inline BOOL LLVector3d::isFinite() const
{
    return (llfinite(mdV[VX]) && llfinite(mdV[VY]) && llfinite(mdV[VZ]));
}


// Clear and Assignment Functions

inline const LLVector3d&    LLVector3d::clear(void)
{
    mdV[0] = 0.f;
    mdV[1] = 0.f;
    mdV[2]= 0.f;
    return (*this);
}

inline const LLVector3d&    LLVector3d::clearVec(void)
{
    mdV[0] = 0.f;
    mdV[1] = 0.f;
    mdV[2]= 0.f;
    return (*this);
}

inline const LLVector3d&    LLVector3d::setZero(void)
{
    mdV[0] = 0.f;
    mdV[1] = 0.f;
    mdV[2] = 0.f;
    return (*this);
}

inline const LLVector3d&    LLVector3d::zeroVec(void)
{
    mdV[0] = 0.f;
    mdV[1] = 0.f;
    mdV[2] = 0.f;
    return (*this);
}

inline const LLVector3d&    LLVector3d::set(const F64 x, const F64 y, const F64 z)
{
    mdV[VX] = x;
    mdV[VY] = y;
    mdV[VZ] = z;
    return (*this);
}

inline const LLVector3d&    LLVector3d::set(const LLVector3d &vec)
{
    mdV[0] = vec.mdV[0];
    mdV[1] = vec.mdV[1];
    mdV[2] = vec.mdV[2];
    return (*this);
}

inline const LLVector3d&    LLVector3d::set(const F64 *vec)
{
    mdV[0] = vec[0];
    mdV[1] = vec[1];
    mdV[2] = vec[2];
    return (*this);
}

inline const LLVector3d&    LLVector3d::setVec(const F64 x, const F64 y, const F64 z)
{
    mdV[VX] = x;
    mdV[VY] = y;
    mdV[VZ] = z;
    return (*this);
}

inline const LLVector3d&    LLVector3d::setVec(const LLVector3d &vec)
{
    mdV[0] = vec.mdV[0];
    mdV[1] = vec.mdV[1];
    mdV[2] = vec.mdV[2];
    return (*this);
}

inline const LLVector3d&    LLVector3d::setVec(const F64 *vec)
{
    mdV[0] = vec[0];
    mdV[1] = vec[1];
    mdV[2] = vec[2];
    return (*this);
}

inline F64 LLVector3d::normVec(void)
{
    F64 mag = (F32) sqrt(mdV[0]*mdV[0] + mdV[1]*mdV[1] + mdV[2]*mdV[2]);
    F64 oomag;

    if (mag > FP_MAG_THRESHOLD)
    {
        oomag = 1.f/mag;
        mdV[0] *= oomag;
        mdV[1] *= oomag;
        mdV[2] *= oomag;
    }
    else
    {
        mdV[0] = 0.f;
        mdV[1] = 0.f;
        mdV[2] = 0.f;
        mag = 0;
    }
    return (mag);
}

inline F64 LLVector3d::normalize(void)
{
    F64 mag = (F32) sqrt(mdV[0]*mdV[0] + mdV[1]*mdV[1] + mdV[2]*mdV[2]);
    F64 oomag;

    if (mag > FP_MAG_THRESHOLD)
    {
        oomag = 1.f/mag;
        mdV[0] *= oomag;
        mdV[1] *= oomag;
        mdV[2] *= oomag;
    }
    else
    {
        mdV[0] = 0.f;
        mdV[1] = 0.f;
        mdV[2] = 0.f;
        mag = 0;
    }
    return (mag);
}

// LLVector3d Magnitude and Normalization Functions

inline F64  LLVector3d::magVec(void) const
{
    return (F32) sqrt(mdV[0]*mdV[0] + mdV[1]*mdV[1] + mdV[2]*mdV[2]);
}

inline F64  LLVector3d::magVecSquared(void) const
{
    return mdV[0]*mdV[0] + mdV[1]*mdV[1] + mdV[2]*mdV[2];
}

inline F64  LLVector3d::length(void) const
{
    return (F32) sqrt(mdV[0]*mdV[0] + mdV[1]*mdV[1] + mdV[2]*mdV[2]);
}

inline F64  LLVector3d::lengthSquared(void) const
{
    return mdV[0]*mdV[0] + mdV[1]*mdV[1] + mdV[2]*mdV[2];
}

inline LLVector3d operator+(const LLVector3d& a, const LLVector3d& b)
{
    LLVector3d c(a);
    return c += b;
}

inline LLVector3d operator-(const LLVector3d& a, const LLVector3d& b)
{
    LLVector3d c(a);
    return c -= b;
}

inline F64  operator*(const LLVector3d& a, const LLVector3d& b)
{
    return (a.mdV[0]*b.mdV[0] + a.mdV[1]*b.mdV[1] + a.mdV[2]*b.mdV[2]);
}

inline LLVector3d operator%(const LLVector3d& a, const LLVector3d& b)
{
    return LLVector3d( a.mdV[1]*b.mdV[2] - b.mdV[1]*a.mdV[2], a.mdV[2]*b.mdV[0] - b.mdV[2]*a.mdV[0], a.mdV[0]*b.mdV[1] - b.mdV[0]*a.mdV[1] );
}

inline LLVector3d operator/(const LLVector3d& a, const F64 k)
{
    F64 t = 1.f / k;
    return LLVector3d( a.mdV[0] * t, a.mdV[1] * t, a.mdV[2] * t );
}

inline LLVector3d operator*(const LLVector3d& a, const F64 k)
{
    return LLVector3d( a.mdV[0] * k, a.mdV[1] * k, a.mdV[2] * k );
}

inline LLVector3d operator*(F64 k, const LLVector3d& a)
{
    return LLVector3d( a.mdV[0] * k, a.mdV[1] * k, a.mdV[2] * k );
}

inline bool operator==(const LLVector3d& a, const LLVector3d& b)
{
    return (  (a.mdV[0] == b.mdV[0])
            &&(a.mdV[1] == b.mdV[1])
            &&(a.mdV[2] == b.mdV[2]));
}

inline bool operator!=(const LLVector3d& a, const LLVector3d& b)
{
    return (  (a.mdV[0] != b.mdV[0])
            ||(a.mdV[1] != b.mdV[1])
            ||(a.mdV[2] != b.mdV[2]));
}

inline const LLVector3d& operator+=(LLVector3d& a, const LLVector3d& b)
{
    a.mdV[0] += b.mdV[0];
    a.mdV[1] += b.mdV[1];
    a.mdV[2] += b.mdV[2];
    return a;
}

inline const LLVector3d& operator-=(LLVector3d& a, const LLVector3d& b)
{
    a.mdV[0] -= b.mdV[0];
    a.mdV[1] -= b.mdV[1];
    a.mdV[2] -= b.mdV[2];
    return a;
}

inline const LLVector3d& operator%=(LLVector3d& a, const LLVector3d& b)
{
    LLVector3d ret( a.mdV[1]*b.mdV[2] - b.mdV[1]*a.mdV[2], a.mdV[2]*b.mdV[0] - b.mdV[2]*a.mdV[0], a.mdV[0]*b.mdV[1] - b.mdV[0]*a.mdV[1]);
    a = ret;
    return a;
}

inline const LLVector3d& operator*=(LLVector3d& a, const F64 k)
{
    a.mdV[0] *= k;
    a.mdV[1] *= k;
    a.mdV[2] *= k;
    return a;
}

inline const LLVector3d& operator/=(LLVector3d& a, const F64 k)
{
    F64 t = 1.f / k;
    a.mdV[0] *= t;
    a.mdV[1] *= t;
    a.mdV[2] *= t;
    return a;
}

inline LLVector3d operator-(const LLVector3d& a)
{
    return LLVector3d( -a.mdV[0], -a.mdV[1], -a.mdV[2] );
}

inline F64  dist_vec(const LLVector3d& a, const LLVector3d& b)
{
    F64 x = a.mdV[0] - b.mdV[0];
    F64 y = a.mdV[1] - b.mdV[1];
    F64 z = a.mdV[2] - b.mdV[2];
    return (F32) sqrt( x*x + y*y + z*z );
}

inline F64  dist_vec_squared(const LLVector3d& a, const LLVector3d& b)
{
    F64 x = a.mdV[0] - b.mdV[0];
    F64 y = a.mdV[1] - b.mdV[1];
    F64 z = a.mdV[2] - b.mdV[2];
    return x*x + y*y + z*z;
}

inline F64  dist_vec_squared2D(const LLVector3d& a, const LLVector3d& b)
{
    F64 x = a.mdV[0] - b.mdV[0];
    F64 y = a.mdV[1] - b.mdV[1];
    return x*x + y*y;
}

inline LLVector3d lerp(const LLVector3d& a, const LLVector3d& b, const F64 u)
{
    return LLVector3d(
        a.mdV[VX] + (b.mdV[VX] - a.mdV[VX]) * u,
        a.mdV[VY] + (b.mdV[VY] - a.mdV[VY]) * u,
        a.mdV[VZ] + (b.mdV[VZ] - a.mdV[VZ]) * u);
}


inline BOOL LLVector3d::isNull() const
{
    if ( F_APPROXIMATELY_ZERO > mdV[VX]*mdV[VX] + mdV[VY]*mdV[VY] + mdV[VZ]*mdV[VZ] )
    {
        return TRUE;
    }
    return FALSE;
}


inline F64 angle_between(const LLVector3d& a, const LLVector3d& b)
{
    LLVector3d an = a;
    LLVector3d bn = b;
    an.normalize();
    bn.normalize();
    F64 cosine = an * bn;
    F64 angle = (cosine >= 1.0f) ? 0.0f :
                (cosine <= -1.0f) ? F_PI :
                acos(cosine);
    return angle;
}

inline BOOL are_parallel(const LLVector3d& a, const LLVector3d& b, const F64 epsilon)
{
    LLVector3d an = a;
    LLVector3d bn = b;
    an.normalize();
    bn.normalize();
    F64 dot = an * bn;
    if ( (1.0f - fabs(dot)) < epsilon)
    {
        return TRUE;
    }
    return FALSE;

}

inline LLVector3d projected_vec(const LLVector3d& a, const LLVector3d& b)
{
    LLVector3d project_axis = b;
    project_axis.normalize();
    return project_axis * (a * project_axis);
}

inline LLVector3d inverse_projected_vec(const LLVector3d& a, const LLVector3d& b)
{
    LLVector3d normalized_a = a;
    normalized_a.normalize();
    LLVector3d normalized_b = b;
    F64 b_length = normalized_b.normalize();

    F64 dot_product = normalized_a * normalized_b;
    return normalized_a * (b_length / dot_product);
}

#endif // LL_V3DMATH_H