/home/slemaign/softs-local/BioMove3D-git/util/vector3d.h

/*
-----------------------------------------------------------------------------
This source file is part of OGRE
    (Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2006 Torus Knot Software Ltd
Also see acknowledgements in Readme.html

This program 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; either version 2 of the License, or (at your option) any later
version.

This program 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 program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt.

You may alternatively use this source under the terms of a specific version of
the OGRE Unrestricted License provided you have obtained such a license from
Torus Knot Software Ltd.
-----------------------------------------------------------------------------
*/
#ifndef __Vector3_H__
#define __Vector3_H__

#include <cstring>
#include <iostream>
#include <cassert>
#include <cmath>
#include "p3d_matrix.h"

//namespace Ogre
//{

class Vector3
{
public:
    double x, y, z;

public:
    inline Vector3()
    {
    }

    inline Vector3( const double fX, const double fY, const double fZ )
            : x( fX ), y( fY ), z( fZ )
    {
    }

    inline explicit Vector3( const double afCoordinate[3] )
            : x( afCoordinate[0] ),
            y( afCoordinate[1] ),
            z( afCoordinate[2] )
    {
    }

    inline explicit Vector3( const int afCoordinate[3] )
    {
        x = (double)afCoordinate[0];
        y = (double)afCoordinate[1];
        z = (double)afCoordinate[2];
    }

    inline explicit Vector3( const p3d_matrix4 m )
    {
        x = m[0][3];
        y = m[1][3];
        z = m[2][3];
    }

    inline explicit Vector3( double* const r )
            : x( r[0] ), y( r[1] ), z( r[2] )
    {
    }

    inline explicit Vector3( const double scaler )
            : x( scaler )
            , y( scaler )
            , z( scaler )
    {
    }

    inline int size() const { return 3; }


    inline double operator [] ( const size_t i ) const
    {
        assert( i < 3 );

        return *(&x+i);
    }

    inline double& operator [] ( const size_t i )
    {
        assert( i < 3 );

        return *(&x+i);
    }
    inline double* ptr()
    {
        return &x;
    }
    inline const double* ptr() const
    {
        return &x;
    }

    inline Vector3& operator = ( const Vector3& rkVector )
                               {
        x = rkVector.x;
        y = rkVector.y;
        z = rkVector.z;

        return *this;
    }

    inline Vector3& operator = ( const double fScaler )
                               {
        x = fScaler;
        y = fScaler;
        z = fScaler;

        return *this;
    }

    inline bool operator == ( const Vector3& rkVector ) const
    {
        return ( x == rkVector.x && y == rkVector.y && z == rkVector.z );
    }

    inline bool operator != ( const Vector3& rkVector ) const
    {
        return ( x != rkVector.x || y != rkVector.y || z != rkVector.z );
    }

    // arithmetic operations
    inline Vector3 operator + ( const Vector3& rkVector ) const
    {
        return Vector3(
                x + rkVector.x,
                y + rkVector.y,
                z + rkVector.z);
    }

    inline Vector3 operator - ( const Vector3& rkVector ) const
    {
        return Vector3(
                x - rkVector.x,
                y - rkVector.y,
                z - rkVector.z);
    }

    inline Vector3 operator * ( const double fScalar ) const
    {
        return Vector3(
                x * fScalar,
                y * fScalar,
                z * fScalar);
    }

    inline Vector3 operator * ( const Vector3& rhs) const
    {
        return Vector3(
                x * rhs.x,
                y * rhs.y,
                z * rhs.z);
    }

    inline Vector3 operator / ( const double fScalar ) const
    {
        assert( fScalar != 0.0 );

        double fInv = 1.0 / fScalar;

        return Vector3(
                x * fInv,
                y * fInv,
                z * fInv);
    }

    inline Vector3 operator / ( const Vector3& rhs) const
    {
        return Vector3(
                x / rhs.x,
                y / rhs.y,
                z / rhs.z);
    }

    inline const Vector3& operator + () const
    {
        return *this;
    }

    inline Vector3 operator - () const
    {
        return Vector3(-x, -y, -z);
    }

    // overloaded operators to help Vector3
    inline friend Vector3 operator * ( const double fScalar, const Vector3& rkVector )
    {
        return Vector3(
                fScalar * rkVector.x,
                fScalar * rkVector.y,
                fScalar * rkVector.z);
    }

    inline friend Vector3 operator / ( const double fScalar, const Vector3& rkVector )
    {
        return Vector3(
                fScalar / rkVector.x,
                fScalar / rkVector.y,
                fScalar / rkVector.z);
    }

    inline friend Vector3 operator + (const Vector3& lhs, const double rhs)
    {
        return Vector3(
                lhs.x + rhs,
                lhs.y + rhs,
                lhs.z + rhs);
    }

    inline friend Vector3 operator + (const double lhs, const Vector3& rhs)
    {
        return Vector3(
                lhs + rhs.x,
                lhs + rhs.y,
                lhs + rhs.z);
    }

    inline friend Vector3 operator - (const Vector3& lhs, const double rhs)
    {
        return Vector3(
                lhs.x - rhs,
                lhs.y - rhs,
                lhs.z - rhs);
    }

    inline friend Vector3 operator - (const double lhs, const Vector3& rhs)
    {
        return Vector3(
                lhs - rhs.x,
                lhs - rhs.y,
                lhs - rhs.z);
    }

    // arithmetic updates
    inline Vector3& operator += ( const Vector3& rkVector )
                                {
        x += rkVector.x;
        y += rkVector.y;
        z += rkVector.z;

        return *this;
    }

    inline Vector3& operator += ( const double fScalar )
                                {
        x += fScalar;
        y += fScalar;
        z += fScalar;
        return *this;
    }

    inline Vector3& operator -= ( const Vector3& rkVector )
                                {
        x -= rkVector.x;
        y -= rkVector.y;
        z -= rkVector.z;

        return *this;
    }

    inline Vector3& operator -= ( const double fScalar )
                                {
        x -= fScalar;
        y -= fScalar;
        z -= fScalar;
        return *this;
    }

    inline Vector3& operator *= ( const double fScalar )
                                {
        x *= fScalar;
        y *= fScalar;
        z *= fScalar;
        return *this;
    }

    inline Vector3& operator *= ( const Vector3& rkVector )
                                {
        x *= rkVector.x;
        y *= rkVector.y;
        z *= rkVector.z;

        return *this;
    }

    inline Vector3& operator /= ( const double fScalar )
                                {
        assert( fScalar != 0.0 );

        double fInv = 1.0 / fScalar;

        x *= fInv;
        y *= fInv;
        z *= fInv;

        return *this;
    }

    inline Vector3& operator /= ( const Vector3& rkVector )
                                {
        x /= rkVector.x;
        y /= rkVector.y;
        z /= rkVector.z;

        return *this;
    }


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

    inline double squaredLength () const
    {
        return x * x + y * y + z * z;
    }

    inline double distance(const Vector3& rhs) const
    {
        return (*this - rhs).length();
    }

    inline double squaredDistance(const Vector3& rhs) const
    {
        return (*this - rhs).squaredLength();
    }

    inline double dotProduct(const Vector3& vec) const
    {
        return x * vec.x + y * vec.y + z * vec.z;
    }

    inline double absDotProduct(const Vector3& vec) const
    {
        return abs(x * vec.x) + abs(y * vec.y) + abs(z * vec.z);
    }

    inline double normalise()
    {
        double fLength = sqrt( x * x + y * y + z * z );

        // Will also work for zero-sized vectors, but will change nothing
        if ( fLength > 1e-08 )
        {
            double fInvLength = 1.0 / fLength;
            x *= fInvLength;
            y *= fInvLength;
            z *= fInvLength;
        }

        return fLength;
    }

    inline Vector3 crossProduct( const Vector3& rkVector ) const
    {
        return Vector3(
                y * rkVector.z - z * rkVector.y,
                z * rkVector.x - x * rkVector.z,
                x * rkVector.y - y * rkVector.x);
    }

    inline Vector3 midPoint( const Vector3& vec ) const
    {
        return Vector3(
                ( x + vec.x ) * 0.5,
                ( y + vec.y ) * 0.5,
                ( z + vec.z ) * 0.5 );
    }

    inline bool operator < ( const Vector3& rhs ) const
    {
        if( x < rhs.x && y < rhs.y && z < rhs.z )
            return true;
        return false;
    }

    inline bool operator > ( const Vector3& rhs ) const
    {
        if( x > rhs.x && y > rhs.y && z > rhs.z )
            return true;
        return false;
    }

    inline void makeFloor( const Vector3& cmp )
    {
        if( cmp.x < x ) x = cmp.x;
        if( cmp.y < y ) y = cmp.y;
        if( cmp.z < z ) z = cmp.z;
    }

    inline void makeCeil( const Vector3& cmp )
    {
        if( cmp.x > x ) x = cmp.x;
        if( cmp.y > y ) y = cmp.y;
        if( cmp.z > z ) z = cmp.z;
    }

    inline Vector3 perpendicular(void) const
    {
        static const double fSquareZero = 1e-06 * 1e-06;

        Vector3 perp = this->crossProduct( Vector3::UNIT_X );

        // Check length
        if( perp.squaredLength() < fSquareZero )
        {
            /* This vector is the Y axis multiplied by a scalar, so we have
                   to use another axis.
                */
            perp = this->crossProduct( Vector3::UNIT_Y );
        }
        perp.normalise();

        return perp;
    }
    //        inline Vector3 randomDeviant(
    //            const Radian& angle,
    //            const Vector3& up = Vector3::ZERO ) const
    //        {
    //            Vector3 newUp;
    //
    //            if (up == Vector3::ZERO)
    //            {
    //                // Generate an up vector
    //                newUp = this->perpendicular();
    //            }
    //            else
    //            {
    //                newUp = up;
    //            }
    //
    //            // Rotate up vector by random amount around this
    //            Quaternion q;
    //            q.FromAngleAxis( Radian(Math::UnitRandom() * Math::TWO_PI), *this );
    //            newUp = q * newUp;
    //
    //            // Finally rotate this by given angle around randomised up
    //            q.FromAngleAxis( angle, newUp );
    //            return q * (*this);
    //        }

    //                inline Radian angleBetween(const Vector3& dest)
    //                {
    //                        double lenProduct = length() * dest.length();
    //
    //                        // Divide by zero check
    //                        if(lenProduct < 1e-6f)
    //                                lenProduct = 1e-6f;
    //
    //                        double f = dotProduct(dest) / lenProduct;
    //
    //                        f = Math::Clamp(f, (double)-1.0, (double)1.0);
    //                        return Math::ACos(f);
    //
    //                }
    //        Quaternion getRotationTo(const Vector3& dest,
    //                        const Vector3& fallbackAxis = Vector3::ZERO) const
    //        {
    //            // Based on Stan Melax's article in Game Programming Gems
    //            Quaternion q;
    //            // Copy, since cannot modify local
    //            Vector3 v0 = *this;
    //            Vector3 v1 = dest;
    //            v0.normalise();
    //            v1.normalise();
    //
    //            double d = v0.dotProduct(v1);
    //            // If dot == 1, vectors are the same
    //            if (d >= 1.0f)
    //            {
    //                return Quaternion::IDENTITY;
    //            }
    //                        if (d < (1e-6f - 1.0f))
    //                        {
    //                                if (fallbackAxis != Vector3::ZERO)
    //                                {
    //                                        // rotate 180 degrees about the fallback axis
    //                                        q.FromAngleAxis(Radian(Math::PI), fallbackAxis);
    //                                }
    //                                else
    //                                {
    //                                        // Generate an axis
    //                                        Vector3 axis = Vector3::UNIT_X.crossProduct(*this);
    //                                        if (axis.isZeroLength()) // pick another if colinear
    //                                                axis = Vector3::UNIT_Y.crossProduct(*this);
    //                                        axis.normalise();
    //                                        q.FromAngleAxis(Radian(Math::PI), axis);
    //                                }
    //                        }
    //                        else
    //                        {
    //                double s = Math::Sqrt( (1+d)*2 );
    //                    double invs = 1 / s;
    //
    //                                Vector3 c = v0.crossProduct(v1);
    //
    //                q.x = c.x * invs;
    //                    q.y = c.y * invs;
    //                q.z = c.z * invs;
    //                q.w = s * 0.5;
    //                                q.normalise();
    //                        }
    //            return q;
    //        }

    inline bool isZeroLength(void) const
    {
        double sqlen = (x * x) + (y * y) + (z * z);
        return (sqlen < (1e-06 * 1e-06));

    }

    inline Vector3 normalisedCopy(void) const
    {
        Vector3 ret = *this;
        ret.normalise();
        return ret;
    }

    inline Vector3 reflect(const Vector3& normal) const
    {
        return Vector3( *this - ( 2 * this->dotProduct(normal) * normal ) );
    }

    inline bool positionEquals(const Vector3& rhs, double tolerance = 1e-03) const
    {
        return doubleEqual(x, rhs.x, tolerance) &&
                doubleEqual(y, rhs.y, tolerance) &&
                doubleEqual(z, rhs.z, tolerance);

    }

    bool doubleEqual(const double& a, const double& b, const double& tolerance) const
    {
        if (fabs(b-a) <= tolerance)
            return true;
        else
            return false;
    }

    inline bool positionCloses(const Vector3& rhs, double tolerance = 1e-03f) const
    {
        return squaredDistance(rhs) <=
                (squaredLength() + rhs.squaredLength()) * tolerance;
    }

    //                inline bool directionEquals(const Vector3& rhs,
    //                        const Radian& tolerance) const
    //                {
    //                        double dot = dotProduct(rhs);
    //                        Radian angle = Math::ACos(dot);
    //
    //                        return Math::Abs(angle.valueRadians()) <= tolerance.valueRadians();
    //
    //                }

    // special points
    static const Vector3 ZERO;
    static const Vector3 UNIT_X;
    static const Vector3 UNIT_Y;
    static const Vector3 UNIT_Z;
    static const Vector3 NEGATIVE_UNIT_X;
    static const Vector3 NEGATIVE_UNIT_Y;
    static const Vector3 NEGATIVE_UNIT_Z;
    static const Vector3 UNIT_SCALE;

    friend std::ostream& operator <<
            ( std::ostream& o, const Vector3& v )
    {
        o << "Vector3(" << v.x << ", " << v.y << ", " << v.z << ")";
        return o;
    }
};
//}
#endif