/indra/llcharacter/lljointsolverrp3.cpp

https://bitbucket.org/lindenlab/viewer-beta/ · C++ · 382 lines · 170 code · 57 blank · 155 comment · 14 complexity · 9a7b42afe06535e663c64efde7027952 MD5 · raw file

  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. // Header Files
  28. //-----------------------------------------------------------------------------
  29. #include "linden_common.h"
  30. #include "lljointsolverrp3.h"
  31. #include "llmath.h"
  32. #define F_EPSILON 0.00001f
  33. //-----------------------------------------------------------------------------
  34. // Constructor
  35. //-----------------------------------------------------------------------------
  36. LLJointSolverRP3::LLJointSolverRP3()
  37. {
  38. mJointA = NULL;
  39. mJointB = NULL;
  40. mJointC = NULL;
  41. mJointGoal = NULL;
  42. mLengthAB = 1.0f;
  43. mLengthBC = 1.0f;
  44. mPoleVector.setVec( 1.0f, 0.0f, 0.0f );
  45. mbUseBAxis = FALSE;
  46. mTwist = 0.0f;
  47. mFirstTime = TRUE;
  48. }
  49. //-----------------------------------------------------------------------------
  50. // Destructor
  51. //-----------------------------------------------------------------------------
  52. /*virtual*/ LLJointSolverRP3::~LLJointSolverRP3()
  53. {
  54. }
  55. //-----------------------------------------------------------------------------
  56. // setupJoints()
  57. //-----------------------------------------------------------------------------
  58. void LLJointSolverRP3::setupJoints( LLJoint* jointA,
  59. LLJoint* jointB,
  60. LLJoint* jointC,
  61. LLJoint* jointGoal )
  62. {
  63. mJointA = jointA;
  64. mJointB = jointB;
  65. mJointC = jointC;
  66. mJointGoal = jointGoal;
  67. mLengthAB = mJointB->getPosition().magVec();
  68. mLengthBC = mJointC->getPosition().magVec();
  69. mJointABaseRotation = jointA->getRotation();
  70. mJointBBaseRotation = jointB->getRotation();
  71. }
  72. //-----------------------------------------------------------------------------
  73. // getPoleVector()
  74. //-----------------------------------------------------------------------------
  75. const LLVector3& LLJointSolverRP3::getPoleVector()
  76. {
  77. return mPoleVector;
  78. }
  79. //-----------------------------------------------------------------------------
  80. // setPoleVector()
  81. //-----------------------------------------------------------------------------
  82. void LLJointSolverRP3::setPoleVector( const LLVector3& poleVector )
  83. {
  84. mPoleVector = poleVector;
  85. mPoleVector.normVec();
  86. }
  87. //-----------------------------------------------------------------------------
  88. // setPoleVector()
  89. //-----------------------------------------------------------------------------
  90. void LLJointSolverRP3::setBAxis( const LLVector3& bAxis )
  91. {
  92. mBAxis = bAxis;
  93. mBAxis.normVec();
  94. mbUseBAxis = TRUE;
  95. }
  96. //-----------------------------------------------------------------------------
  97. // getTwist()
  98. //-----------------------------------------------------------------------------
  99. F32 LLJointSolverRP3::getTwist()
  100. {
  101. return mTwist;
  102. }
  103. //-----------------------------------------------------------------------------
  104. // setTwist()
  105. //-----------------------------------------------------------------------------
  106. void LLJointSolverRP3::setTwist( F32 twist )
  107. {
  108. mTwist = twist;
  109. }
  110. //-----------------------------------------------------------------------------
  111. // solve()
  112. //-----------------------------------------------------------------------------
  113. void LLJointSolverRP3::solve()
  114. {
  115. // llinfos << llendl;
  116. // llinfos << "LLJointSolverRP3::solve()" << llendl;
  117. //-------------------------------------------------------------------------
  118. // setup joints in their base rotations
  119. //-------------------------------------------------------------------------
  120. mJointA->setRotation( mJointABaseRotation );
  121. mJointB->setRotation( mJointBBaseRotation );
  122. //-------------------------------------------------------------------------
  123. // get joint positions in world space
  124. //-------------------------------------------------------------------------
  125. LLVector3 aPos = mJointA->getWorldPosition();
  126. LLVector3 bPos = mJointB->getWorldPosition();
  127. LLVector3 cPos = mJointC->getWorldPosition();
  128. LLVector3 gPos = mJointGoal->getWorldPosition();
  129. // llinfos << "bPosLocal = " << mJointB->getPosition() << llendl;
  130. // llinfos << "cPosLocal = " << mJointC->getPosition() << llendl;
  131. // llinfos << "bRotLocal = " << mJointB->getRotation() << llendl;
  132. // llinfos << "cRotLocal = " << mJointC->getRotation() << llendl;
  133. // llinfos << "aPos : " << aPos << llendl;
  134. // llinfos << "bPos : " << bPos << llendl;
  135. // llinfos << "cPos : " << cPos << llendl;
  136. // llinfos << "gPos : " << gPos << llendl;
  137. //-------------------------------------------------------------------------
  138. // get the poleVector in world space
  139. //-------------------------------------------------------------------------
  140. LLMatrix4 worldJointAParentMat;
  141. if ( mJointA->getParent() )
  142. {
  143. worldJointAParentMat = mJointA->getParent()->getWorldMatrix();
  144. }
  145. LLVector3 poleVec = rotate_vector( mPoleVector, worldJointAParentMat );
  146. //-------------------------------------------------------------------------
  147. // compute the following:
  148. // vector from A to B
  149. // vector from B to C
  150. // vector from A to C
  151. // vector from A to G (goal)
  152. //-------------------------------------------------------------------------
  153. LLVector3 abVec = bPos - aPos;
  154. LLVector3 bcVec = cPos - bPos;
  155. LLVector3 acVec = cPos - aPos;
  156. LLVector3 agVec = gPos - aPos;
  157. // llinfos << "abVec : " << abVec << llendl;
  158. // llinfos << "bcVec : " << bcVec << llendl;
  159. // llinfos << "acVec : " << acVec << llendl;
  160. // llinfos << "agVec : " << agVec << llendl;
  161. //-------------------------------------------------------------------------
  162. // compute needed lengths of those vectors
  163. //-------------------------------------------------------------------------
  164. F32 abLen = abVec.magVec();
  165. F32 bcLen = bcVec.magVec();
  166. F32 agLen = agVec.magVec();
  167. // llinfos << "abLen : " << abLen << llendl;
  168. // llinfos << "bcLen : " << bcLen << llendl;
  169. // llinfos << "agLen : " << agLen << llendl;
  170. //-------------------------------------------------------------------------
  171. // compute component vector of (A->B) orthogonal to (A->C)
  172. //-------------------------------------------------------------------------
  173. LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));
  174. // llinfos << "abacCompOrthoVec : " << abacCompOrthoVec << llendl;
  175. //-------------------------------------------------------------------------
  176. // compute the normal of the original ABC plane (and store for later)
  177. //-------------------------------------------------------------------------
  178. LLVector3 abcNorm;
  179. if (!mbUseBAxis)
  180. {
  181. if( are_parallel(abVec, bcVec, 0.001f) )
  182. {
  183. // the current solution is maxed out, so we use the axis that is
  184. // orthogonal to both poleVec and A->B
  185. if ( are_parallel(poleVec, abVec, 0.001f) )
  186. {
  187. // ACK! the problem is singular
  188. if ( are_parallel(poleVec, agVec, 0.001f) )
  189. {
  190. // the solutions is also singular
  191. return;
  192. }
  193. else
  194. {
  195. abcNorm = poleVec % agVec;
  196. }
  197. }
  198. else
  199. {
  200. abcNorm = poleVec % abVec;
  201. }
  202. }
  203. else
  204. {
  205. abcNorm = abVec % bcVec;
  206. }
  207. }
  208. else
  209. {
  210. abcNorm = mBAxis * mJointB->getWorldRotation();
  211. }
  212. //-------------------------------------------------------------------------
  213. // compute rotation of B
  214. //-------------------------------------------------------------------------
  215. // angle between A->B and B->C
  216. F32 abbcAng = angle_between(abVec, bcVec);
  217. // vector orthogonal to A->B and B->C
  218. LLVector3 abbcOrthoVec = abVec % bcVec;
  219. if (abbcOrthoVec.magVecSquared() < 0.001f)
  220. {
  221. abbcOrthoVec = poleVec % abVec;
  222. abacCompOrthoVec = poleVec;
  223. }
  224. abbcOrthoVec.normVec();
  225. F32 agLenSq = agLen * agLen;
  226. // angle arm for extension
  227. F32 cosTheta = (agLenSq - abLen*abLen - bcLen*bcLen) / (2.0f * abLen * bcLen);
  228. if (cosTheta > 1.0f)
  229. cosTheta = 1.0f;
  230. else if (cosTheta < -1.0f)
  231. cosTheta = -1.0f;
  232. F32 theta = acos(cosTheta);
  233. LLQuaternion bRot(theta - abbcAng, abbcOrthoVec);
  234. // llinfos << "abbcAng : " << abbcAng << llendl;
  235. // llinfos << "abbcOrthoVec : " << abbcOrthoVec << llendl;
  236. // llinfos << "agLenSq : " << agLenSq << llendl;
  237. // llinfos << "cosTheta : " << cosTheta << llendl;
  238. // llinfos << "theta : " << theta << llendl;
  239. // llinfos << "bRot : " << bRot << llendl;
  240. // llinfos << "theta abbcAng theta-abbcAng: " << theta*180.0/F_PI << " " << abbcAng*180.0f/F_PI << " " << (theta - abbcAng)*180.0f/F_PI << llendl;
  241. //-------------------------------------------------------------------------
  242. // compute rotation that rotates new A->C to A->G
  243. //-------------------------------------------------------------------------
  244. // rotate B->C by bRot
  245. bcVec = bcVec * bRot;
  246. // update A->C
  247. acVec = abVec + bcVec;
  248. LLQuaternion cgRot;
  249. cgRot.shortestArc( acVec, agVec );
  250. // llinfos << "bcVec : " << bcVec << llendl;
  251. // llinfos << "acVec : " << acVec << llendl;
  252. // llinfos << "cgRot : " << cgRot << llendl;
  253. // update A->B and B->C with rotation from C to G
  254. abVec = abVec * cgRot;
  255. bcVec = bcVec * cgRot;
  256. abcNorm = abcNorm * cgRot;
  257. acVec = abVec + bcVec;
  258. //-------------------------------------------------------------------------
  259. // compute the normal of the APG plane
  260. //-------------------------------------------------------------------------
  261. if (are_parallel(agVec, poleVec, 0.001f))
  262. {
  263. // the solution plane is undefined ==> we're done
  264. return;
  265. }
  266. LLVector3 apgNorm = poleVec % agVec;
  267. apgNorm.normVec();
  268. if (!mbUseBAxis)
  269. {
  270. //---------------------------------------------------------------------
  271. // compute the normal of the new ABC plane
  272. // (only necessary if we're NOT using mBAxis)
  273. //---------------------------------------------------------------------
  274. if( are_parallel(abVec, bcVec, 0.001f) )
  275. {
  276. // G is either too close or too far away
  277. // we'll use the old ABCnormal
  278. }
  279. else
  280. {
  281. abcNorm = abVec % bcVec;
  282. }
  283. abcNorm.normVec();
  284. }
  285. //-------------------------------------------------------------------------
  286. // calcuate plane rotation
  287. //-------------------------------------------------------------------------
  288. LLQuaternion pRot;
  289. if ( are_parallel( abcNorm, apgNorm, 0.001f) )
  290. {
  291. if (abcNorm * apgNorm < 0.0f)
  292. {
  293. // we must be PI radians off ==> rotate by PI around agVec
  294. pRot.setQuat(F_PI, agVec);
  295. }
  296. else
  297. {
  298. // we're done
  299. }
  300. }
  301. else
  302. {
  303. pRot.shortestArc( abcNorm, apgNorm );
  304. }
  305. // llinfos << "abcNorm = " << abcNorm << llendl;
  306. // llinfos << "apgNorm = " << apgNorm << llendl;
  307. // llinfos << "pRot = " << pRot << llendl;
  308. //-------------------------------------------------------------------------
  309. // compute twist rotation
  310. //-------------------------------------------------------------------------
  311. LLQuaternion twistRot( mTwist, agVec );
  312. // llinfos << "twist : " << mTwist*180.0/F_PI << llendl;
  313. // llinfos << "agNormVec: " << agNormVec << llendl;
  314. // llinfos << "twistRot : " << twistRot << llendl;
  315. //-------------------------------------------------------------------------
  316. // compute rotation of A
  317. //-------------------------------------------------------------------------
  318. LLQuaternion aRot = cgRot * pRot * twistRot;
  319. //-------------------------------------------------------------------------
  320. // apply the rotations
  321. //-------------------------------------------------------------------------
  322. mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
  323. mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
  324. }
  325. // End