vector3d.hpp

// Copyright (C) 2002-2012 Nikolaus Gebhardt
// This file is part of the "Irrlicht Engine".
// For conditions of distribution and use, see copyright notice in duckcpp/duckcpp.hpp
 
#ifndef DCPP_POINT_3D_HPP_INCLUDED
#define DCPP_POINT_3D_HPP_INCLUDED
 
#include <duckcpp/core/engine/irrMath.hpp>
 
namespace dcpp
{
namespace nub
{
 
    template <class T>
    class vector3d;
 
    using vector3df = vector3d<dcpp::float32_kt>;
 
    using vector3di = vector3d<dcpp::int32_kt>;
 
 
    template <class T>
    class vector3d
    {
    public:
        vector3d() : X(0), Y(0), Z(0) {}
        vector3d(T nx, T ny, T nz) : X(nx), Y(ny), Z(nz) {}
        explicit vector3d(T n) : X(n), Y(n), Z(n) {}
 
        // operators
 
        vector3d<T> operator-() const { return vector3d<T>(-X, -Y, -Z); }
 
        vector3d<T> operator+(const vector3d<T>& other) const { return vector3d<T>(X + other.X, Y + other.Y, Z + other.Z); }
        vector3d<T>& operator+=(const vector3d<T>& other) { X+=other.X; Y+=other.Y; Z+=other.Z; return *this; }
        vector3d<T> operator+(const T val) const { return vector3d<T>(X + val, Y + val, Z + val); }
        vector3d<T>& operator+=(const T val) { X+=val; Y+=val; Z+=val; return *this; }
 
        vector3d<T> operator-(const vector3d<T>& other) const { return vector3d<T>(X - other.X, Y - other.Y, Z - other.Z); }
        vector3d<T>& operator-=(const vector3d<T>& other) { X-=other.X; Y-=other.Y; Z-=other.Z; return *this; }
        vector3d<T> operator-(const T val) const { return vector3d<T>(X - val, Y - val, Z - val); }
        vector3d<T>& operator-=(const T val) { X-=val; Y-=val; Z-=val; return *this; }
 
        vector3d<T> operator*(const vector3d<T>& other) const { return vector3d<T>(X * other.X, Y * other.Y, Z * other.Z); }
        vector3d<T>& operator*=(const vector3d<T>& other) { X*=other.X; Y*=other.Y; Z*=other.Z; return *this; }
        vector3d<T> operator*(const T v) const { return vector3d<T>(X * v, Y * v, Z * v); }
        vector3d<T>& operator*=(const T v) { X*=v; Y*=v; Z*=v; return *this; }
 
        vector3d<T> operator/(const vector3d<T>& other) const { return vector3d<T>(X / other.X, Y / other.Y, Z / other.Z); }
        vector3d<T>& operator/=(const vector3d<T>& other) { X/=other.X; Y/=other.Y; Z/=other.Z; return *this; }
        vector3d<T> operator/(const T v) const { return vector3d<T>(X/v, Y/v, Z/v); }
        vector3d<T>& operator/=(const T v) { X/=v; Y/=v; Z/=v; return *this; }
 
        T& operator [](dcpp::uint32_kt index)
        {
            DCPP_DEBUG_BREAK_IF(index>2) // access violation
 
            return *(&X+index);
        }
 
        const T& operator [](dcpp::uint32_kt index) const
        {
            DCPP_DEBUG_BREAK_IF(index>2) // access violation
 
            return *(&X+index);
        }
 
        bool operator<=(const vector3d<T>&other) const
        {
            return (X<other.X || dcpp::nub::equals(X, other.X)) ||
                    (dcpp::nub::equals(X, other.X) && (Y<other.Y || dcpp::nub::equals(Y, other.Y))) ||
                    (dcpp::nub::equals(X, other.X) && dcpp::nub::equals(Y, other.Y) && (Z<other.Z || dcpp::nub::equals(Z, other.Z)));
        }
 
        bool operator>=(const vector3d<T>&other) const
        {
            return (X>other.X || dcpp::nub::equals(X, other.X)) ||
                    (dcpp::nub::equals(X, other.X) && (Y>other.Y || dcpp::nub::equals(Y, other.Y))) ||
                    (dcpp::nub::equals(X, other.X) && dcpp::nub::equals(Y, other.Y) && (Z>other.Z || dcpp::nub::equals(Z, other.Z)));
        }
 
        bool operator<(const vector3d<T>&other) const
        {
            return (X<other.X && !dcpp::nub::equals(X, other.X)) ||
                    (dcpp::nub::equals(X, other.X) && Y<other.Y && !dcpp::nub::equals(Y, other.Y)) ||
                    (dcpp::nub::equals(X, other.X) && dcpp::nub::equals(Y, other.Y) && Z<other.Z && !dcpp::nub::equals(Z, other.Z));
        }
 
        bool operator>(const vector3d<T>&other) const
        {
            return (X>other.X && !dcpp::nub::equals(X, other.X)) ||
                    (dcpp::nub::equals(X, other.X) && Y>other.Y && !dcpp::nub::equals(Y, other.Y)) ||
                    (dcpp::nub::equals(X, other.X) && dcpp::nub::equals(Y, other.Y) && Z>other.Z && !dcpp::nub::equals(Z, other.Z));
        }
 
        bool operator==(const vector3d<T>& other) const
        {
            return this->equals(other);
        }
 
        bool operator!=(const vector3d<T>& other) const
        {
            return !this->equals(other);
        }
 
        // functions
 
        bool equals(const vector3d<T>& other, const T tolerance = (T)ROUNDING_ERROR_Float32 ) const
        {
            return dcpp::nub::equals(X, other.X, tolerance) &&
                dcpp::nub::equals(Y, other.Y, tolerance) &&
                dcpp::nub::equals(Z, other.Z, tolerance);
        }
 
        vector3d<T>& set(const T nx, const T ny, const T nz) {X=nx; Y=ny; Z=nz; return *this;}
        vector3d<T>& set(const vector3d<T>& p) {X=p.X; Y=p.Y; Z=p.Z;return *this;}
 
        T getLength() const { return dcpp::nub::squareroot( X*X + Y*Y + Z*Z ); }
 
 
        T getLengthSQ() const { return X*X + Y*Y + Z*Z; }
 
        T dotProduct(const vector3d<T>& other) const
        {
            return X*other.X + Y*other.Y + Z*other.Z;
        }
 
 
        T getDistanceFrom(const vector3d<T>& other) const
        {
            return vector3d<T>(X - other.X, Y - other.Y, Z - other.Z).getLength();
        }
 
 
        T getDistanceFromSQ(const vector3d<T>& other) const
        {
            return vector3d<T>(X - other.X, Y - other.Y, Z - other.Z).getLengthSQ();
        }
 
 
        vector3d<T> crossProduct(const vector3d<T>& p) const
        {
            return vector3d<T>(Y * p.Z - Z * p.Y, Z * p.X - X * p.Z, X * p.Y - Y * p.X);
        }
 
 
        bool isBetweenPoints(const vector3d<T>& begin, const vector3d<T>& end) const
        {
            const T f = (end - begin).getLengthSQ();
            return getDistanceFromSQ(begin) <= f &&
                getDistanceFromSQ(end) <= f;
        }
 
 
        vector3d<T>& normalize()
        {
            dcpp::float64_kt length = X*X + Y*Y + Z*Z;
            if (length == 0 ) // this check isn't an optimization but prevents getting NAN in the sqrt.
                return *this;
            length = dcpp::nub::reciprocal_squareroot(length);
 
            X = (T)(X * length);
            Y = (T)(Y * length);
            Z = (T)(Z * length);
            return *this;
        }
 
        vector3d<T>& setLength(T newlength)
        {
            normalize();
            return (*this *= newlength);
        }
 
#if defined(_DCPP_COMPILE_WITH_90_DEGREE_CAMERA)
        vector3d<T>& normalize_camera_direction(const vector3d<T>& def)
        {
            dcpp::float64_kt l = (dcpp::float64_kt)X * X + (dcpp::float64_kt)Y * Y + (dcpp::float64_kt)Z * Z;
            if (::fabs(l) < 0.000000001)
            {
                X = def.X;
                Y = def.Y;
                Z = def.Z;
            }
            else
            {
                l = 1.0 / ::sqrt(l);
                dcpp::float64_kt v;
                v = X * l; X = ::fabs(v) < 0.00000001 ? (T)0 : (T)v;
                v = Y * l; Y = ::fabs(v) < 0.00000001 ? (T)0 : (T)v;
                v = Z * l; Z = ::fabs(v) < 0.00000001 ? (T)0 : (T)v;
            }
            return *this;
        }
#define normalize_x() normalize_camera_direction(dcpp::nub::vector3df(1.f, 0.f, 0.f))
#define normalize_z() normalize_camera_direction(dcpp::nub::vector3df(0.f, 0.f, 1.f))
#define normalize_y(v) dcpp::nub::vector3df(v).normalize_camera_direction(dcpp::nub::vector3df(0.f, 1.f, 0.f))
#else
#define normalize_x() normalize()
#define normalize_z() normalize()
#define normalize_y(v) v
#endif
 
 
        vector3d<T>& invert()
        {
            X *= -1;
            Y *= -1;
            Z *= -1;
            return *this;
        }
 
 
        void rotateXZBy(dcpp::float64_kt degrees, const vector3d<T>& center=vector3d<T>())
        {
            degrees *= DEGTORAD64;
            dcpp::float64_kt cs = cos(degrees);
            dcpp::float64_kt sn = sin(degrees);
            X -= center.X;
            Z -= center.Z;
            set((T)(X*cs - Z*sn), Y, (T)(X*sn + Z*cs));
            X += center.X;
            Z += center.Z;
        }
 
 
        void rotateXYBy(dcpp::float64_kt degrees, const vector3d<T>& center=vector3d<T>())
        {
            degrees *= DEGTORAD64;
            dcpp::float64_kt cs = cos(degrees);
            dcpp::float64_kt sn = sin(degrees);
            X -= center.X;
            Y -= center.Y;
            set((T)(X*cs - Y*sn), (T)(X*sn + Y*cs), Z);
            X += center.X;
            Y += center.Y;
        }
 
 
        void rotateYZBy(dcpp::float64_kt degrees, const vector3d<T>& center=vector3d<T>())
        {
            degrees *= DEGTORAD64;
            dcpp::float64_kt cs = cos(degrees);
            dcpp::float64_kt sn = sin(degrees);
            Z -= center.Z;
            Y -= center.Y;
            set(X, (T)(Y*cs - Z*sn), (T)(Y*sn + Z*cs));
            Z += center.Z;
            Y += center.Y;
        }
 
 
        vector3d<T> getInterpolated(const vector3d<T>& other, dcpp::float64_kt d) const
        {
            const dcpp::float64_kt inv = 1.0 - d;
            return vector3d<T>((T)(other.X*inv + X*d), (T)(other.Y*inv + Y*d), (T)(other.Z*inv + Z*d));
        }
 
 
        vector3d<T> getInterpolated_quadratic(const vector3d<T>& v2, const vector3d<T>& v3, dcpp::float64_kt d) const
        {
            // this*(1-d)*(1-d) + 2 * v2 * (1-d) + v3 * d * d;
            const dcpp::float64_kt inv = (T) 1.0 - d;
            const dcpp::float64_kt mul0 = inv * inv;
            const dcpp::float64_kt mul1 = (T) 2.0 * d * inv;
            const dcpp::float64_kt mul2 = d * d;
 
            return vector3d<T> ((T)(X * mul0 + v2.X * mul1 + v3.X * mul2),
                    (T)(Y * mul0 + v2.Y * mul1 + v3.Y * mul2),
                    (T)(Z * mul0 + v2.Z * mul1 + v3.Z * mul2));
        }
 
 
        vector3d<T>& interpolate(const vector3d<T>& a, const vector3d<T>& b, dcpp::float64_kt d)
        {
            X = (T)((dcpp::float64_kt)b.X + ( ( a.X - b.X ) * d ));
            Y = (T)((dcpp::float64_kt)b.Y + ( ( a.Y - b.Y ) * d ));
            Z = (T)((dcpp::float64_kt)b.Z + ( ( a.Z - b.Z ) * d ));
            return *this;
        }
 
 
 
        vector3d<T> getHorizontalAngle() const
        {
            vector3d<T> angle;
 
            // tmp avoids some precision troubles on some compilers when working with T=dcpp::int32_kt
            dcpp::float64_kt tmp = (atan2((dcpp::float64_kt)X, (dcpp::float64_kt)Z) * RADTODEG64);
            angle.Y = (T)tmp;
 
            if (angle.Y < 0)
                angle.Y += 360;
            if (angle.Y >= 360)
                angle.Y -= 360;
 
            const dcpp::float64_kt z1 = dcpp::nub::squareroot(X*X + Z*Z);
 
            tmp = (atan2((dcpp::float64_kt)z1, (dcpp::float64_kt)Y) * RADTODEG64 - 90.0);
            angle.X = (T)tmp;
 
            if (angle.X < 0)
                angle.X += 360;
            if (angle.X >= 360)
                angle.X -= 360;
 
            return angle;
        }
 
 
        vector3d<T> getSphericalCoordinateAngles() const
        {
            vector3d<T> angle;
            const dcpp::float64_kt length = X*X + Y*Y + Z*Z;
 
            if (length)
            {
                if (X!=0)
                {
                    angle.Y = (T)(atan2((dcpp::float64_kt)Z,(dcpp::float64_kt)X) * RADTODEG64);
                }
                else if (Z<0)
                    angle.Y=180;
 
                angle.X = (T)(acos(Y * dcpp::nub::reciprocal_squareroot(length)) * RADTODEG64);
            }
            return angle;
        }
 
 
        vector3d<T> rotationToDirection(const vector3d<T> & forwards = vector3d<T>(0, 0, 1)) const
        {
            const dcpp::float64_kt cr = cos( dcpp::nub::DEGTORAD64 * X );
            const dcpp::float64_kt sr = sin( dcpp::nub::DEGTORAD64 * X );
            const dcpp::float64_kt cp = cos( dcpp::nub::DEGTORAD64 * Y );
            const dcpp::float64_kt sp = sin( dcpp::nub::DEGTORAD64 * Y );
            const dcpp::float64_kt cy = cos( dcpp::nub::DEGTORAD64 * Z );
            const dcpp::float64_kt sy = sin( dcpp::nub::DEGTORAD64 * Z );
 
            const dcpp::float64_kt srsp = sr*sp;
            const dcpp::float64_kt crsp = cr*sp;
 
            const dcpp::float64_kt pseudoMatrix[] = {
                ( cp*cy ), ( cp*sy ), ( -sp ),
                ( srsp*cy-cr*sy ), ( srsp*sy+cr*cy ), ( sr*cp ),
                ( crsp*cy+sr*sy ), ( crsp*sy-sr*cy ), ( cr*cp )};
 
            return vector3d<T>(
                (T)(forwards.X * pseudoMatrix[0] +
                    forwards.Y * pseudoMatrix[3] +
                    forwards.Z * pseudoMatrix[6]),
                (T)(forwards.X * pseudoMatrix[1] +
                    forwards.Y * pseudoMatrix[4] +
                    forwards.Z * pseudoMatrix[7]),
                (T)(forwards.X * pseudoMatrix[2] +
                    forwards.Y * pseudoMatrix[5] +
                    forwards.Z * pseudoMatrix[8]));
        }
 
 
        void getAs4Values(T* array) const
        {
            array[0] = X;
            array[1] = Y;
            array[2] = Z;
            array[3] = 0;
        }
 
 
        void getAs3Values(T* array) const
        {
            array[0] = X;
            array[1] = Y;
            array[2] = Z;
        }
 
 
        T X;
 
        T Y;
 
        T Z;
    };
 
    // Implementer note: inline keyword needed due to template specialization for dcpp::int32_kt. Otherwise put specialization into a .cpp
    template <>
    inline vector3di vector3di::operator /(dcpp::int32_kt val) const {return dcpp::nub::vector3di(X/val,Y/val,Z/val);}
    template <>
    inline vector3di& vector3di::operator /=(dcpp::int32_kt val) {X/=val;Y/=val;Z/=val; return *this;}
 
    template <>
    inline vector3di vector3di::getSphericalCoordinateAngles() const
    {
        vector3di angle;
        const dcpp::float64_kt length = X*X + Y*Y + Z*Z;
 
        if (length)
        {
            if (X!=0)
            {
                angle.Y = round32((dcpp::float32_kt)(atan2((dcpp::float64_kt)Z,(dcpp::float64_kt)X) * RADTODEG64));
            }
            else if (Z<0)
                angle.Y=180;
 
            angle.X = round32((dcpp::float32_kt)(acos(Y * dcpp::nub::reciprocal_squareroot(length)) * RADTODEG64));
        }
        return angle;
    }
 
    template<class S, class T>
    vector3d<T> operator*(const S scalar, const vector3d<T>& vector) { return vector*scalar; }
 
} // end namespace nub
} // end namespace dcpp
 
#endif