K3dVector3Work.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2007 by Jan Koci   *
 *   honza.koci@email.cz   *
 *   http://kengine.sourceforge.net/tutorial/
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU 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 General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU 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.             *
 ***************************************************************************/

#include "K3dVector3Work.h"

K3dVector3Work::K3dVector3Work ( K3dGameData *_pGameData )
{
        m_pGameData = _pGameData;
        // Set this pointer to the game data
        m_pGameData->SetVector3Work ( this );
}

K3dVector3Work::~K3dVector3Work ( void )
{}

void K3dVector3Work::CheckVector3Array ( TVector3Array &_rvVertex )
{
        cout << endl;
        cout << "Num verts" << _rvVertex.size() << endl;
        for ( int i=0; i< ( int ) _rvVertex.size(); i++ )
        {
                cout << "Vertex " << i << endl;
                cout << "Vertex X = " << _rvVertex[i]->GetX() << endl;
                cout << "Vertex Y = " << _rvVertex[i]->GetY() << endl;
                cout << "Vertex Z = " << _rvVertex[i]->GetZ() << endl;
        }
}

K3dVector3Obj &K3dVector3Work::Sqr ( const K3dVector3Obj &_rkV0, const K3dVector3Obj &_rkV1 )
{
        m_kVector = _rkV0 * _rkV1;
        return m_kVector;
}

K3dVector3Obj &K3dVector3Work::Cross ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1 )
{
        m_kVector[0] = ( ( _rkV0[1] * _rkV1[2] ) - ( _rkV0[2] * _rkV1[1] ) );
        m_kVector[1] = ( ( _rkV0[2] * _rkV1[0] ) - ( _rkV0[0] * _rkV1[2] ) );
        m_kVector[2] = ( ( _rkV0[0] * _rkV1[1] ) - ( _rkV0[1] * _rkV1[0] ) );
        return m_kVector;
}

K3dVector3Obj &K3dVector3Work::UnitCross ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1 )
{
        K3dVector3Obj kCross = Cross ( _rkV0,_rkV1 );
        m_kVector = Normalize ( kCross );
        return m_kVector;
}

K3dVector3Obj &K3dVector3Work::Normal ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1,const K3dVector3Obj &_rkV2 )
{
        // Equation is
        // N = (P1 - P0) x (P2 - P0)
        K3dVector3Obj kDiff0, kDiff1;
        kDiff0 = _rkV1 - _rkV0;
        kDiff1 = _rkV2 - _rkV1;
        return UnitCross ( kDiff0, kDiff1 );
}

void K3dVector3Work::AddScalar ( K3dVector3Obj &_rkV, const float _fScalar, const int _iId )
{
        if ( ! ( 0 <= _iId ) && ( _iId < 3 ) )
        {
                cerr << "Error -- K3dVector3Work::AddScalar() -- Index must be from 0 to 2" << _iId <<endl;
                return;
        }
        _rkV[_iId] += _fScalar;
}



void K3dVector3Work::SubtractScalar ( K3dVector3Obj &_rkV, const float _fScalar, const int _iId )
{
        if ( ! ( 0 <= _iId ) && ( _iId < 3 ) )
        {
                cerr << "Error -- K3dVector3Work::SubtractScalar() -- Index must be from 0 to 2" << _iId <<endl;
                return;
        }
        _rkV[_iId] -= _fScalar;
}

K3dVector3Obj& K3dVector3Work::Normalize (const K3dVector3Obj &_rkV )
{
        float fLength0, fLength1;
        m_kVector = _rkV;
        // Calculate vector lenght
        fLength0 = Length ( _rkV );
        if ( fLength0 != ( float ) 0 )
        {
                fLength1 = 1.0f/fLength0;
                m_kVector.GetX() *= fLength1;
                m_kVector.GetY() *= fLength1;
                m_kVector.GetZ() *= fLength1;
        }
        return m_kVector;
}

float K3dVector3Work::DotAngleBetweenVectors ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1 )
{
        // Normalize vectors
        K3dVector3Obj kN0 = _rkV0;
        K3dVector3Obj kN1 = _rkV1;
        kN0 = Normalize ( kN0 );
        kN1 = Normalize ( kN1 );

        return Dot ( kN0, kN1 );
}

K3dVector3Obj& K3dVector3Work::Reflection ( const K3dVector3Obj &_rkDirection,const K3dVector3Obj &_rkNormal )
{
        // Equation is
        // R = L - 2 perpN L
        //   = L - 2[L -(N.L)N]
        //   = 2(N.L)N - L

        K3dVector3Obj kL, kN;
        float fDot;
        K3dVector3 kPerpN, kDot;

        // Set actual vector
        //kL = m_afVector;

        // Set normal vector
        kN = _rkNormal;
        kN = Normalize ( kN );


        // Calculate dot product
        fDot = Dot ( kN , _rkDirection );
        //fDot = kN.Dot(kL);
        // Calc twice dot
        fDot = 2*fDot;
        // Calc new normal
        kN = kN * fDot;
        // Calc reflection
        m_kVector = kN - _rkDirection;
        //kR = kN - kL;

        return m_kVector;
}

float K3dVector3Work::Length ( const K3dVector3Obj &_rkV )
{
        return ( float ) sqrt ( (_rkV.GetX() * _rkV.GetX()) + (_rkV.GetY() * _rkV.GetY()) + (_rkV.GetZ() * _rkV.GetZ()) );
}

K3dVector3Obj &K3dVector3Work::VectorBetween ( const K3dVector3Obj &_rkV1,const K3dVector3Obj &_rkV2 )
{
        m_kVector[0] = _rkV1[0] - _rkV2[0];
        m_kVector[1] = _rkV1[1] - _rkV2[1];
        m_kVector[2] = _rkV1[2] - _rkV2[2];
        return m_kVector;
}

void K3dVector3Work::Angle ( K3dVector3Obj &_rkAngle )
{
        // Length of vector
        float fLength;
        // Get length of actual vector
        fLength = Length ( _rkAngle );
        if ( fLength )
        {
                // Calculate length for multiplication instead dividing
                fLength = 1/fLength;
                // Calculate X angle
                m_kVector.GetX() = K3dMath::ACos ( _rkAngle.GetX() * fLength );
                // Calculate Y angle
                m_kVector.GetY() = K3dMath::ACos ( _rkAngle.GetY() * fLength );
                // Calculate Z angle
                m_kVector.GetX() = K3dMath::ACos ( _rkAngle.GetX() * fLength );
                // Set result
                _rkAngle = m_kVector;
        }
}

float K3dVector3Work::SquaredLength ( const K3dVector3Obj &_rkV )
{
        float fSqrLen = 0.0f;
        for ( int i = 0; i < 3; i++ )
                fSqrLen += ( float ) _rkV.GetVector() [i]*_rkV.GetVector() [i];
        return fSqrLen;
}

const float K3dVector3Work::SquaredLength ( const K3dVector3Obj &_rkV ) const
{
        float fSqrLen = 0.0f;
        for ( int i = 0; i < 3; i++ )
                fSqrLen += ( float ) _rkV.GetVector() [i]*_rkV.GetVector() [i];
        return fSqrLen;
}

float K3dVector3Work::UnitLength ( const K3dVector3Obj &_rkV )
{
        return _rkV.GetX() + _rkV.GetY() + _rkV.GetZ();
}

K3dVector3Obj &K3dVector3Work::AddVector ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1 )
{
        m_kVector[0] = _rkV0[0] + _rkV1[0];
        m_kVector[1] = _rkV0[1] + _rkV1[1];
        m_kVector[2] = _rkV0[2] + _rkV1[2];
        return m_kVector;
}



void K3dVector3Work::DivideVector ( K3dVector3Obj &_rkV ,const float _fDivider )
{
        if ( _fDivider!=0 )
        {
                _rkV[0] = _rkV[0] / _fDivider;
                _rkV[1] = _rkV[1] / _fDivider;
                _rkV[2] = _rkV[2] / _fDivider;
        }
        else
        {
                cerr << "inline void K3dVector3Work::DivideVector() -- Error - divide by zero" << endl;
                return;
        }
}

void K3dVector3Work::Translate ( K3dVector3 &_rkV, const K3dVector3 &_rkDir, const float _fDist )
{
        m_kVector[0] = _rkV[0] + ( _rkDir[0] * _fDist );
        m_kVector[1] = _rkV[1] + ( _rkDir[1] * _fDist );
        m_kVector[2] = _rkV[2] + ( _rkDir[2] * _fDist );
        _rkV = m_kVector;
}

void K3dVector3Work::Invert ( K3dVector3 &_rkV )
{
        _rkV[0] = -_rkV[0];
        _rkV[1] = -_rkV[1];
        _rkV[2] = -_rkV[2];

}

float K3dVector3Work::Dot ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1 )
{
        float fX,fY,fZ,fResult;
        fX = _rkV0[0] * _rkV1[0];
        fY = _rkV0[1] * _rkV1[1];
        fZ = _rkV0[2] * _rkV1[2];
        fResult = fX + fY + fZ;
        return fResult;
}

float K3dVector3Work::AngleBetweenNormals ( const K3dVector3Obj &_rkNormal0,const K3dVector3Obj &_rkNormal1 )
{
        /*
        * angle = acos(|U|*|V| * U·V)
        *
        * If vector is normalized, than UV lengths = 1
        *       
        * angle = acos(1*U·V)
        *
        * Consequently
        *
        * angle = acos(U·V)
        */

        float fDot = Dot ( _rkNormal0, _rkNormal1 );
        return K3dMath::ACos( fDot );
}

float K3dVector3Work::AngleBetweenVectors ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1 )
{
        /*
        * Equation is
        * ANGLE = acos( DOT(_rkV0,_rkV1) / (LENGTH(_rkV0) * LENGTH(_rkV1)) )
        * If normalized vectors equation is
        * NORMALIZE(_rkV0)
        * NORMALIZE(_rkV1)
        * ANGLE = acos( DOT(_rkV0,_rkV1))
        * Therefore length of normalized vector is equals one
        */

        // Normalize vectors
        K3dVector3Obj kN0 = _rkV0;
        K3dVector3Obj kN1 = _rkV1;
        kN0 = Normalize ( kN0 );
        kN1 = Normalize ( kN1 );
        return AngleBetweenNormals ( kN0, kN1 );
}

float K3dVector3Work::AngleBetweenVectors ( const K3dVector3Obj &_rkV0,const K3dVector3Obj &_rkV1, const K3dVector3Obj &_rkCentre )
{
        // Calculate direction vector between first vector and centre
        K3dVector3Obj kDirection0 = _rkV0 - _rkCentre;
        // Calculate direction vector between second vector and centre
        K3dVector3Obj kDirection1 = _rkV1 - _rkCentre;
        // Calculate angle between direction  vectors
        float fAngle = AngleBetweenVectors ( kDirection0, kDirection1 );
        // Calculate cross product between direction vectors
        K3dVector3Obj kCross = Cross ( kDirection0, kDirection1 );
        // Calculate unit length of cross product
        float fLen = UnitLength ( kCross );
        // Convert radian angle to degree angle
        fAngle = fAngle * K3dMath::K_RAD_TO_DEG;
        // If unit length is positive then angle is greater than 180 degree
        if ( fLen > ( float ) 0 )
        {
                fAngle = ( float ) 360 - fAngle;
        }
        return fAngle;
}

K3dVector3Obj &K3dVector3Work::Projection ( const K3dVector3Obj &_rkP ,const K3dVector3Obj &_rkQ )
{
        //
        //            P dot Q
        // proj qP = --------- Q
        //            Q dot Q
        //

        m_kVector.Set ( 0,0,0 );
        // Dot product
        float fDotPQ, fDotQQ;
        // Calc PQ dot product
        fDotPQ = Dot ( _rkP, _rkQ );
        // Calc QQ dot product
        fDotQQ = Dot ( _rkQ, _rkQ );
        // Calc projection
        if ( fDotQQ )
        {
                m_kVector = _rkQ* ( fDotPQ/fDotQQ );
        }
        return m_kVector;
}

K3dVector3Obj &K3dVector3Work::Perpendicular ( const K3dVector3Obj &_rkP,const  K3dVector3Obj &_rkQ )
{
        //
        // perp qP = P - proj qP
        //

        // Projection vector
        K3dVector3Obj kProj;
        // Calc projection vector
        kProj = Projection ( _rkP, _rkQ );
        // Calc perpendicular vector
        m_kVector = _rkP - kProj;
        return m_kVector;
}

K3dVector3Obj &K3dVector3Work::Multiply ( const K3dVector3Obj &_rkV0, const  K3dVector3Obj &_rkV1 )
{
        m_kVector[0] = _rkV0[0] * _rkV1[0];
        m_kVector[1] = _rkV0[1] * _rkV1[1];
        m_kVector[2] = _rkV0[2] * _rkV1[2];

        return m_kVector;
}

bool K3dVector3Work::CompareIntScalars ( const K3dVector3Obj &_rkV0,const  K3dVector3Obj &_rkV1 )
{
        if ( ( ( int ) _rkV0[0] == ( int ) _rkV1[0] )
                && ( ( int ) _rkV0[1] == ( int ) _rkV1[1] )
                && ( ( int ) _rkV0[2] == ( int ) _rkV1[2] ) )
        {
                return true;
        }
        else
        {
                return false;
        }
}

bool K3dVector3Work::EqualRange ( const K3dVector3Obj &_rkV0,const  K3dVector3Obj &_rkV1 , const float _fRange )
{
        if (
            ( K3dMath::Positive ( _rkV0[0] - _rkV1[0] ) <= _fRange )
            && ( K3dMath::Positive ( _rkV0[1] - _rkV1[1] ) <= _fRange )
            && ( K3dMath::Positive ( _rkV0[2] - _rkV1[2] ) <= _fRange )
        )
        {
                return true;
        }
        else
        {
                return false;
        }
}


K3dVector3Obj &K3dVector3Work::CalcCentre ( const K3dVector3Obj &_rkV0, const K3dVector3Obj &_rkV1, const K3dVector3Obj &_rkV2 )
{
        float fX = _rkV0[0] + _rkV1[0] + _rkV2[0];
        float fY = _rkV0[1] + _rkV1[1] + _rkV2[1];
        float fZ = _rkV0[2] + _rkV1[2] + _rkV2[2];

        if ( fX != 0 )
        {
                m_kVector[0] = ( fX ) / ( float ) 3;
        }
        else
        {
                m_kVector[0] = ( float ) 0;
        }

        if ( fY != 0 )
        {
                m_kVector[1] = ( fY ) / ( float ) 3;
        }
        else
        {
                m_kVector[1] = ( float ) 0;
        }

        if ( fZ != 0 )
        {
                m_kVector[2] = ( fZ ) / ( float ) 3;
        }
        else
        {
                m_kVector[2] = ( float ) 0;
        }
        return m_kVector;
}

void K3dVector3Work::SwapXY(K3dVector3Obj &_rkV)
{
        float f = _rkV[0];
        _rkV[0] = _rkV[1];
        _rkV[1] = f;
}

void K3dVector3Work::SwapXZ(K3dVector3Obj &_rkV)
{
        float f = _rkV[0];
        _rkV[0] = _rkV[2];
        _rkV[2] = f;
}

void K3dVector3Work::SwapYZ(K3dVector3Obj &_rkV)
{
        float f = _rkV[1];
        _rkV[1] = _rkV[2];
        _rkV[2] = f;
}

void K3dVector3Work::CalcVectorArrayCentre(K3dVector3Obj &_rkVCentre, const TVector3Array &_rvVector)
{
        // Get number of vertices
        int iNum = (int) _rvVector.size();
        _rkVCentre.Reset();
        
        // Go through all vertices
        for(int i=0; i<iNum; i++)
        {
                K3dVector3Obj *pPos = _rvVector[i];
                _rkVCentre += *pPos;
        }
        if(iNum > 0)
        {
                _rkVCentre /= (float) iNum;
        }
}

---