summaryrefslogtreecommitdiff
path: root/indra/llcharacter/lljointsolverrp3.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'indra/llcharacter/lljointsolverrp3.cpp')
-rw-r--r--indra/llcharacter/lljointsolverrp3.cpp518
1 files changed, 259 insertions, 259 deletions
diff --git a/indra/llcharacter/lljointsolverrp3.cpp b/indra/llcharacter/lljointsolverrp3.cpp
index f3d5e2e324..6ec04bdb00 100644
--- a/indra/llcharacter/lljointsolverrp3.cpp
+++ b/indra/llcharacter/lljointsolverrp3.cpp
@@ -1,25 +1,25 @@
-/**
+/**
* @file lljointsolverrp3.cpp
* @brief Implementation of Joint Solver in 3D Real Projective space (RP3). See: https://en.wikipedia.org/wiki/Real_projective_space
*
* $LicenseInfo:firstyear=2001&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$
*/
@@ -46,16 +46,16 @@
//-----------------------------------------------------------------------------
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;
+ 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;
}
@@ -70,21 +70,21 @@ LLJointSolverRP3::LLJointSolverRP3()
//-----------------------------------------------------------------------------
// setupJoints()
//-----------------------------------------------------------------------------
-void LLJointSolverRP3::setupJoints( LLJoint* jointA,
- LLJoint* jointB,
- LLJoint* jointC,
- LLJoint* jointGoal )
+void LLJointSolverRP3::setupJoints( LLJoint* jointA,
+ LLJoint* jointB,
+ LLJoint* jointC,
+ LLJoint* jointGoal )
{
- mJointA = jointA;
- mJointB = jointB;
- mJointC = jointC;
- mJointGoal = jointGoal;
+ mJointA = jointA;
+ mJointB = jointB;
+ mJointC = jointC;
+ mJointGoal = jointGoal;
- mLengthAB = mJointB->getPosition().magVec();
- mLengthBC = mJointC->getPosition().magVec();
+ mLengthAB = mJointB->getPosition().magVec();
+ mLengthBC = mJointC->getPosition().magVec();
- mJointABaseRotation = jointA->getRotation();
- mJointBBaseRotation = jointB->getRotation();
+ mJointABaseRotation = jointA->getRotation();
+ mJointBBaseRotation = jointB->getRotation();
}
@@ -93,7 +93,7 @@ void LLJointSolverRP3::setupJoints( LLJoint* jointA,
//-----------------------------------------------------------------------------
const LLVector3& LLJointSolverRP3::getPoleVector()
{
- return mPoleVector;
+ return mPoleVector;
}
@@ -102,8 +102,8 @@ const LLVector3& LLJointSolverRP3::getPoleVector()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setPoleVector( const LLVector3& poleVector )
{
- mPoleVector = poleVector;
- mPoleVector.normVec();
+ mPoleVector = poleVector;
+ mPoleVector.normVec();
}
@@ -112,9 +112,9 @@ void LLJointSolverRP3::setPoleVector( const LLVector3& poleVector )
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setBAxis( const LLVector3& bAxis )
{
- mBAxis = bAxis;
- mBAxis.normVec();
- mbUseBAxis = TRUE;
+ mBAxis = bAxis;
+ mBAxis.normVec();
+ mbUseBAxis = TRUE;
}
//-----------------------------------------------------------------------------
@@ -122,7 +122,7 @@ void LLJointSolverRP3::setBAxis( const LLVector3& bAxis )
//-----------------------------------------------------------------------------
F32 LLJointSolverRP3::getTwist()
{
- return mTwist;
+ return mTwist;
}
@@ -131,7 +131,7 @@ F32 LLJointSolverRP3::getTwist()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::setTwist( F32 twist )
{
- mTwist = twist;
+ mTwist = twist;
}
@@ -141,254 +141,254 @@ void LLJointSolverRP3::setTwist( F32 twist )
void LLJointSolverRP3::solve()
{
- //-------------------------------------------------------------------------
- // setup joints in their base rotations
- //-------------------------------------------------------------------------
- mJointA->setRotation( mJointABaseRotation );
- mJointB->setRotation( mJointBBaseRotation );
+ //-------------------------------------------------------------------------
+ // 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();
+ //-------------------------------------------------------------------------
+ // get joint positions in world space
+ //-------------------------------------------------------------------------
+ LLVector3 aPos = mJointA->getWorldPosition();
+ LLVector3 bPos = mJointB->getWorldPosition();
+ LLVector3 cPos = mJointC->getWorldPosition();
+ LLVector3 gPos = mJointGoal->getWorldPosition();
#if DEBUG_JOINT_SOLVER
- LL_DEBUGS("JointSolver") << "LLJointSolverRP3::solve()" << LL_NEWLINE
- << "bPosLocal = " << mJointB->getPosition() << LL_NEWLINE
- << "cPosLocal = " << mJointC->getPosition() << LL_NEWLINE
- << "bRotLocal = " << mJointB->getRotation() << LL_NEWLINE
- << "cRotLocal = " << mJointC->getRotation() << LL_NEWLINE
- << "aPos : " << aPos << LL_NEWLINE
- << "bPos : " << bPos << LL_NEWLINE
- << "cPos : " << cPos << LL_NEWLINE
- << "gPos : " << gPos << LL_ENDL;
+ LL_DEBUGS("JointSolver") << "LLJointSolverRP3::solve()" << LL_NEWLINE
+ << "bPosLocal = " << mJointB->getPosition() << LL_NEWLINE
+ << "cPosLocal = " << mJointC->getPosition() << LL_NEWLINE
+ << "bRotLocal = " << mJointB->getRotation() << LL_NEWLINE
+ << "cRotLocal = " << mJointC->getRotation() << LL_NEWLINE
+ << "aPos : " << aPos << LL_NEWLINE
+ << "bPos : " << bPos << LL_NEWLINE
+ << "cPos : " << cPos << LL_NEWLINE
+ << "gPos : " << gPos << LL_ENDL;
#endif
- //-------------------------------------------------------------------------
- // 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;
-
- //-------------------------------------------------------------------------
- // compute needed lengths of those vectors
- //-------------------------------------------------------------------------
- F32 abLen = abVec.magVec();
- F32 bcLen = bcVec.magVec();
- F32 agLen = agVec.magVec();
-
- //-------------------------------------------------------------------------
- // compute component vector of (A->B) orthogonal to (A->C)
- //-------------------------------------------------------------------------
- LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));
+ //-------------------------------------------------------------------------
+ // 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;
+
+ //-------------------------------------------------------------------------
+ // compute needed lengths of those vectors
+ //-------------------------------------------------------------------------
+ F32 abLen = abVec.magVec();
+ F32 bcLen = bcVec.magVec();
+ F32 agLen = agVec.magVec();
+
+ //-------------------------------------------------------------------------
+ // compute component vector of (A->B) orthogonal to (A->C)
+ //-------------------------------------------------------------------------
+ LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));
#if DEBUG_JOINT_SOLVER
- LL_DEBUGS("JointSolver") << "abVec : " << abVec << LL_NEWLINE
- << "bcVec : " << bcVec << LL_NEWLINE
- << "acVec : " << acVec << LL_NEWLINE
- << "agVec : " << agVec << LL_NEWLINE
- << "abLen : " << abLen << LL_NEWLINE
- << "bcLen : " << bcLen << LL_NEWLINE
- << "agLen : " << agLen << LL_NEWLINE
- << "abacCompOrthoVec : " << abacCompOrthoVec << LL_ENDL;
+ LL_DEBUGS("JointSolver") << "abVec : " << abVec << LL_NEWLINE
+ << "bcVec : " << bcVec << LL_NEWLINE
+ << "acVec : " << acVec << LL_NEWLINE
+ << "agVec : " << agVec << LL_NEWLINE
+ << "abLen : " << abLen << LL_NEWLINE
+ << "bcLen : " << bcLen << LL_NEWLINE
+ << "agLen : " << agLen << LL_NEWLINE
+ << "abacCompOrthoVec : " << abacCompOrthoVec << LL_ENDL;
#endif
- //-------------------------------------------------------------------------
- // 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);
+ //-------------------------------------------------------------------------
+ // 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);
#if DEBUG_JOINT_SOLVER
- LL_DEBUGS("JointSolver") << "abbcAng : " << abbcAng << LL_NEWLINE
- << "abbcOrthoVec : " << abbcOrthoVec << LL_NEWLINE
- << "agLenSq : " << agLenSq << LL_NEWLINE
- << "cosTheta : " << cosTheta << LL_NEWLINE
- << "theta : " << theta << LL_NEWLINE
- << "bRot : " << bRot << LL_NEWLINE
- << "theta abbcAng theta-abbcAng: "
- << theta*180.0/F_PI << " "
- << abbcAng*180.0f/F_PI << " "
- << (theta - abbcAng)*180.0f/F_PI
- << LL_ENDL;
+ LL_DEBUGS("JointSolver") << "abbcAng : " << abbcAng << LL_NEWLINE
+ << "abbcOrthoVec : " << abbcOrthoVec << LL_NEWLINE
+ << "agLenSq : " << agLenSq << LL_NEWLINE
+ << "cosTheta : " << cosTheta << LL_NEWLINE
+ << "theta : " << theta << LL_NEWLINE
+ << "bRot : " << bRot << LL_NEWLINE
+ << "theta abbcAng theta-abbcAng: "
+ << theta*180.0/F_PI << " "
+ << abbcAng*180.0f/F_PI << " "
+ << (theta - abbcAng)*180.0f/F_PI
+ << LL_ENDL;
#endif
- //-------------------------------------------------------------------------
- // compute rotation that rotates new A->C to A->G
- //-------------------------------------------------------------------------
- // rotate B->C by bRot
- bcVec = bcVec * bRot;
+ //-------------------------------------------------------------------------
+ // compute rotation that rotates new A->C to A->G
+ //-------------------------------------------------------------------------
+ // rotate B->C by bRot
+ bcVec = bcVec * bRot;
- // update A->C
- acVec = abVec + bcVec;
+ // update A->C
+ acVec = abVec + bcVec;
- LLQuaternion cgRot;
- cgRot.shortestArc( acVec, agVec );
+ LLQuaternion cgRot;
+ cgRot.shortestArc( acVec, agVec );
#if DEBUG_JOINT_SOLVER
- LL_DEBUGS("JointSolver") << "bcVec : " << bcVec << LL_NEWLINE
- << "acVec : " << acVec << LL_NEWLINE
- << "cgRot : " << cgRot << LL_ENDL;
+ LL_DEBUGS("JointSolver") << "bcVec : " << bcVec << LL_NEWLINE
+ << "acVec : " << acVec << LL_NEWLINE
+ << "cgRot : " << cgRot << LL_ENDL;
#endif
- // 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 );
- }
-
- //-------------------------------------------------------------------------
- // compute twist rotation
- //-------------------------------------------------------------------------
- LLQuaternion twistRot( mTwist, agVec );
+ // 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 );
+ }
+
+ //-------------------------------------------------------------------------
+ // compute twist rotation
+ //-------------------------------------------------------------------------
+ LLQuaternion twistRot( mTwist, agVec );
#if DEBUG_JOINT_SOLVER
- LL_DEBUGS("JointSolver") << "abcNorm = " << abcNorm << LL_NEWLINE
- << "apgNorm = " << apgNorm << LL_NEWLINE
- << "pRot = " << pRot << LL_NEWLINE
- << "twist : " << mTwist*180.0/F_PI << LL_NEWLINE
- << "twistRot : " << twistRot << LL_ENDL;
+ LL_DEBUGS("JointSolver") << "abcNorm = " << abcNorm << LL_NEWLINE
+ << "apgNorm = " << apgNorm << LL_NEWLINE
+ << "pRot = " << pRot << LL_NEWLINE
+ << "twist : " << mTwist*180.0/F_PI << LL_NEWLINE
+ << "twistRot : " << twistRot << LL_ENDL;
#endif
- //-------------------------------------------------------------------------
- // compute rotation of A
- //-------------------------------------------------------------------------
- LLQuaternion aRot = cgRot * pRot * twistRot;
+ //-------------------------------------------------------------------------
+ // compute rotation of A
+ //-------------------------------------------------------------------------
+ LLQuaternion aRot = cgRot * pRot * twistRot;
- //-------------------------------------------------------------------------
- // apply the rotations
- //-------------------------------------------------------------------------
- mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
- mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
+ //-------------------------------------------------------------------------
+ // apply the rotations
+ //-------------------------------------------------------------------------
+ mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
+ mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-10 00:18:43 +0000