LinearR3.cpp 24 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822
  1. /*
  2. *
  3. * Mathematics Subpackage (VrMath)
  4. *
  5. *
  6. * Author: Samuel R. Buss, [email protected].
  7. * Web page: http://math.ucsd.edu/~sbuss/MathCG
  8. *
  9. *
  10. This software is provided 'as-is', without any express or implied warranty.
  11. In no event will the authors be held liable for any damages arising from the use of this software.
  12. Permission is granted to anyone to use this software for any purpose,
  13. including commercial applications, and to alter it and redistribute it freely,
  14. subject to the following restrictions:
  15. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  16. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  17. 3. This notice may not be removed or altered from any source distribution.
  18. *
  19. *
  20. */
  21. #include "MathMisc.h"
  22. #include "LinearR3.h"
  23. #include "Spherical.h"
  24. // ******************************************************
  25. // * VectorR3 class - math library functions *
  26. // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  27. const VectorR3 UnitVecIR3(1.0, 0.0, 0.0);
  28. const VectorR3 UnitVecJR3(0.0, 1.0, 0.0);
  29. const VectorR3 UnitVecKR3(0.0, 0.0, 1.0);
  30. const VectorR3 VectorR3::Zero(0.0, 0.0, 0.0);
  31. const VectorR3 VectorR3::UnitX( 1.0, 0.0, 0.0);
  32. const VectorR3 VectorR3::UnitY( 0.0, 1.0, 0.0);
  33. const VectorR3 VectorR3::UnitZ( 0.0, 0.0, 1.0);
  34. const VectorR3 VectorR3::NegUnitX(-1.0, 0.0, 0.0);
  35. const VectorR3 VectorR3::NegUnitY( 0.0,-1.0, 0.0);
  36. const VectorR3 VectorR3::NegUnitZ( 0.0, 0.0,-1.0);
  37. const Matrix3x3 Matrix3x3::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
  38. const Matrix3x4 Matrix3x4::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
  39. double VectorR3::MaxAbs() const
  40. {
  41. register double m;
  42. m = (x>0.0) ? x : -x;
  43. if ( y>m ) m=y;
  44. else if ( -y >m ) m = -y;
  45. if ( z>m ) m=z;
  46. else if ( -z>m ) m = -z;
  47. return m;
  48. }
  49. VectorR3& VectorR3::Set( const Quaternion& q )
  50. {
  51. double sinhalf = sqrt( Square(q.x)+Square(q.y)+Square(q.z) );
  52. if (sinhalf>0.0) {
  53. double theta = atan2( sinhalf, q.w );
  54. theta += theta;
  55. this->Set( q.x, q.y, q.z );
  56. (*this) *= (theta/sinhalf);
  57. }
  58. else {
  59. this->SetZero();
  60. }
  61. return *this;
  62. }
  63. // *********************************************************************
  64. // Rotation routines *
  65. // *********************************************************************
  66. // s.Rotate(theta, u) rotates s and returns s
  67. // rotated theta degrees around unit vector w.
  68. VectorR3& VectorR3::Rotate( double theta, const VectorR3& w)
  69. {
  70. double c = cos(theta);
  71. double s = sin(theta);
  72. double dotw = (x*w.x + y*w.y + z*w.z);
  73. double v0x = dotw*w.x;
  74. double v0y = dotw*w.y; // v0 = provjection onto w
  75. double v0z = dotw*w.z;
  76. double v1x = x-v0x;
  77. double v1y = y-v0y; // v1 = projection onto plane normal to w
  78. double v1z = z-v0z;
  79. double v2x = w.y*v1z - w.z*v1y;
  80. double v2y = w.z*v1x - w.x*v1z; // v2 = w * v1 (cross product)
  81. double v2z = w.x*v1y - w.y*v1x;
  82. x = v0x + c*v1x + s*v2x;
  83. y = v0y + c*v1y + s*v2y;
  84. z = v0z + c*v1z + s*v2z;
  85. return ( *this );
  86. }
  87. // Rotate unit vector x in the direction of "dir": length of dir is rotation angle.
  88. // x must be a unit vector. dir must be perpindicular to x.
  89. VectorR3& VectorR3::RotateUnitInDirection ( const VectorR3& dir)
  90. {
  91. double theta = dir.NormSq();
  92. if ( theta==0.0 ) {
  93. return *this;
  94. }
  95. else {
  96. theta = sqrt(theta);
  97. double costheta = cos(theta);
  98. double sintheta = sin(theta);
  99. VectorR3 dirUnit = dir/theta;
  100. *this = costheta*(*this) + sintheta*dirUnit;
  101. return ( *this );
  102. }
  103. }
  104. // ******************************************************
  105. // * Matrix3x3 class - math library functions *
  106. // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  107. Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix
  108. {
  109. register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared
  110. register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared
  111. register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared
  112. alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor
  113. beta = 1.0 - 0.5*(beta-1.0);
  114. gamma = 1.0 - 0.5*(gamma-1.0);
  115. m11 *= alpha; // Renormalize first column
  116. m21 *= alpha;
  117. m31 *= alpha;
  118. m12 *= beta; // Renormalize second column
  119. m22 *= beta;
  120. m32 *= beta;
  121. m13 *= gamma;
  122. m23 *= gamma;
  123. m33 *= gamma;
  124. alpha = m11*m12+m21*m22+m31*m32; // First and second column dot product
  125. beta = m11*m13+m21*m23+m31*m33; // First and third column dot product
  126. gamma = m12*m13+m22*m23+m32*m33; // Second and third column dot product
  127. alpha *= 0.5;
  128. beta *= 0.5;
  129. gamma *= 0.5;
  130. register double temp1, temp2;
  131. temp1 = m11-alpha*m12-beta*m13; // Update row1
  132. temp2 = m12-alpha*m11-gamma*m13;
  133. m13 -= beta*m11+gamma*m12;
  134. m11 = temp1;
  135. m12 = temp2;
  136. temp1 = m21-alpha*m22-beta*m23; // Update row2
  137. temp2 = m22-alpha*m21-gamma*m23;
  138. m23 -= beta*m21+gamma*m22;
  139. m21 = temp1;
  140. m22 = temp2;
  141. temp1 = m31-alpha*m32-beta*m33; // Update row3
  142. temp2 = m32-alpha*m31-gamma*m33;
  143. m33 -= beta*m31+gamma*m32;
  144. m31 = temp1;
  145. m32 = temp2;
  146. return *this;
  147. }
  148. void Matrix3x3::OperatorTimesEquals(const Matrix3x3& B) // Matrix product
  149. {
  150. double t1, t2; // temporary values
  151. t1 = m11*B.m11 + m12*B.m21 + m13*B.m31;
  152. t2 = m11*B.m12 + m12*B.m22 + m13*B.m32;
  153. m13 = m11*B.m13 + m12*B.m23 + m13*B.m33;
  154. m11 = t1;
  155. m12 = t2;
  156. t1 = m21*B.m11 + m22*B.m21 + m23*B.m31;
  157. t2 = m21*B.m12 + m22*B.m22 + m23*B.m32;
  158. m23 = m21*B.m13 + m22*B.m23 + m23*B.m33;
  159. m21 = t1;
  160. m22 = t2;
  161. t1 = m31*B.m11 + m32*B.m21 + m33*B.m31;
  162. t2 = m31*B.m12 + m32*B.m22 + m33*B.m32;
  163. m33 = m31*B.m13 + m32*B.m23 + m33*B.m33;
  164. m31 = t1;
  165. m32 = t2;
  166. return;
  167. }
  168. VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution
  169. { // based on Cramer's rule
  170. double sd11 = m22*m33-m23*m32;
  171. double sd21 = m32*m13-m12*m33;
  172. double sd31 = m12*m23-m22*m13;
  173. double sd12 = m31*m23-m21*m33;
  174. double sd22 = m11*m33-m31*m13;
  175. double sd32 = m21*m13-m11*m23;
  176. double sd13 = m21*m32-m31*m22;
  177. double sd23 = m31*m12-m11*m32;
  178. double sd33 = m11*m22-m21*m12;
  179. register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
  180. double rx = (u.x*sd11 + u.y*sd21 + u.z*sd31)*detInv;
  181. double ry = (u.x*sd12 + u.y*sd22 + u.z*sd32)*detInv;
  182. double rz = (u.x*sd13 + u.y*sd23 + u.z*sd33)*detInv;
  183. return ( VectorR3( rx, ry, rz ) );
  184. }
  185. // ******************************************************
  186. // * Matrix3x4 class - math library functions *
  187. // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  188. Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix
  189. {
  190. register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared
  191. register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared
  192. register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared
  193. alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor
  194. beta = 1.0 - 0.5*(beta-1.0);
  195. gamma = 1.0 - 0.5*(gamma-1.0);
  196. m11 *= alpha; // Renormalize first column
  197. m21 *= alpha;
  198. m31 *= alpha;
  199. m12 *= beta; // Renormalize second column
  200. m22 *= beta;
  201. m32 *= beta;
  202. m13 *= gamma;
  203. m23 *= gamma;
  204. m33 *= gamma;
  205. alpha = m11*m12+m21*m22+m31*m32; // First and second column dot product
  206. beta = m11*m13+m21*m23+m31*m33; // First and third column dot product
  207. gamma = m12*m13+m22*m23+m32*m33; // Second and third column dot product
  208. alpha *= 0.5;
  209. beta *= 0.5;
  210. gamma *= 0.5;
  211. register double temp1, temp2;
  212. temp1 = m11-alpha*m12-beta*m13; // Update row1
  213. temp2 = m12-alpha*m11-gamma*m13;
  214. m13 -= beta*m11+gamma*m12;
  215. m11 = temp1;
  216. m12 = temp2;
  217. temp1 = m21-alpha*m22-beta*m23; // Update row2
  218. temp2 = m22-alpha*m21-gamma*m23;
  219. m23 -= beta*m21+gamma*m22;
  220. m21 = temp1;
  221. m22 = temp2;
  222. temp1 = m31-alpha*m32-beta*m33; // Update row3
  223. temp2 = m32-alpha*m31-gamma*m33;
  224. m33 -= beta*m31+gamma*m32;
  225. m31 = temp1;
  226. m32 = temp2;
  227. return *this;
  228. }
  229. void Matrix3x4::OperatorTimesEquals (const Matrix3x4& B) // Composition
  230. {
  231. m14 += m11*B.m14 + m12*B.m24 + m13*B.m34;
  232. m24 += m21*B.m14 + m22*B.m24 + m23*B.m34;
  233. m34 += m31*B.m14 + m32*B.m24 + m33*B.m34;
  234. double t1, t2; // temporary values
  235. t1 = m11*B.m11 + m12*B.m21 + m13*B.m31;
  236. t2 = m11*B.m12 + m12*B.m22 + m13*B.m32;
  237. m13 = m11*B.m13 + m12*B.m23 + m13*B.m33;
  238. m11 = t1;
  239. m12 = t2;
  240. t1 = m21*B.m11 + m22*B.m21 + m23*B.m31;
  241. t2 = m21*B.m12 + m22*B.m22 + m23*B.m32;
  242. m23 = m21*B.m13 + m22*B.m23 + m23*B.m33;
  243. m21 = t1;
  244. m22 = t2;
  245. t1 = m31*B.m11 + m32*B.m21 + m33*B.m31;
  246. t2 = m31*B.m12 + m32*B.m22 + m33*B.m32;
  247. m33 = m31*B.m13 + m32*B.m23 + m33*B.m33;
  248. m31 = t1;
  249. m32 = t2;
  250. }
  251. void Matrix3x4::OperatorTimesEquals (const Matrix3x3& B) // Composition
  252. {
  253. double t1, t2; // temporary values
  254. t1 = m11*B.m11 + m12*B.m21 + m13*B.m31;
  255. t2 = m11*B.m12 + m12*B.m22 + m13*B.m32;
  256. m13 = m11*B.m13 + m12*B.m23 + m13*B.m33;
  257. m11 = t1;
  258. m12 = t2;
  259. t1 = m21*B.m11 + m22*B.m21 + m23*B.m31;
  260. t2 = m21*B.m12 + m22*B.m22 + m23*B.m32;
  261. m23 = m21*B.m13 + m22*B.m23 + m23*B.m33;
  262. m21 = t1;
  263. m22 = t2;
  264. t1 = m31*B.m11 + m32*B.m21 + m33*B.m31;
  265. t2 = m31*B.m12 + m32*B.m22 + m33*B.m32;
  266. m33 = m31*B.m13 + m32*B.m23 + m33*B.m33;
  267. m31 = t1;
  268. m32 = t2;
  269. }
  270. // ******************************************************
  271. // * LinearMapR3 class - math library functions *
  272. // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  273. LinearMapR3 operator* ( const LinearMapR3& A, const LinearMapR3& B)
  274. {
  275. return( LinearMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
  276. A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
  277. A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
  278. A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
  279. A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
  280. A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
  281. A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
  282. A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
  283. A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 ) );
  284. }
  285. double LinearMapR3::Determinant () const // Returns the determinant
  286. {
  287. return ( m11*(m22*m33-m23*m32)
  288. - m12*(m21*m33-m31*m23)
  289. + m13*(m21*m23-m31*m22) );
  290. }
  291. LinearMapR3 LinearMapR3::Inverse() const // Returns inverse
  292. {
  293. double sd11 = m22*m33-m23*m32;
  294. double sd21 = m32*m13-m12*m33;
  295. double sd31 = m12*m23-m22*m13;
  296. double sd12 = m31*m23-m21*m33;
  297. double sd22 = m11*m33-m31*m13;
  298. double sd32 = m21*m13-m11*m23;
  299. double sd13 = m21*m32-m31*m22;
  300. double sd23 = m31*m12-m11*m32;
  301. double sd33 = m11*m22-m21*m12;
  302. register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
  303. return( LinearMapR3( sd11*detInv, sd12*detInv, sd13*detInv,
  304. sd21*detInv, sd22*detInv, sd23*detInv,
  305. sd31*detInv, sd32*detInv, sd33*detInv ) );
  306. }
  307. LinearMapR3& LinearMapR3::Invert() // Converts into inverse.
  308. {
  309. double sd11 = m22*m33-m23*m32;
  310. double sd21 = m32*m13-m12*m33;
  311. double sd31 = m12*m23-m22*m13;
  312. double sd12 = m31*m23-m21*m33;
  313. double sd22 = m11*m33-m31*m13;
  314. double sd32 = m21*m13-m11*m23;
  315. double sd13 = m21*m32-m31*m22;
  316. double sd23 = m31*m12-m11*m32;
  317. double sd33 = m11*m22-m21*m12;
  318. register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
  319. m11 = sd11*detInv;
  320. m12 = sd21*detInv;
  321. m13 = sd31*detInv;
  322. m21 = sd12*detInv;
  323. m22 = sd22*detInv;
  324. m23 = sd32*detInv;
  325. m31 = sd13*detInv;
  326. m32 = sd23*detInv;
  327. m33 = sd33*detInv;
  328. return ( *this );
  329. }
  330. // ******************************************************
  331. // * AffineMapR3 class - math library functions *
  332. // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  333. AffineMapR3 operator* ( const AffineMapR3& A, const AffineMapR3& B )
  334. {
  335. return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
  336. A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
  337. A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
  338. A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
  339. A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
  340. A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
  341. A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
  342. A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
  343. A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33,
  344. A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34 + A.m14,
  345. A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34 + A.m24,
  346. A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 + A.m34));
  347. }
  348. AffineMapR3 operator* ( const LinearMapR3& A, const AffineMapR3& B )
  349. {
  350. return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
  351. A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
  352. A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
  353. A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
  354. A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
  355. A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
  356. A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
  357. A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
  358. A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33,
  359. A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34,
  360. A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34,
  361. A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 ));
  362. }
  363. AffineMapR3 operator* ( const AffineMapR3& A, const LinearMapR3& B )
  364. {
  365. return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
  366. A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
  367. A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
  368. A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
  369. A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
  370. A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
  371. A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
  372. A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
  373. A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33,
  374. A.m14,
  375. A.m24,
  376. A.m34 ) );
  377. }
  378. AffineMapR3 AffineMapR3::Inverse() const // Returns inverse
  379. {
  380. double sd11 = m22*m33-m23*m32;
  381. double sd21 = m32*m13-m12*m33;
  382. double sd31 = m12*m23-m22*m13;
  383. double sd12 = m31*m23-m21*m33;
  384. double sd22 = m11*m33-m31*m13;
  385. double sd32 = m21*m13-m11*m23;
  386. double sd13 = m21*m32-m31*m22;
  387. double sd23 = m31*m12-m11*m32;
  388. double sd33 = m11*m22-m21*m12;
  389. register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
  390. // Make sd's hold the (transpose of) the inverse of the 3x3 part
  391. sd11 *= detInv;
  392. sd12 *= detInv;
  393. sd13 *= detInv;
  394. sd21 *= detInv;
  395. sd22 *= detInv;
  396. sd23 *= detInv;
  397. sd31 *= detInv;
  398. sd32 *= detInv;
  399. sd33 *= detInv;
  400. double sd41 = -(m14*sd11 + m24*sd21 + m34*sd31);
  401. double sd42 = -(m14*sd12 + m24*sd22 + m34*sd32);
  402. double sd43 = -(m14*sd12 + m24*sd23 + m34*sd33);
  403. return( AffineMapR3( sd11, sd12, sd13,
  404. sd21, sd22, sd23,
  405. sd31, sd32, sd33,
  406. sd41, sd42, sd43 ) );
  407. }
  408. AffineMapR3& AffineMapR3::Invert() // Converts into inverse.
  409. {
  410. double sd11 = m22*m33-m23*m32;
  411. double sd21 = m32*m13-m12*m33;
  412. double sd31 = m12*m23-m22*m13;
  413. double sd12 = m31*m23-m21*m33;
  414. double sd22 = m11*m33-m31*m13;
  415. double sd32 = m21*m13-m11*m23;
  416. double sd13 = m21*m32-m31*m22;
  417. double sd23 = m31*m12-m11*m32;
  418. double sd33 = m11*m22-m21*m12;
  419. register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
  420. m11 = sd11*detInv;
  421. m12 = sd21*detInv;
  422. m13 = sd31*detInv;
  423. m21 = sd12*detInv;
  424. m22 = sd22*detInv;
  425. m23 = sd32*detInv;
  426. m31 = sd13*detInv;
  427. m32 = sd23*detInv;
  428. m33 = sd33*detInv;
  429. double sd41 = -(m14*m11 + m24*m12 + m34*m13); // Compare to ::Inverse.
  430. double sd42 = -(m14*m21 + m24*m22 + m34*m23);
  431. double sd43 = -(m14*m31 + m24*m32 + m34*m33);
  432. m14 = sd41;
  433. m24 = sd42;
  434. m34 = sd43;
  435. return ( *this );
  436. }
  437. // **************************************************************
  438. // * RotationMapR3 class - math library functions *
  439. // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
  440. RotationMapR3 operator*(const RotationMapR3& A, const RotationMapR3& B)
  441. // Matrix product (composition)
  442. {
  443. return(RotationMapR3(A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
  444. A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
  445. A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
  446. A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
  447. A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
  448. A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
  449. A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
  450. A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
  451. A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 ) );
  452. }
  453. RotationMapR3& RotationMapR3::Set( const Quaternion& quat )
  454. {
  455. double wSq = quat.w*quat.w;
  456. double xSq = quat.x*quat.x;
  457. double ySq = quat.y*quat.y;
  458. double zSq = quat.z*quat.z;
  459. double Dqwx = 2.0*quat.w*quat.x;
  460. double Dqwy = 2.0*quat.w*quat.y;
  461. double Dqwz = 2.0*quat.w*quat.z;
  462. double Dqxy = 2.0*quat.x*quat.y;
  463. double Dqyz = 2.0*quat.y*quat.z;
  464. double Dqxz = 2.0*quat.x*quat.z;
  465. m11 = wSq+xSq-ySq-zSq;
  466. m22 = wSq-xSq+ySq-zSq;
  467. m33 = wSq-xSq-ySq+zSq;
  468. m12 = Dqxy-Dqwz;
  469. m21 = Dqxy+Dqwz;
  470. m13 = Dqxz+Dqwy;
  471. m31 = Dqxz-Dqwy;
  472. m23 = Dqyz-Dqwx;
  473. m32 = Dqyz+Dqwx;
  474. return *this;
  475. }
  476. RotationMapR3& RotationMapR3::Set( const VectorR3& u, double theta )
  477. {
  478. assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
  479. register double c = cos(theta);
  480. register double s = sin(theta);
  481. register double mc = 1.0-c;
  482. double xmc = u.x*mc;
  483. double xymc = xmc*u.y;
  484. double xzmc = xmc*u.z;
  485. double yzmc = u.y*u.z*mc;
  486. double xs = u.x*s;
  487. double ys = u.y*s;
  488. double zs = u.z*s;
  489. Matrix3x3::Set( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
  490. xymc-zs, u.y*u.y*mc+c, yzmc+xs,
  491. xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
  492. return *this;
  493. }
  494. // The rotation axis vector u MUST be a UNIT vector!!!
  495. RotationMapR3& RotationMapR3::Set( const VectorR3& u, double s, double c )
  496. {
  497. assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
  498. double mc = 1.0-c;
  499. double xmc = u.x*mc;
  500. double xymc = xmc*u.y;
  501. double xzmc = xmc*u.z;
  502. double yzmc = u.y*u.z*mc;
  503. double xs = u.x*s;
  504. double ys = u.y*s;
  505. double zs = u.z*s;
  506. Matrix3x3::Set( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
  507. xymc-zs, u.y*u.y*mc+c, yzmc+xs,
  508. xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
  509. return *this;
  510. }
  511. // ToAxisAndAngle - find a unit vector in the direction of the rotation axis,
  512. // and the angle theta of rotation. Returns true if the rotation angle is non-zero,
  513. // and false if it is zero.
  514. bool RotationMapR3::ToAxisAndAngle( VectorR3* u, double *theta ) const
  515. {
  516. double alpha = m11+m22+m33-1.0;
  517. double beta = sqrt(Square(m32-m23)+Square(m13-m31)+Square(m21-m12));
  518. if ( beta==0.0 ) {
  519. *u = VectorR3::UnitY;
  520. *theta = 0.0;
  521. return false;
  522. }
  523. else {
  524. u->Set(m32-m23, m13-m31, m21-m12);
  525. *u /= beta;
  526. *theta = atan2( beta, alpha );
  527. return true;
  528. }
  529. }
  530. // VrRotate is similar to glRotate. Returns a matrix (LinearMapR3)
  531. // that will perform the rotation when multiplied on the left.
  532. // u is supposed to be a *unit* vector. Otherwise, the LinearMapR3
  533. // returned will be garbage!
  534. RotationMapR3 VrRotate( double theta, const VectorR3& u )
  535. {
  536. RotationMapR3 ret;
  537. ret.Set( u, theta );
  538. return ret;
  539. }
  540. // This version of rotate takes the cosine and sine of theta
  541. // instead of theta. u must still be a unit vector.
  542. RotationMapR3 VrRotate( double c, double s, const VectorR3& u )
  543. {
  544. RotationMapR3 ret;
  545. ret.Set( u, s, c );
  546. return ret;
  547. }
  548. // Returns a RotationMapR3 which rotates the fromVec to be colinear
  549. // with toVec.
  550. RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec)
  551. {
  552. VectorR3 crossVec = fromVec;
  553. crossVec *= toVec;
  554. double sinetheta = crossVec.Norm(); // Not yet normalized!
  555. if ( sinetheta < 1.0e-40 ) {
  556. return ( RotationMapR3(
  557. 1.0, 0.0, 0.0,
  558. 0.0, 1.0, 0.0,
  559. 0.0, 0.0, 1.0) );
  560. }
  561. crossVec /= sinetheta; // Normalize the vector
  562. double scale = 1.0/sqrt(fromVec.NormSq()*toVec.NormSq());
  563. sinetheta *= scale;
  564. double cosinetheta = (fromVec^toVec)*scale;
  565. return ( VrRotate( cosinetheta, sinetheta, crossVec) );
  566. }
  567. // RotateToMap returns a rotation map which rotates fromVec to have the
  568. // same direction as toVec.
  569. // fromVec and toVec should be unit vectors
  570. RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec)
  571. {
  572. VectorR3 crossVec = fromVec;
  573. crossVec *= toVec;
  574. double sintheta = crossVec.Norm();
  575. double costheta = fromVec^toVec;
  576. if ( sintheta <= 1.0e-40 ) {
  577. if ( costheta>0.0 ) {
  578. return ( RotationMapR3(
  579. 1.0, 0.0, 0.0,
  580. 0.0, 1.0, 0.0,
  581. 0.0, 0.0, 1.0) );
  582. }
  583. else {
  584. GetOrtho ( toVec, crossVec ); // Get arbitrary orthonormal vector
  585. return( VrRotate(costheta, sintheta, crossVec ) );
  586. }
  587. }
  588. else {
  589. crossVec /= sintheta; // Normalize the vector
  590. return ( VrRotate( costheta, sintheta, crossVec) );
  591. }
  592. }
  593. // **************************************************************
  594. // * RigidMapR3 class - math library functions *
  595. // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
  596. // The rotation axis vector u MUST be a UNIT vector!!!
  597. RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double theta )
  598. {
  599. assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
  600. register double c = cos(theta);
  601. register double s = sin(theta);
  602. register double mc = 1.0-c;
  603. double xmc = u.x*mc;
  604. double xymc = xmc*u.y;
  605. double xzmc = xmc*u.z;
  606. double yzmc = u.y*u.z*mc;
  607. double xs = u.x*s;
  608. double ys = u.y*s;
  609. double zs = u.z*s;
  610. Matrix3x4::Set3x3( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
  611. xymc-zs, u.y*u.y*mc+c, yzmc+xs,
  612. xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
  613. return *this;
  614. }
  615. // The rotation axis vector u MUST be a UNIT vector!!!
  616. RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double s, double c )
  617. {
  618. assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
  619. double mc = 1.0-c;
  620. double xmc = u.x*mc;
  621. double xymc = xmc*u.y;
  622. double xzmc = xmc*u.z;
  623. double yzmc = u.y*u.z*mc;
  624. double xs = u.x*s;
  625. double ys = u.y*s;
  626. double zs = u.z*s;
  627. Matrix3x4::Set3x3( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
  628. xymc-zs, u.y*u.y*mc+c, yzmc+xs,
  629. xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
  630. return *this;
  631. }
  632. // CalcGlideRotation - converts a rigid map into an equivalent
  633. // glide rotation (screw motion). It returns the rotation axis
  634. // as base point u, and a rotation axis v. The vectors u and v are
  635. // always orthonormal. v will be a unit vector.
  636. // It also returns the glide distance, which is the translation parallel
  637. // to v. Further, it returns the signed rotation angle theta (right hand rule
  638. // specifies the direction.
  639. // The glide rotation means a rotation around the point u with axis direction v.
  640. // Return code "true" if the rotation amount is non-zero. "false" if pure translation.
  641. bool RigidMapR3::CalcGlideRotation( VectorR3* u, VectorR3* v,
  642. double *glideDist, double *rotation ) const
  643. {
  644. // Compare to the code for ToAxisAndAngle.
  645. double alpha = m11+m22+m33-1.0;
  646. double beta = sqrt(Square(m32-m23)+Square(m13-m31)+Square(m21-m12));
  647. if ( beta==0.0 ) {
  648. double vN = m14*m14 + m24*m24 + m34*m34;
  649. if ( vN>0.0 ) {
  650. vN = sqrt(vN);
  651. v->Set( m14, m24, m34 );
  652. *v /= vN;
  653. *glideDist = vN;
  654. }
  655. else {
  656. *v = VectorR3::UnitX;
  657. *glideDist = 0.0;
  658. }
  659. u->SetZero();
  660. *rotation = 0;
  661. return false;
  662. }
  663. else {
  664. v->Set(m32-m23, m13-m31, m21-m12);
  665. *v /= beta; // v - unit vector, rotation axis
  666. *rotation = atan2( beta, alpha );
  667. u->Set( m14, m24, m34 );
  668. *glideDist = ((*u)^(*v));
  669. VectorR3 temp = *v;
  670. temp *= *glideDist;
  671. *u -= temp; // Subtract component in direction of rot. axis v
  672. temp = *v;
  673. temp *= *u;
  674. temp /= tan((*rotation)/2); // temp = (v \times u) / tan(rotation/2)
  675. *u += temp;
  676. *u *= 0.5;
  677. return true;
  678. }
  679. }
  680. // ***************************************************************
  681. // Linear Algebra Utilities *
  682. // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  683. // Returns a righthanded orthonormal basis to complement vector u
  684. void GetOrtho( const VectorR3& u, VectorR3& v, VectorR3& w)
  685. {
  686. if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
  687. v.Set ( u.y, -u.x, 0.0 );
  688. }
  689. else {
  690. v.Set ( 0.0, u.z, -u.y);
  691. }
  692. v.Normalize();
  693. w = u;
  694. w *= v;
  695. w.Normalize();
  696. // w.NormalizeFast();
  697. return;
  698. }
  699. // Returns a vector v orthonormal to unit vector u
  700. void GetOrtho( const VectorR3& u, VectorR3& v )
  701. {
  702. if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
  703. v.Set ( u.y, -u.x, 0.0 );
  704. }
  705. else {
  706. v.Set ( 0.0, u.z, -u.y);
  707. }
  708. v.Normalize();
  709. return;
  710. }
  711. // ***************************************************************
  712. // Stream Output Routines *
  713. // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  714. ostream& operator<< ( ostream& os, const VectorR3& u )
  715. {
  716. return (os << "<" << u.x << "," << u.y << "," << u.z << ">");
  717. }
  718. ostream& operator<< ( ostream& os, const Matrix3x3& A )
  719. {
  720. os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13 << ">\n"
  721. << " <" << A.m21 << ", " << A.m22 << ", " << A.m23 << ">\n"
  722. << " <" << A.m31 << ", " << A.m32 << ", " << A.m33 << ">\n" ;
  723. return (os);
  724. }
  725. ostream& operator<< ( ostream& os, const Matrix3x4& A )
  726. {
  727. os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13
  728. << "; " << A.m14 << ">\n"
  729. << " <" << A.m21 << ", " << A.m22 << ", " << A.m23
  730. << "; " << A.m24 << ">\n"
  731. << " <" << A.m31 << ", " << A.m32 << ", " << A.m33
  732. << "; " << A.m34 << ">\n" ;
  733. return (os);
  734. }