lljointsolverrp3.cpp

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// Header Files
//-----------------------------------------------------------------------------
#include "linden_common.h"

#include "lljointsolverrp3.h"

#include "llmath.h"

#define F_EPSILON 0.00001f


//-----------------------------------------------------------------------------
// Constructor
//-----------------------------------------------------------------------------
LLJointSolverRP3::LLJointSolverRP3()
{
        mJointA = NULL;
        mJointB = NULL;
        mJointC = NULL;
        mJointGoal = NULL;
        mLengthAB = 1.0f;
        mLengthBC = 1.0f;
        mPoleVector.setVec( 1.0f, 0.0f, 0.0f );
        mbUseBAxis = FALSE;
        mTwist = 0.0f;
        mFirstTime = TRUE;
}


//-----------------------------------------------------------------------------
// Destructor
//-----------------------------------------------------------------------------
/*virtual*/ LLJointSolverRP3::~LLJointSolverRP3()
{
}


//-----------------------------------------------------------------------------
// setupJoints()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setupJoints(     LLJoint* jointA,
                                                                        LLJoint* jointB,
                                                                        LLJoint* jointC,
                                                                        LLJoint* jointGoal )
{
        mJointA = jointA;
        mJointB = jointB;
        mJointC = jointC;
        mJointGoal = jointGoal;

        mLengthAB = mJointB->getPosition().magVec();
        mLengthBC = mJointC->getPosition().magVec();

        mJointABaseRotation = jointA->getRotation();
        mJointBBaseRotation = jointB->getRotation();
}


//-----------------------------------------------------------------------------
// getPoleVector()
//-----------------------------------------------------------------------------
const LLVector3& LLJointSolverRP3::getPoleVector()
{
        return mPoleVector;
}


//-----------------------------------------------------------------------------
// setPoleVector()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setPoleVector( const LLVector3& poleVector )
{
        mPoleVector = poleVector;
        mPoleVector.normVec();
}


//-----------------------------------------------------------------------------
// setPoleVector()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setBAxis( const LLVector3& bAxis )
{
        mBAxis = bAxis;
        mBAxis.normVec();
        mbUseBAxis = TRUE;
}

//-----------------------------------------------------------------------------
// getTwist()
//-----------------------------------------------------------------------------
F32 LLJointSolverRP3::getTwist()
{
        return mTwist;
}


//-----------------------------------------------------------------------------
// setTwist()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setTwist( F32 twist )
{
        mTwist = twist;
}


//-----------------------------------------------------------------------------
// solve()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::solve()
{
//      llinfos << llendl;
//      llinfos << "LLJointSolverRP3::solve()" << llendl;

        //-------------------------------------------------------------------------
        // setup joints in their base rotations
        //-------------------------------------------------------------------------
        mJointA->setRotation( mJointABaseRotation );
        mJointB->setRotation( mJointBBaseRotation );

        //-------------------------------------------------------------------------
        // get joint positions in world space
        //-------------------------------------------------------------------------
        LLVector3 aPos = mJointA->getWorldPosition();
        LLVector3 bPos = mJointB->getWorldPosition();
        LLVector3 cPos = mJointC->getWorldPosition();
        LLVector3 gPos = mJointGoal->getWorldPosition();

//      llinfos << "bPosLocal = " << mJointB->getPosition() << llendl;
//      llinfos << "cPosLocal = " << mJointC->getPosition() << llendl;
//      llinfos << "bRotLocal = " << mJointB->getRotation() << llendl;
//      llinfos << "cRotLocal = " << mJointC->getRotation() << llendl;

//      llinfos << "aPos : " << aPos << llendl;
//      llinfos << "bPos : " << bPos << llendl;
//      llinfos << "cPos : " << cPos << llendl;
//      llinfos << "gPos : " << gPos << llendl;

        //-------------------------------------------------------------------------
        // get the poleVector in world space
        //-------------------------------------------------------------------------
        LLMatrix4 worldJointAParentMat;
        if ( mJointA->getParent() )
        {
                worldJointAParentMat = mJointA->getParent()->getWorldMatrix();
        }
        LLVector3 poleVec = rotate_vector( mPoleVector, worldJointAParentMat );

        //-------------------------------------------------------------------------
        // compute the following:
        // vector from A to B
        // vector from B to C
        // vector from A to C
        // vector from A to G (goal)
        //-------------------------------------------------------------------------
        LLVector3 abVec = bPos - aPos;
        LLVector3 bcVec = cPos - bPos;
        LLVector3 acVec = cPos - aPos;
        LLVector3 agVec = gPos - aPos;

//      llinfos << "abVec : " << abVec << llendl;
//      llinfos << "bcVec : " << bcVec << llendl;
//      llinfos << "acVec : " << acVec << llendl;
//      llinfos << "agVec : " << agVec << llendl;

        //-------------------------------------------------------------------------
        // compute needed lengths of those vectors
        //-------------------------------------------------------------------------
        F32 abLen = abVec.magVec();
        F32 bcLen = bcVec.magVec();
        F32 agLen = agVec.magVec();

//      llinfos << "abLen : " << abLen << llendl;
//      llinfos << "bcLen : " << bcLen << llendl;
//      llinfos << "agLen : " << agLen << llendl;

        //-------------------------------------------------------------------------
        // compute component vector of (A->B) orthogonal to (A->C)
        //-------------------------------------------------------------------------
        LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));

//      llinfos << "abacCompOrthoVec : " << abacCompOrthoVec << llendl

        //-------------------------------------------------------------------------
        // compute the normal of the original ABC plane (and store for later)
        //-------------------------------------------------------------------------
        LLVector3 abcNorm;
        if (!mbUseBAxis)
        {
                if( are_parallel(abVec, bcVec, 0.001f) )
                {
                        // the current solution is maxed out, so we use the axis that is
                        // orthogonal to both poleVec and A->B
                        if ( are_parallel(poleVec, abVec, 0.001f) )
                        {
                                // ACK! the problem is singular
                                if ( are_parallel(poleVec, agVec, 0.001f) )
                                {
                                        // the solutions is also singular
                                        return;
                                }
                                else
                                {
                                        abcNorm = poleVec % agVec;
                                }
                        }
                        else
                        {
                                abcNorm = poleVec % abVec;
                        }
                }
                else
                {
                        abcNorm = abVec % bcVec;
                }
        }
        else
        {
                abcNorm = mBAxis * mJointB->getWorldRotation();
        }

        //-------------------------------------------------------------------------
        // compute rotation of B
        //-------------------------------------------------------------------------
        // angle between A->B and B->C
        F32 abbcAng = angle_between(abVec, bcVec);

        // vector orthogonal to A->B and B->C
        LLVector3 abbcOrthoVec = abVec % bcVec;
        if (abbcOrthoVec.magVecSquared() < 0.001f)
        {
                abbcOrthoVec = poleVec % abVec;
                abacCompOrthoVec = poleVec;
        }
        abbcOrthoVec.normVec();

        F32 agLenSq = agLen * agLen;

        // angle arm for extension
        F32 cosTheta =  (agLenSq - abLen*abLen - bcLen*bcLen) / (2.0f * abLen * bcLen);
        if (cosTheta > 1.0f)
                cosTheta = 1.0f;
        else if (cosTheta < -1.0f)
                cosTheta = -1.0f;

        F32 theta = acos(cosTheta);

        LLQuaternion bRot(theta - abbcAng, abbcOrthoVec);

//      llinfos << "abbcAng      : " << abbcAng << llendl;
//      llinfos << "abbcOrthoVec : " << abbcOrthoVec << llendl;
//      llinfos << "agLenSq      : " << agLenSq << llendl;
//      llinfos << "cosTheta     : " << cosTheta << llendl;
//      llinfos << "theta        : " << theta << llendl;
//      llinfos << "bRot         : " << bRot << llendl;
//      llinfos << "theta abbcAng theta-abbcAng: " << theta*180.0/F_PI << " " << abbcAng*180.0f/F_PI << " " << (theta - abbcAng)*180.0f/F_PI << llendl;

        //-------------------------------------------------------------------------
        // compute rotation that rotates new A->C to A->G
        //-------------------------------------------------------------------------
        // rotate B->C by bRot
        bcVec = bcVec * bRot;

        // update A->C
        acVec = abVec + bcVec;

        LLQuaternion cgRot;
        cgRot.shortestArc( acVec, agVec );

//      llinfos << "bcVec : " << bcVec << llendl;
//      llinfos << "acVec : " << acVec << llendl;
//      llinfos << "cgRot : " << cgRot << llendl;

        // update A->B and B->C with rotation from C to G
        abVec = abVec * cgRot;
        bcVec = bcVec * cgRot;
        abcNorm = abcNorm * cgRot;
        acVec = abVec + bcVec;

        //-------------------------------------------------------------------------
        // compute the normal of the APG plane
        //-------------------------------------------------------------------------
        if (are_parallel(agVec, poleVec, 0.001f))
        {
                // the solution plane is undefined ==> we're done
                return;
        }
        LLVector3 apgNorm = poleVec % agVec;
        apgNorm.normVec();

        if (!mbUseBAxis)
        {
                //---------------------------------------------------------------------
                // compute the normal of the new ABC plane
                // (only necessary if we're NOT using mBAxis)
                //---------------------------------------------------------------------
                if( are_parallel(abVec, bcVec, 0.001f) )
                {
                        // G is either too close or too far away
                        // we'll use the old ABCnormal 
                }
                else
                {
                        abcNorm = abVec % bcVec;
                }
                abcNorm.normVec();
        }

        //-------------------------------------------------------------------------
        // calcuate plane rotation
        //-------------------------------------------------------------------------
        LLQuaternion pRot;
        if ( are_parallel( abcNorm, apgNorm, 0.001f) )
        {
                if (abcNorm * apgNorm < 0.0f)
                {
                        // we must be PI radians off ==> rotate by PI around agVec
                        pRot.setQuat(F_PI, agVec);
                }
                else
                {
                        // we're done
                }
        }
        else
        {
                pRot.shortestArc( abcNorm, apgNorm );
        }

//      llinfos << "abcNorm = " << abcNorm << llendl;
//      llinfos << "apgNorm = " << apgNorm << llendl;
//      llinfos << "pRot = " << pRot << llendl;

        //-------------------------------------------------------------------------
        // compute twist rotation
        //-------------------------------------------------------------------------
        LLQuaternion twistRot( mTwist, agVec );

//      llinfos << "twist    : " << mTwist*180.0/F_PI << llendl;
//      llinfos << "agNormVec: " << agNormVec << llendl;
//      llinfos << "twistRot : " << twistRot << llendl;

        //-------------------------------------------------------------------------
        // compute rotation of A
        //-------------------------------------------------------------------------
        LLQuaternion aRot = cgRot * pRot * twistRot;

        //-------------------------------------------------------------------------
        // apply the rotations
        //-------------------------------------------------------------------------
        mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
        mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
}


// End

---