/indra/llcharacter/lljointsolverrp3.cpp

  1/** 
  2 * @file lljointsolverrp3.cpp
  3 * @brief Implementation of LLJointSolverRP3 class.
  4 *
  5 * $LicenseInfo:firstyear=2001&license=viewerlgpl$
  6 * Second Life Viewer Source Code
  7 * Copyright (C) 2010, Linden Research, Inc.
  8 * 
  9 * This library is free software; you can redistribute it and/or
 10 * modify it under the terms of the GNU Lesser General Public
 11 * License as published by the Free Software Foundation;
 12 * version 2.1 of the License only.
 13 * 
 14 * This library is distributed in the hope that it will be useful,
 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 17 * Lesser General Public License for more details.
 18 * 
 19 * You should have received a copy of the GNU Lesser General Public
 20 * License along with this library; if not, write to the Free Software
 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 22 * 
 23 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 24 * $/LicenseInfo$
 25 */
 26
 27//-----------------------------------------------------------------------------
 28// Header Files
 29//-----------------------------------------------------------------------------
 30#include "linden_common.h"
 31
 32#include "lljointsolverrp3.h"
 33
 34#include "llmath.h"
 35
 36#define F_EPSILON 0.00001f
 37
 38
 39//-----------------------------------------------------------------------------
 40// Constructor
 41//-----------------------------------------------------------------------------
 42LLJointSolverRP3::LLJointSolverRP3()
 43{
 44	mJointA = NULL;
 45	mJointB = NULL;
 46	mJointC = NULL;
 47	mJointGoal = NULL;
 48	mLengthAB = 1.0f;
 49	mLengthBC = 1.0f;
 50	mPoleVector.setVec( 1.0f, 0.0f, 0.0f );
 51	mbUseBAxis = FALSE;
 52	mTwist = 0.0f;
 53	mFirstTime = TRUE;
 54}
 55
 56
 57//-----------------------------------------------------------------------------
 58// Destructor
 59//-----------------------------------------------------------------------------
 60/*virtual*/ LLJointSolverRP3::~LLJointSolverRP3()
 61{
 62}
 63
 64
 65//-----------------------------------------------------------------------------
 66// setupJoints()
 67//-----------------------------------------------------------------------------
 68void LLJointSolverRP3::setupJoints(	LLJoint* jointA,
 69									LLJoint* jointB,
 70									LLJoint* jointC,
 71									LLJoint* jointGoal )
 72{
 73	mJointA = jointA;
 74	mJointB = jointB;
 75	mJointC = jointC;
 76	mJointGoal = jointGoal;
 77
 78	mLengthAB = mJointB->getPosition().magVec();
 79	mLengthBC = mJointC->getPosition().magVec();
 80
 81	mJointABaseRotation = jointA->getRotation();
 82	mJointBBaseRotation = jointB->getRotation();
 83}
 84
 85
 86//-----------------------------------------------------------------------------
 87// getPoleVector()
 88//-----------------------------------------------------------------------------
 89const LLVector3& LLJointSolverRP3::getPoleVector()
 90{
 91	return mPoleVector;
 92}
 93
 94
 95//-----------------------------------------------------------------------------
 96// setPoleVector()
 97//-----------------------------------------------------------------------------
 98void LLJointSolverRP3::setPoleVector( const LLVector3& poleVector )
 99{
100	mPoleVector = poleVector;
101	mPoleVector.normVec();
102}
103
104
105//-----------------------------------------------------------------------------
106// setPoleVector()
107//-----------------------------------------------------------------------------
108void LLJointSolverRP3::setBAxis( const LLVector3& bAxis )
109{
110	mBAxis = bAxis;
111	mBAxis.normVec();
112	mbUseBAxis = TRUE;
113}
114
115//-----------------------------------------------------------------------------
116// getTwist()
117//-----------------------------------------------------------------------------
118F32 LLJointSolverRP3::getTwist()
119{
120	return mTwist;
121}
122
123
124//-----------------------------------------------------------------------------
125// setTwist()
126//-----------------------------------------------------------------------------
127void LLJointSolverRP3::setTwist( F32 twist )
128{
129	mTwist = twist;
130}
131
132
133//-----------------------------------------------------------------------------
134// solve()
135//-----------------------------------------------------------------------------
136void LLJointSolverRP3::solve()
137{
138//	llinfos << llendl;
139//	llinfos << "LLJointSolverRP3::solve()" << llendl;
140
141	//-------------------------------------------------------------------------
142	// setup joints in their base rotations
143	//-------------------------------------------------------------------------
144	mJointA->setRotation( mJointABaseRotation );
145	mJointB->setRotation( mJointBBaseRotation );
146
147	//-------------------------------------------------------------------------
148	// get joint positions in world space
149	//-------------------------------------------------------------------------
150	LLVector3 aPos = mJointA->getWorldPosition();
151	LLVector3 bPos = mJointB->getWorldPosition();
152	LLVector3 cPos = mJointC->getWorldPosition();
153	LLVector3 gPos = mJointGoal->getWorldPosition();
154
155//	llinfos << "bPosLocal = " << mJointB->getPosition() << llendl;
156//	llinfos << "cPosLocal = " << mJointC->getPosition() << llendl;
157//	llinfos << "bRotLocal = " << mJointB->getRotation() << llendl;
158//	llinfos << "cRotLocal = " << mJointC->getRotation() << llendl;
159
160//	llinfos << "aPos : " << aPos << llendl;
161//	llinfos << "bPos : " << bPos << llendl;
162//	llinfos << "cPos : " << cPos << llendl;
163//	llinfos << "gPos : " << gPos << llendl;
164
165	//-------------------------------------------------------------------------
166	// get the poleVector in world space
167	//-------------------------------------------------------------------------
168	LLMatrix4 worldJointAParentMat;
169	if ( mJointA->getParent() )
170	{
171		worldJointAParentMat = mJointA->getParent()->getWorldMatrix();
172	}
173	LLVector3 poleVec = rotate_vector( mPoleVector, worldJointAParentMat );
174
175	//-------------------------------------------------------------------------
176	// compute the following:
177	// vector from A to B
178	// vector from B to C
179	// vector from A to C
180	// vector from A to G (goal)
181	//-------------------------------------------------------------------------
182	LLVector3 abVec = bPos - aPos;
183	LLVector3 bcVec = cPos - bPos;
184	LLVector3 acVec = cPos - aPos;
185	LLVector3 agVec = gPos - aPos;
186
187//	llinfos << "abVec : " << abVec << llendl;
188//	llinfos << "bcVec : " << bcVec << llendl;
189//	llinfos << "acVec : " << acVec << llendl;
190//	llinfos << "agVec : " << agVec << llendl;
191
192	//-------------------------------------------------------------------------
193	// compute needed lengths of those vectors
194	//-------------------------------------------------------------------------
195	F32 abLen = abVec.magVec();
196	F32 bcLen = bcVec.magVec();
197	F32 agLen = agVec.magVec();
198
199//	llinfos << "abLen : " << abLen << llendl;
200//	llinfos << "bcLen : " << bcLen << llendl;
201//	llinfos << "agLen : " << agLen << llendl;
202
203	//-------------------------------------------------------------------------
204	// compute component vector of (A->B) orthogonal to (A->C)
205	//-------------------------------------------------------------------------
206	LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));
207
208//	llinfos << "abacCompOrthoVec : " << abacCompOrthoVec << llendl;
209
210	//-------------------------------------------------------------------------
211	// compute the normal of the original ABC plane (and store for later)
212	//-------------------------------------------------------------------------
213	LLVector3 abcNorm;
214	if (!mbUseBAxis)
215	{
216		if( are_parallel(abVec, bcVec, 0.001f) )
217		{
218			// the current solution is maxed out, so we use the axis that is
219			// orthogonal to both poleVec and A->B
220			if ( are_parallel(poleVec, abVec, 0.001f) )
221			{
222				// ACK! the problem is singular
223				if ( are_parallel(poleVec, agVec, 0.001f) )
224				{
225					// the solutions is also singular
226					return;
227				}
228				else
229				{
230					abcNorm = poleVec % agVec;
231				}
232			}
233			else
234			{
235				abcNorm = poleVec % abVec;
236			}
237		}
238		else
239		{
240			abcNorm = abVec % bcVec;
241		}
242	}
243	else
244	{
245		abcNorm = mBAxis * mJointB->getWorldRotation();
246	}
247
248	//-------------------------------------------------------------------------
249	// compute rotation of B
250	//-------------------------------------------------------------------------
251	// angle between A->B and B->C
252	F32 abbcAng = angle_between(abVec, bcVec);
253
254	// vector orthogonal to A->B and B->C
255	LLVector3 abbcOrthoVec = abVec % bcVec;
256	if (abbcOrthoVec.magVecSquared() < 0.001f)
257	{
258		abbcOrthoVec = poleVec % abVec;
259		abacCompOrthoVec = poleVec;
260	}
261	abbcOrthoVec.normVec();
262
263	F32 agLenSq = agLen * agLen;
264
265	// angle arm for extension
266	F32 cosTheta =	(agLenSq - abLen*abLen - bcLen*bcLen) / (2.0f * abLen * bcLen);
267	if (cosTheta > 1.0f)
268		cosTheta = 1.0f;
269	else if (cosTheta < -1.0f)
270		cosTheta = -1.0f;
271
272	F32 theta = acos(cosTheta);
273
274	LLQuaternion bRot(theta - abbcAng, abbcOrthoVec);
275
276//	llinfos << "abbcAng      : " << abbcAng << llendl;
277//	llinfos << "abbcOrthoVec : " << abbcOrthoVec << llendl;
278//	llinfos << "agLenSq      : " << agLenSq << llendl;
279//	llinfos << "cosTheta     : " << cosTheta << llendl;
280//	llinfos << "theta        : " << theta << llendl;
281//	llinfos << "bRot         : " << bRot << llendl;
282//	llinfos << "theta abbcAng theta-abbcAng: " << theta*180.0/F_PI << " " << abbcAng*180.0f/F_PI << " " << (theta - abbcAng)*180.0f/F_PI << llendl;
283
284	//-------------------------------------------------------------------------
285	// compute rotation that rotates new A->C to A->G
286	//-------------------------------------------------------------------------
287	// rotate B->C by bRot
288	bcVec = bcVec * bRot;
289
290	// update A->C
291	acVec = abVec + bcVec;
292
293	LLQuaternion cgRot;
294	cgRot.shortestArc( acVec, agVec );
295
296//	llinfos << "bcVec : " << bcVec << llendl;
297//	llinfos << "acVec : " << acVec << llendl;
298//	llinfos << "cgRot : " << cgRot << llendl;
299
300	// update A->B and B->C with rotation from C to G
301	abVec = abVec * cgRot;
302	bcVec = bcVec * cgRot;
303	abcNorm = abcNorm * cgRot;
304	acVec = abVec + bcVec;
305
306	//-------------------------------------------------------------------------
307	// compute the normal of the APG plane
308	//-------------------------------------------------------------------------
309	if (are_parallel(agVec, poleVec, 0.001f))
310	{
311		// the solution plane is undefined ==> we're done
312		return;
313	}
314	LLVector3 apgNorm = poleVec % agVec;
315	apgNorm.normVec();
316
317	if (!mbUseBAxis)
318	{
319		//---------------------------------------------------------------------
320		// compute the normal of the new ABC plane
321		// (only necessary if we're NOT using mBAxis)
322		//---------------------------------------------------------------------
323		if( are_parallel(abVec, bcVec, 0.001f) )
324		{
325			// G is either too close or too far away
326			// we'll use the old ABCnormal 
327		}
328		else
329		{
330			abcNorm = abVec % bcVec;
331		}
332		abcNorm.normVec();
333	}
334
335	//-------------------------------------------------------------------------
336	// calcuate plane rotation
337	//-------------------------------------------------------------------------
338	LLQuaternion pRot;
339	if ( are_parallel( abcNorm, apgNorm, 0.001f) )
340	{
341		if (abcNorm * apgNorm < 0.0f)
342		{
343			// we must be PI radians off ==> rotate by PI around agVec
344			pRot.setQuat(F_PI, agVec);
345		}
346		else
347		{
348			// we're done
349		}
350	}
351	else
352	{
353		pRot.shortestArc( abcNorm, apgNorm );
354	}
355
356//	llinfos << "abcNorm = " << abcNorm << llendl;
357//	llinfos << "apgNorm = " << apgNorm << llendl;
358//	llinfos << "pRot = " << pRot << llendl;
359
360	//-------------------------------------------------------------------------
361	// compute twist rotation
362	//-------------------------------------------------------------------------
363	LLQuaternion twistRot( mTwist, agVec );
364
365//	llinfos	<< "twist    : " << mTwist*180.0/F_PI << llendl;
366//	llinfos << "agNormVec: " << agNormVec << llendl;
367//	llinfos << "twistRot : " << twistRot << llendl;
368
369	//-------------------------------------------------------------------------
370	// compute rotation of A
371	//-------------------------------------------------------------------------
372	LLQuaternion aRot = cgRot * pRot * twistRot;
373
374	//-------------------------------------------------------------------------
375	// apply the rotations
376	//-------------------------------------------------------------------------
377	mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
378	mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
379}
380
381
382// End