123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822 |
- /*
- *
- * Mathematics Subpackage (VrMath)
- *
- *
- * Author: Samuel R. Buss, [email protected].
- * Web page: http://math.ucsd.edu/~sbuss/MathCG
- *
- *
- This software is provided 'as-is', without any express or implied warranty.
- In no event will the authors be held liable for any damages arising from the use of this software.
- Permission is granted to anyone to use this software for any purpose,
- including commercial applications, and to alter it and redistribute it freely,
- subject to the following restrictions:
- 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.
- 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
- 3. This notice may not be removed or altered from any source distribution.
- *
- *
- */
- #include "MathMisc.h"
- #include "LinearR3.h"
- #include "Spherical.h"
- // ******************************************************
- // * VectorR3 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * **
- const VectorR3 UnitVecIR3(1.0, 0.0, 0.0);
- const VectorR3 UnitVecJR3(0.0, 1.0, 0.0);
- const VectorR3 UnitVecKR3(0.0, 0.0, 1.0);
- const VectorR3 VectorR3::Zero(0.0, 0.0, 0.0);
- const VectorR3 VectorR3::UnitX( 1.0, 0.0, 0.0);
- const VectorR3 VectorR3::UnitY( 0.0, 1.0, 0.0);
- const VectorR3 VectorR3::UnitZ( 0.0, 0.0, 1.0);
- const VectorR3 VectorR3::NegUnitX(-1.0, 0.0, 0.0);
- const VectorR3 VectorR3::NegUnitY( 0.0,-1.0, 0.0);
- const VectorR3 VectorR3::NegUnitZ( 0.0, 0.0,-1.0);
- const Matrix3x3 Matrix3x3::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
- 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);
- double VectorR3::MaxAbs() const
- {
- register double m;
- m = (x>0.0) ? x : -x;
- if ( y>m ) m=y;
- else if ( -y >m ) m = -y;
- if ( z>m ) m=z;
- else if ( -z>m ) m = -z;
- return m;
- }
- VectorR3& VectorR3::Set( const Quaternion& q )
- {
- double sinhalf = sqrt( Square(q.x)+Square(q.y)+Square(q.z) );
- if (sinhalf>0.0) {
- double theta = atan2( sinhalf, q.w );
- theta += theta;
- this->Set( q.x, q.y, q.z );
- (*this) *= (theta/sinhalf);
- }
- else {
- this->SetZero();
- }
- return *this;
- }
- // *********************************************************************
- // Rotation routines *
- // *********************************************************************
- // s.Rotate(theta, u) rotates s and returns s
- // rotated theta degrees around unit vector w.
- VectorR3& VectorR3::Rotate( double theta, const VectorR3& w)
- {
- double c = cos(theta);
- double s = sin(theta);
- double dotw = (x*w.x + y*w.y + z*w.z);
- double v0x = dotw*w.x;
- double v0y = dotw*w.y; // v0 = provjection onto w
- double v0z = dotw*w.z;
- double v1x = x-v0x;
- double v1y = y-v0y; // v1 = projection onto plane normal to w
- double v1z = z-v0z;
- double v2x = w.y*v1z - w.z*v1y;
- double v2y = w.z*v1x - w.x*v1z; // v2 = w * v1 (cross product)
- double v2z = w.x*v1y - w.y*v1x;
-
- x = v0x + c*v1x + s*v2x;
- y = v0y + c*v1y + s*v2y;
- z = v0z + c*v1z + s*v2z;
- return ( *this );
- }
- // Rotate unit vector x in the direction of "dir": length of dir is rotation angle.
- // x must be a unit vector. dir must be perpindicular to x.
- VectorR3& VectorR3::RotateUnitInDirection ( const VectorR3& dir)
- {
- double theta = dir.NormSq();
- if ( theta==0.0 ) {
- return *this;
- }
- else {
- theta = sqrt(theta);
- double costheta = cos(theta);
- double sintheta = sin(theta);
- VectorR3 dirUnit = dir/theta;
- *this = costheta*(*this) + sintheta*dirUnit;
- return ( *this );
- }
- }
- // ******************************************************
- // * Matrix3x3 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * **
- Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix
- {
- register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared
- register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared
- register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared
- alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor
- beta = 1.0 - 0.5*(beta-1.0);
- gamma = 1.0 - 0.5*(gamma-1.0);
- m11 *= alpha; // Renormalize first column
- m21 *= alpha;
- m31 *= alpha;
- m12 *= beta; // Renormalize second column
- m22 *= beta;
- m32 *= beta;
- m13 *= gamma;
- m23 *= gamma;
- m33 *= gamma;
- alpha = m11*m12+m21*m22+m31*m32; // First and second column dot product
- beta = m11*m13+m21*m23+m31*m33; // First and third column dot product
- gamma = m12*m13+m22*m23+m32*m33; // Second and third column dot product
- alpha *= 0.5;
- beta *= 0.5;
- gamma *= 0.5;
- register double temp1, temp2;
- temp1 = m11-alpha*m12-beta*m13; // Update row1
- temp2 = m12-alpha*m11-gamma*m13;
- m13 -= beta*m11+gamma*m12;
- m11 = temp1;
- m12 = temp2;
- temp1 = m21-alpha*m22-beta*m23; // Update row2
- temp2 = m22-alpha*m21-gamma*m23;
- m23 -= beta*m21+gamma*m22;
- m21 = temp1;
- m22 = temp2;
- temp1 = m31-alpha*m32-beta*m33; // Update row3
- temp2 = m32-alpha*m31-gamma*m33;
- m33 -= beta*m31+gamma*m32;
- m31 = temp1;
- m32 = temp2;
- return *this;
- }
- void Matrix3x3::OperatorTimesEquals(const Matrix3x3& B) // Matrix product
- {
- double t1, t2; // temporary values
- t1 = m11*B.m11 + m12*B.m21 + m13*B.m31;
- t2 = m11*B.m12 + m12*B.m22 + m13*B.m32;
- m13 = m11*B.m13 + m12*B.m23 + m13*B.m33;
- m11 = t1;
- m12 = t2;
- t1 = m21*B.m11 + m22*B.m21 + m23*B.m31;
- t2 = m21*B.m12 + m22*B.m22 + m23*B.m32;
- m23 = m21*B.m13 + m22*B.m23 + m23*B.m33;
- m21 = t1;
- m22 = t2;
- t1 = m31*B.m11 + m32*B.m21 + m33*B.m31;
- t2 = m31*B.m12 + m32*B.m22 + m33*B.m32;
- m33 = m31*B.m13 + m32*B.m23 + m33*B.m33;
- m31 = t1;
- m32 = t2;
- return;
- }
- VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution
- { // based on Cramer's rule
- double sd11 = m22*m33-m23*m32;
- double sd21 = m32*m13-m12*m33;
- double sd31 = m12*m23-m22*m13;
- double sd12 = m31*m23-m21*m33;
- double sd22 = m11*m33-m31*m13;
- double sd32 = m21*m13-m11*m23;
- double sd13 = m21*m32-m31*m22;
- double sd23 = m31*m12-m11*m32;
- double sd33 = m11*m22-m21*m12;
- register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
- double rx = (u.x*sd11 + u.y*sd21 + u.z*sd31)*detInv;
- double ry = (u.x*sd12 + u.y*sd22 + u.z*sd32)*detInv;
- double rz = (u.x*sd13 + u.y*sd23 + u.z*sd33)*detInv;
- return ( VectorR3( rx, ry, rz ) );
- }
- // ******************************************************
- // * Matrix3x4 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * **
- Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix
- {
- register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared
- register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared
- register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared
- alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor
- beta = 1.0 - 0.5*(beta-1.0);
- gamma = 1.0 - 0.5*(gamma-1.0);
- m11 *= alpha; // Renormalize first column
- m21 *= alpha;
- m31 *= alpha;
- m12 *= beta; // Renormalize second column
- m22 *= beta;
- m32 *= beta;
- m13 *= gamma;
- m23 *= gamma;
- m33 *= gamma;
- alpha = m11*m12+m21*m22+m31*m32; // First and second column dot product
- beta = m11*m13+m21*m23+m31*m33; // First and third column dot product
- gamma = m12*m13+m22*m23+m32*m33; // Second and third column dot product
- alpha *= 0.5;
- beta *= 0.5;
- gamma *= 0.5;
- register double temp1, temp2;
- temp1 = m11-alpha*m12-beta*m13; // Update row1
- temp2 = m12-alpha*m11-gamma*m13;
- m13 -= beta*m11+gamma*m12;
- m11 = temp1;
- m12 = temp2;
- temp1 = m21-alpha*m22-beta*m23; // Update row2
- temp2 = m22-alpha*m21-gamma*m23;
- m23 -= beta*m21+gamma*m22;
- m21 = temp1;
- m22 = temp2;
- temp1 = m31-alpha*m32-beta*m33; // Update row3
- temp2 = m32-alpha*m31-gamma*m33;
- m33 -= beta*m31+gamma*m32;
- m31 = temp1;
- m32 = temp2;
- return *this;
- }
- void Matrix3x4::OperatorTimesEquals (const Matrix3x4& B) // Composition
- {
- m14 += m11*B.m14 + m12*B.m24 + m13*B.m34;
- m24 += m21*B.m14 + m22*B.m24 + m23*B.m34;
- m34 += m31*B.m14 + m32*B.m24 + m33*B.m34;
- double t1, t2; // temporary values
- t1 = m11*B.m11 + m12*B.m21 + m13*B.m31;
- t2 = m11*B.m12 + m12*B.m22 + m13*B.m32;
- m13 = m11*B.m13 + m12*B.m23 + m13*B.m33;
- m11 = t1;
- m12 = t2;
- t1 = m21*B.m11 + m22*B.m21 + m23*B.m31;
- t2 = m21*B.m12 + m22*B.m22 + m23*B.m32;
- m23 = m21*B.m13 + m22*B.m23 + m23*B.m33;
- m21 = t1;
- m22 = t2;
- t1 = m31*B.m11 + m32*B.m21 + m33*B.m31;
- t2 = m31*B.m12 + m32*B.m22 + m33*B.m32;
- m33 = m31*B.m13 + m32*B.m23 + m33*B.m33;
- m31 = t1;
- m32 = t2;
- }
- void Matrix3x4::OperatorTimesEquals (const Matrix3x3& B) // Composition
- {
- double t1, t2; // temporary values
- t1 = m11*B.m11 + m12*B.m21 + m13*B.m31;
- t2 = m11*B.m12 + m12*B.m22 + m13*B.m32;
- m13 = m11*B.m13 + m12*B.m23 + m13*B.m33;
- m11 = t1;
- m12 = t2;
- t1 = m21*B.m11 + m22*B.m21 + m23*B.m31;
- t2 = m21*B.m12 + m22*B.m22 + m23*B.m32;
- m23 = m21*B.m13 + m22*B.m23 + m23*B.m33;
- m21 = t1;
- m22 = t2;
- t1 = m31*B.m11 + m32*B.m21 + m33*B.m31;
- t2 = m31*B.m12 + m32*B.m22 + m33*B.m32;
- m33 = m31*B.m13 + m32*B.m23 + m33*B.m33;
- m31 = t1;
- m32 = t2;
- }
- // ******************************************************
- // * LinearMapR3 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * **
- LinearMapR3 operator* ( const LinearMapR3& A, const LinearMapR3& B)
- {
- return( LinearMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
- A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
- A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
- A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
- A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
- A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
- A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
- A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
- A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 ) );
- }
- double LinearMapR3::Determinant () const // Returns the determinant
- {
- return ( m11*(m22*m33-m23*m32)
- - m12*(m21*m33-m31*m23)
- + m13*(m21*m23-m31*m22) );
- }
- LinearMapR3 LinearMapR3::Inverse() const // Returns inverse
- {
- double sd11 = m22*m33-m23*m32;
- double sd21 = m32*m13-m12*m33;
- double sd31 = m12*m23-m22*m13;
- double sd12 = m31*m23-m21*m33;
- double sd22 = m11*m33-m31*m13;
- double sd32 = m21*m13-m11*m23;
- double sd13 = m21*m32-m31*m22;
- double sd23 = m31*m12-m11*m32;
- double sd33 = m11*m22-m21*m12;
- register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
- return( LinearMapR3( sd11*detInv, sd12*detInv, sd13*detInv,
- sd21*detInv, sd22*detInv, sd23*detInv,
- sd31*detInv, sd32*detInv, sd33*detInv ) );
- }
- LinearMapR3& LinearMapR3::Invert() // Converts into inverse.
- {
- double sd11 = m22*m33-m23*m32;
- double sd21 = m32*m13-m12*m33;
- double sd31 = m12*m23-m22*m13;
- double sd12 = m31*m23-m21*m33;
- double sd22 = m11*m33-m31*m13;
- double sd32 = m21*m13-m11*m23;
- double sd13 = m21*m32-m31*m22;
- double sd23 = m31*m12-m11*m32;
- double sd33 = m11*m22-m21*m12;
- register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
- m11 = sd11*detInv;
- m12 = sd21*detInv;
- m13 = sd31*detInv;
- m21 = sd12*detInv;
- m22 = sd22*detInv;
- m23 = sd32*detInv;
- m31 = sd13*detInv;
- m32 = sd23*detInv;
- m33 = sd33*detInv;
- return ( *this );
- }
- // ******************************************************
- // * AffineMapR3 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * **
- AffineMapR3 operator* ( const AffineMapR3& A, const AffineMapR3& B )
- {
- return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
- A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
- A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
- A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
- A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
- A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
- A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
- A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
- A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33,
- A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34 + A.m14,
- A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34 + A.m24,
- A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 + A.m34));
- }
- AffineMapR3 operator* ( const LinearMapR3& A, const AffineMapR3& B )
- {
- return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
- A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
- A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
- A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
- A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
- A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
- A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
- A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
- A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33,
- A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34,
- A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34,
- A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 ));
- }
-
- AffineMapR3 operator* ( const AffineMapR3& A, const LinearMapR3& B )
- {
- return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
- A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
- A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
- A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
- A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
- A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
- A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
- A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
- A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33,
- A.m14,
- A.m24,
- A.m34 ) );
- }
-
- AffineMapR3 AffineMapR3::Inverse() const // Returns inverse
- {
- double sd11 = m22*m33-m23*m32;
- double sd21 = m32*m13-m12*m33;
- double sd31 = m12*m23-m22*m13;
- double sd12 = m31*m23-m21*m33;
- double sd22 = m11*m33-m31*m13;
- double sd32 = m21*m13-m11*m23;
- double sd13 = m21*m32-m31*m22;
- double sd23 = m31*m12-m11*m32;
- double sd33 = m11*m22-m21*m12;
- register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
- // Make sd's hold the (transpose of) the inverse of the 3x3 part
- sd11 *= detInv;
- sd12 *= detInv;
- sd13 *= detInv;
- sd21 *= detInv;
- sd22 *= detInv;
- sd23 *= detInv;
- sd31 *= detInv;
- sd32 *= detInv;
- sd33 *= detInv;
- double sd41 = -(m14*sd11 + m24*sd21 + m34*sd31);
- double sd42 = -(m14*sd12 + m24*sd22 + m34*sd32);
- double sd43 = -(m14*sd12 + m24*sd23 + m34*sd33);
- return( AffineMapR3( sd11, sd12, sd13,
- sd21, sd22, sd23,
- sd31, sd32, sd33,
- sd41, sd42, sd43 ) );
- }
- AffineMapR3& AffineMapR3::Invert() // Converts into inverse.
- {
- double sd11 = m22*m33-m23*m32;
- double sd21 = m32*m13-m12*m33;
- double sd31 = m12*m23-m22*m13;
- double sd12 = m31*m23-m21*m33;
- double sd22 = m11*m33-m31*m13;
- double sd32 = m21*m13-m11*m23;
- double sd13 = m21*m32-m31*m22;
- double sd23 = m31*m12-m11*m32;
- double sd33 = m11*m22-m21*m12;
- register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13);
- m11 = sd11*detInv;
- m12 = sd21*detInv;
- m13 = sd31*detInv;
- m21 = sd12*detInv;
- m22 = sd22*detInv;
- m23 = sd32*detInv;
- m31 = sd13*detInv;
- m32 = sd23*detInv;
- m33 = sd33*detInv;
- double sd41 = -(m14*m11 + m24*m12 + m34*m13); // Compare to ::Inverse.
- double sd42 = -(m14*m21 + m24*m22 + m34*m23);
- double sd43 = -(m14*m31 + m24*m32 + m34*m33);
- m14 = sd41;
- m24 = sd42;
- m34 = sd43;
- return ( *this );
- }
- // **************************************************************
- // * RotationMapR3 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
- RotationMapR3 operator*(const RotationMapR3& A, const RotationMapR3& B)
- // Matrix product (composition)
- {
- return(RotationMapR3(A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31,
- A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31,
- A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31,
- A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32,
- A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32,
- A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32,
- A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33,
- A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33,
- A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 ) );
- }
- RotationMapR3& RotationMapR3::Set( const Quaternion& quat )
- {
- double wSq = quat.w*quat.w;
- double xSq = quat.x*quat.x;
- double ySq = quat.y*quat.y;
- double zSq = quat.z*quat.z;
- double Dqwx = 2.0*quat.w*quat.x;
- double Dqwy = 2.0*quat.w*quat.y;
- double Dqwz = 2.0*quat.w*quat.z;
- double Dqxy = 2.0*quat.x*quat.y;
- double Dqyz = 2.0*quat.y*quat.z;
- double Dqxz = 2.0*quat.x*quat.z;
- m11 = wSq+xSq-ySq-zSq;
- m22 = wSq-xSq+ySq-zSq;
- m33 = wSq-xSq-ySq+zSq;
- m12 = Dqxy-Dqwz;
- m21 = Dqxy+Dqwz;
- m13 = Dqxz+Dqwy;
- m31 = Dqxz-Dqwy;
- m23 = Dqyz-Dqwx;
- m32 = Dqyz+Dqwx;
- return *this;
- }
- RotationMapR3& RotationMapR3::Set( const VectorR3& u, double theta )
- {
- assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
- register double c = cos(theta);
- register double s = sin(theta);
- register double mc = 1.0-c;
- double xmc = u.x*mc;
- double xymc = xmc*u.y;
- double xzmc = xmc*u.z;
- double yzmc = u.y*u.z*mc;
- double xs = u.x*s;
- double ys = u.y*s;
- double zs = u.z*s;
- Matrix3x3::Set( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
- xymc-zs, u.y*u.y*mc+c, yzmc+xs,
- xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
- return *this;
- }
- // The rotation axis vector u MUST be a UNIT vector!!!
- RotationMapR3& RotationMapR3::Set( const VectorR3& u, double s, double c )
- {
- assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
- double mc = 1.0-c;
- double xmc = u.x*mc;
- double xymc = xmc*u.y;
- double xzmc = xmc*u.z;
- double yzmc = u.y*u.z*mc;
- double xs = u.x*s;
- double ys = u.y*s;
- double zs = u.z*s;
- Matrix3x3::Set( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
- xymc-zs, u.y*u.y*mc+c, yzmc+xs,
- xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
- return *this;
- }
- // ToAxisAndAngle - find a unit vector in the direction of the rotation axis,
- // and the angle theta of rotation. Returns true if the rotation angle is non-zero,
- // and false if it is zero.
- bool RotationMapR3::ToAxisAndAngle( VectorR3* u, double *theta ) const
- {
- double alpha = m11+m22+m33-1.0;
- double beta = sqrt(Square(m32-m23)+Square(m13-m31)+Square(m21-m12));
- if ( beta==0.0 ) {
- *u = VectorR3::UnitY;
- *theta = 0.0;
- return false;
- }
- else {
- u->Set(m32-m23, m13-m31, m21-m12);
- *u /= beta;
- *theta = atan2( beta, alpha );
- return true;
- }
- }
- // VrRotate is similar to glRotate. Returns a matrix (LinearMapR3)
- // that will perform the rotation when multiplied on the left.
- // u is supposed to be a *unit* vector. Otherwise, the LinearMapR3
- // returned will be garbage!
- RotationMapR3 VrRotate( double theta, const VectorR3& u )
- {
- RotationMapR3 ret;
- ret.Set( u, theta );
- return ret;
- }
- // This version of rotate takes the cosine and sine of theta
- // instead of theta. u must still be a unit vector.
- RotationMapR3 VrRotate( double c, double s, const VectorR3& u )
- {
- RotationMapR3 ret;
- ret.Set( u, s, c );
- return ret;
- }
- // Returns a RotationMapR3 which rotates the fromVec to be colinear
- // with toVec.
- RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec)
- {
- VectorR3 crossVec = fromVec;
- crossVec *= toVec;
- double sinetheta = crossVec.Norm(); // Not yet normalized!
- if ( sinetheta < 1.0e-40 ) {
- return ( RotationMapR3(
- 1.0, 0.0, 0.0,
- 0.0, 1.0, 0.0,
- 0.0, 0.0, 1.0) );
- }
- crossVec /= sinetheta; // Normalize the vector
- double scale = 1.0/sqrt(fromVec.NormSq()*toVec.NormSq());
- sinetheta *= scale;
- double cosinetheta = (fromVec^toVec)*scale;
- return ( VrRotate( cosinetheta, sinetheta, crossVec) );
- }
- // RotateToMap returns a rotation map which rotates fromVec to have the
- // same direction as toVec.
- // fromVec and toVec should be unit vectors
- RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec)
- {
- VectorR3 crossVec = fromVec;
- crossVec *= toVec;
- double sintheta = crossVec.Norm();
- double costheta = fromVec^toVec;
- if ( sintheta <= 1.0e-40 ) {
- if ( costheta>0.0 ) {
- return ( RotationMapR3(
- 1.0, 0.0, 0.0,
- 0.0, 1.0, 0.0,
- 0.0, 0.0, 1.0) );
- }
- else {
- GetOrtho ( toVec, crossVec ); // Get arbitrary orthonormal vector
- return( VrRotate(costheta, sintheta, crossVec ) );
- }
- }
- else {
- crossVec /= sintheta; // Normalize the vector
- return ( VrRotate( costheta, sintheta, crossVec) );
- }
- }
- // **************************************************************
- // * RigidMapR3 class - math library functions *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
- // The rotation axis vector u MUST be a UNIT vector!!!
- RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double theta )
- {
- assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
- register double c = cos(theta);
- register double s = sin(theta);
- register double mc = 1.0-c;
- double xmc = u.x*mc;
- double xymc = xmc*u.y;
- double xzmc = xmc*u.z;
- double yzmc = u.y*u.z*mc;
- double xs = u.x*s;
- double ys = u.y*s;
- double zs = u.z*s;
- Matrix3x4::Set3x3( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
- xymc-zs, u.y*u.y*mc+c, yzmc+xs,
- xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
- return *this;
- }
- // The rotation axis vector u MUST be a UNIT vector!!!
- RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double s, double c )
- {
- assert ( fabs(u.NormSq()-1.0)<2.0e-6 );
- double mc = 1.0-c;
- double xmc = u.x*mc;
- double xymc = xmc*u.y;
- double xzmc = xmc*u.z;
- double yzmc = u.y*u.z*mc;
- double xs = u.x*s;
- double ys = u.y*s;
- double zs = u.z*s;
- Matrix3x4::Set3x3( u.x*u.x*mc+c, xymc+zs, xzmc-ys,
- xymc-zs, u.y*u.y*mc+c, yzmc+xs,
- xzmc+ys, yzmc-xs, u.z*u.z*mc+c );
- return *this;
- }
- // CalcGlideRotation - converts a rigid map into an equivalent
- // glide rotation (screw motion). It returns the rotation axis
- // as base point u, and a rotation axis v. The vectors u and v are
- // always orthonormal. v will be a unit vector.
- // It also returns the glide distance, which is the translation parallel
- // to v. Further, it returns the signed rotation angle theta (right hand rule
- // specifies the direction.
- // The glide rotation means a rotation around the point u with axis direction v.
- // Return code "true" if the rotation amount is non-zero. "false" if pure translation.
- bool RigidMapR3::CalcGlideRotation( VectorR3* u, VectorR3* v,
- double *glideDist, double *rotation ) const
- {
- // Compare to the code for ToAxisAndAngle.
- double alpha = m11+m22+m33-1.0;
- double beta = sqrt(Square(m32-m23)+Square(m13-m31)+Square(m21-m12));
- if ( beta==0.0 ) {
- double vN = m14*m14 + m24*m24 + m34*m34;
- if ( vN>0.0 ) {
- vN = sqrt(vN);
- v->Set( m14, m24, m34 );
- *v /= vN;
- *glideDist = vN;
- }
- else {
- *v = VectorR3::UnitX;
- *glideDist = 0.0;
- }
- u->SetZero();
- *rotation = 0;
- return false;
- }
- else {
- v->Set(m32-m23, m13-m31, m21-m12);
- *v /= beta; // v - unit vector, rotation axis
- *rotation = atan2( beta, alpha );
- u->Set( m14, m24, m34 );
- *glideDist = ((*u)^(*v));
- VectorR3 temp = *v;
- temp *= *glideDist;
- *u -= temp; // Subtract component in direction of rot. axis v
- temp = *v;
- temp *= *u;
- temp /= tan((*rotation)/2); // temp = (v \times u) / tan(rotation/2)
- *u += temp;
- *u *= 0.5;
- return true;
- }
- }
- // ***************************************************************
- // Linear Algebra Utilities *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
- // Returns a righthanded orthonormal basis to complement vector u
- void GetOrtho( const VectorR3& u, VectorR3& v, VectorR3& w)
- {
- if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
- v.Set ( u.y, -u.x, 0.0 );
- }
- else {
- v.Set ( 0.0, u.z, -u.y);
- }
- v.Normalize();
- w = u;
- w *= v;
- w.Normalize();
- // w.NormalizeFast();
- return;
- }
- // Returns a vector v orthonormal to unit vector u
- void GetOrtho( const VectorR3& u, VectorR3& v )
- {
- if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
- v.Set ( u.y, -u.x, 0.0 );
- }
- else {
- v.Set ( 0.0, u.z, -u.y);
- }
- v.Normalize();
- return;
- }
- // ***************************************************************
- // Stream Output Routines *
- // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
- ostream& operator<< ( ostream& os, const VectorR3& u )
- {
- return (os << "<" << u.x << "," << u.y << "," << u.z << ">");
- }
- ostream& operator<< ( ostream& os, const Matrix3x3& A )
- {
- os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13 << ">\n"
- << " <" << A.m21 << ", " << A.m22 << ", " << A.m23 << ">\n"
- << " <" << A.m31 << ", " << A.m32 << ", " << A.m33 << ">\n" ;
- return (os);
- }
- ostream& operator<< ( ostream& os, const Matrix3x4& A )
- {
- os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13
- << "; " << A.m14 << ">\n"
- << " <" << A.m21 << ", " << A.m22 << ", " << A.m23
- << "; " << A.m24 << ">\n"
- << " <" << A.m31 << ", " << A.m32 << ", " << A.m33
- << "; " << A.m34 << ">\n" ;
- return (os);
- }
|