0; j-- ) {
+ double tmp;
+ alpha += (*ux)*(*(dSx++));
+ tmp = Square( *(ux++) );
+ alpha += (*ux)*(*(dSx++));
+ tmp += Square(*(ux++));
+ alpha += (*ux)*(*(dSx++));
+ tmp += Square(*(ux++));
+ N += sqrt(tmp);
+ }
+
+ // P is the quasi-1-norm of the response to angles changing according to the i-th column of V
+ double P = 0.0;
+ double *vx = V.GetColumnPtr(i);
+ jnx = Jnorms.GetPtr();
+ for ( j=nCols; j>0; j-- ) {
+ double accum=0.0;
+ for ( long k=numEndEffectors; k>0; k-- ) {
+ accum += *(jnx++);
+ }
+ P += fabs((*(vx++)))*accum;
+ }
+
+ double lambda = 1.0;
+ if ( NMaxAngleSDLS ) {
+ dTheta *= MaxAngleSDLS/maxChange;
+ }
+} */
+
+
+
+
diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h
new file mode 100644
index 000000000..b1c683f8a
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h
@@ -0,0 +1,131 @@
+
+/*
+*
+* Inverse Kinematics software, with several solvers including
+* Selectively Damped Least Squares Method
+* Damped Least Squares Method
+* Pure Pseudoinverse Method
+* Jacobian Transpose Method
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
+*
+*
+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 "Node.h"
+#include "Tree.h"
+#include "MathMisc.h"
+#include "LinearR3.h"
+#include "VectorRn.h"
+#include "MatrixRmn.h"
+
+#ifndef _CLASS_JACOBIAN
+#define _CLASS_JACOBIAN
+
+#ifdef _DYNAMIC
+const double BASEMAXDIST = 0.02;
+#else
+const double MAXDIST = 0.08; // optimal value for double Y shape : 0.08
+#endif
+const double DELTA = 0.4;
+const long double LAMBDA = 2.0; // only for DLS. optimal : 0.24
+const double NEARZERO = 0.0000000001;
+
+enum UpdateMode {
+ JACOB_Undefined = 0,
+ JACOB_JacobianTranspose = 1,
+ JACOB_PseudoInverse = 2,
+ JACOB_DLS = 3,
+ JACOB_SDLS = 4 };
+
+class Jacobian {
+public:
+ Jacobian(Tree*);
+
+ void ComputeJacobian();
+ const MatrixRmn& ActiveJacobian() const { return *Jactive; }
+ void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
+ void SetJtargetActive() { Jactive = &Jtarget; }
+
+ void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
+ void ZeroDeltaThetas();
+ void CalcDeltaThetasTranspose();
+ void CalcDeltaThetasPseudoinverse();
+ void CalcDeltaThetasDLS();
+ void CalcDeltaThetasDLSwithSVD();
+ void CalcDeltaThetasSDLS();
+
+ void UpdateThetas();
+ double UpdateErrorArray(); // Returns sum of errors
+ const VectorRn& GetErrorArray() const { return errorArray; }
+ void UpdatedSClampValue();
+
+ void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; }
+ UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }
+ void SetDampingDLS( double lambda ) { DampingLambda = lambda; DampingLambdaSq = Square(lambda); }
+
+ void Reset();
+
+ static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 );
+ static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies );
+
+private:
+ Tree* tree; // tree associated with this Jacobian matrix
+ int nEffector; // Number of end effectors
+ int nJoint; // Number of joints
+ int nRow; // Total number of rows the real J (= 3*number of end effectors for now)
+ int nCol; // Total number of columns in the real J (= number of joints for now)
+
+ MatrixRmn Jend; // Jacobian matrix based on end effector positions
+ MatrixRmn Jtarget; // Jacobian matrix based on target positions
+ MatrixRmn Jnorms; // Norms of 3-vectors in active Jacobian (SDLS only)
+
+ MatrixRmn U; // J = U * Diag(w) * V^T (Singular Value Decomposition)
+ VectorRn w;
+ MatrixRmn V;
+
+ UpdateMode CurrentUpdateMode;
+
+ VectorRn dS; // delta s
+ VectorRn dT1; // delta t -- these are delta S values clamped to smaller magnitude
+ VectorRn dSclamp; // Value to clamp magnitude of dT at.
+ VectorRn dTheta; // delta theta
+ VectorRn dPreTheta; // delta theta for single eigenvalue (SDLS only)
+
+ VectorRn errorArray; // Distance of end effectors from target after updating
+
+ // Parameters for pseudoinverses
+ static const double PseudoInverseThresholdFactor; // Threshold for treating eigenvalue as zero (fraction of largest eigenvalue)
+
+ // Parameters for damped least squares
+ static const double DefaultDampingLambda;
+ double DampingLambda;
+ double DampingLambdaSq;
+ //double DampingLambdaSDLS;
+
+ // Cap on max. value of changes in angles in single update step
+ static const double MaxAngleJtranspose;
+ static const double MaxAnglePseudoinverse;
+ static const double MaxAngleDLS;
+ static const double MaxAngleSDLS;
+ MatrixRmn* Jactive;
+
+ void CalcdTClampedFromdS();
+ static const double BaseMaxTargetDist;
+
+};
+
+#endif
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/LinearR2.cpp b/examples/ThirdPartyLibs/BussIK/LinearR2.cpp
new file mode 100644
index 000000000..9e3398f65
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR2.cpp
@@ -0,0 +1,101 @@
+ /*
+ *
+ * Mathematics Subpackage (VrMath)
+ *
+ *
+ * Author: Samuel R. Buss, sbuss@ucsd.edu.
+ * 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 "LinearR2.h"
+
+
+#include
+
+// ******************************************************
+// * VectorR2 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+const VectorR2 VectorR2::Zero(0.0, 0.0);
+const VectorR2 VectorR2::UnitX( 1.0, 0.0);
+const VectorR2 VectorR2::UnitY( 0.0, 1.0);
+const VectorR2 VectorR2::NegUnitX(-1.0, 0.0);
+const VectorR2 VectorR2::NegUnitY( 0.0,-1.0);
+
+const Matrix2x2 Matrix2x2::Identity(1.0, 0.0, 0.0, 1.0);
+
+// ******************************************************
+// * Matrix2x2 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+
+// ******************************************************
+// * LinearMapR2 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+
+LinearMapR2 LinearMapR2::Inverse() const // Returns inverse
+{
+
+
+ register double detInv = 1.0/(m11*m22 - m12*m21) ;
+
+ return( LinearMapR2( m22*detInv, -m21*detInv, -m12*detInv, m11*detInv ) );
+}
+
+LinearMapR2& LinearMapR2::Invert() // Converts into inverse.
+{
+ register double detInv = 1.0/(m11*m22 - m12*m21) ;
+
+ double temp;
+ temp = m11*detInv;
+ m11= m22*detInv;
+ m22=temp;
+ m12 = -m12*detInv;
+ m21 = -m22*detInv;
+
+ return ( *this );
+}
+
+VectorR2 LinearMapR2::Solve(const VectorR2& u) const // Returns solution
+{
+ // Just uses Inverse() for now.
+ return ( Inverse()*u );
+}
+
+// ******************************************************
+// * RotationMapR2 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+
+
+// ***************************************************************
+// * 2-space vector and matrix utilities *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+
+
+
+// ***************************************************************
+// Stream Output Routines *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+ostream& operator<< ( ostream& os, const VectorR2& u )
+{
+ return (os << "<" << u.x << "," << u.y << ">");
+}
+
+
diff --git a/examples/ThirdPartyLibs/BussIK/LinearR2.h b/examples/ThirdPartyLibs/BussIK/LinearR2.h
new file mode 100644
index 000000000..4c4140c71
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR2.h
@@ -0,0 +1,981 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+//
+// Linear Algebra Classes over R2
+//
+//
+// A. Vector and Position classes
+//
+// A.1. VectorR2: a column vector of length 2
+//
+// A.2. VectorHgR2 - homogenous vector for R2 (a 3-Vector)
+//
+// B. Matrix Classes
+//
+// B.1 LinearMapR2 - arbitrary linear map; 2x2 real matrix
+//
+// B.2 RotationMapR2 - orthonormal 2x2 matrix
+//
+
+#ifndef LINEAR_R2_H
+#define LINEAR_R2_H
+
+#include
+#include
+#include
+#include "MathMisc.h"
+using namespace std;
+
+class VectorR2; // R2 Vector
+class VectorHgR2;
+class Matrix2x2;
+class LinearMapR2; // 2x2 real matrix
+class AffineMapR3; // Affine Map (3x4 Matrix)
+class RotationMapR2; // 2x2 rotation map
+
+// **************************************
+// VectorR2 class *
+// * * * * * * * * * * * * * * * * * * **
+
+class VectorR2 {
+
+public:
+ double x, y; // The x & y coordinates.
+
+public:
+ VectorR2( ) : x(0.0), y(0.0) {}
+ VectorR2( double xVal, double yVal )
+ : x(xVal), y(yVal) {}
+ VectorR2( const VectorHgR2& uH );
+
+ VectorR2& SetZero() { x=0.0; y=0.0; return *this;}
+ VectorR2& Set( double xx, double yy )
+ { x=xx; y=yy; return *this;}
+ VectorR2& Load( const double* v );
+ VectorR2& Load( const float* v );
+ void Dump( double* v ) const;
+ void Dump( float* v ) const;
+
+ static const VectorR2 Zero;
+ static const VectorR2 UnitX;
+ static const VectorR2 UnitY;
+ static const VectorR2 NegUnitX;
+ static const VectorR2 NegUnitY;
+
+ VectorR2& operator+= ( const VectorR2& v )
+ { x+=v.x; y+=v.y; return(*this); }
+ VectorR2& operator-= ( const VectorR2& v )
+ { x-=v.x; y-=v.y; return(*this); }
+ VectorR2& operator*= ( double m )
+ { x*=m; y*=m; return(*this); }
+ VectorR2& operator/= ( double m )
+ { register double mInv = 1.0/m;
+ x*=mInv; y*=mInv;
+ return(*this); }
+ VectorR2 operator- () const { return ( VectorR2(-x, -y) ); }
+ VectorR2& ArrayProd(const VectorR2&); // Component-wise product
+
+ VectorR2& AddScaled( const VectorR2& u, double s );
+
+ double Norm() const { return ( sqrt( x*x + y*y ) ); }
+ double L1Norm() const { return (Max(fabs(x),fabs(y))); }
+ double Dist( const VectorR2& u ) const; // Distance from u
+ double DistSq( const VectorR2& u ) const; // Distance from u
+ double NormSq() const { return ( x*x + y*y ); }
+ double MaxAbs() const;
+ VectorR2& Normalize () { *this /= Norm(); return *this;} // No error checking
+ VectorR2& MakeUnit(); // Normalize() with error checking
+ VectorR2& ReNormalize();
+ bool IsUnit( double tolerance = 1.0e-15 ) const
+ { register double norm = Norm();
+ return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); }
+ bool IsZero() const { return ( x==0.0 && y==0.0 ); }
+ bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );}
+ // tolerance should be non-negative
+
+ VectorR2& Rotate( double theta ); // rotate through angle theta
+ VectorR2& Rotate( double costheta, double sintheta );
+
+};
+
+inline VectorR2 operator+( const VectorR2& u, const VectorR2& v );
+inline VectorR2 operator-( const VectorR2& u, const VectorR2& v );
+inline VectorR2 operator*( const VectorR2& u, double m);
+inline VectorR2 operator*( double m, const VectorR2& u);
+inline VectorR2 operator/( const VectorR2& u, double m);
+inline int operator==( const VectorR2& u, const VectorR2& v );
+
+inline double operator^ (const VectorR2& u, const VectorR2& v ); // Dot Product
+inline VectorR2 ArrayProd ( const VectorR2& u, const VectorR2& v );
+
+inline double Mag(const VectorR2& u) { return u.Norm(); }
+inline double Dist(const VectorR2& u, const VectorR2& v) { return u.Dist(v); }
+inline double DistSq(const VectorR2& u, const VectorR2& v) { return u.DistSq(v); }
+inline double NormalizeError (const VectorR2&);
+
+// ****************************************
+// VectorHgR2 class *
+// * * * * * * * * * * * * * * * * * * * **
+
+class VectorHgR2 {
+
+public:
+ double x, y, w; // The x & y & w coordinates.
+
+public:
+ VectorHgR2( ) : x(0.0), y(0.0), w(1.0) {}
+ VectorHgR2( double xVal, double yVal )
+ : x(xVal), y(yVal), w(1.0) {}
+ VectorHgR2( double xVal, double yVal, double wVal )
+ : x(xVal), y(yVal), w(wVal) {}
+ VectorHgR2 ( const VectorR2& u ) : x(u.x), y(u.y), w(1.0) {}
+};
+
+// ********************************************************************
+// Matrix2x2 - base class for 2x2 matrices *
+// * * * * * * * * * * * * * * * * * * * * * **************************
+
+class Matrix2x2 {
+
+public:
+ double m11, m12, m21, m22;
+
+ // Implements a 2x2 matrix: m_i_j - row-i and column-j entry
+
+ static const Matrix2x2 Identity;
+
+public:
+
+ inline Matrix2x2();
+ inline Matrix2x2( const VectorR2&, const VectorR2& ); // Sets by columns!
+ inline Matrix2x2( double, double, double, double ); // Sets by columns
+
+ inline void SetIdentity (); // Set to the identity map
+ inline void SetZero (); // Set to the zero map
+ inline void Set( const VectorR2&, const VectorR2& );
+ inline void Set( double, double, double, double );
+ inline void SetByRows( const VectorR2&, const VectorR2& );
+ inline void SetByRows( double, double, double, double );
+ inline void SetColumn1 ( double, double );
+ inline void SetColumn2 ( double, double );
+ inline void SetColumn1 ( const VectorR2& );
+ inline void SetColumn2 ( const VectorR2& );
+ inline VectorR2 Column1() const;
+ inline VectorR2 Column2() const;
+
+ inline void SetRow1 ( double, double );
+ inline void SetRow2 ( double, double );
+ inline void SetRow1 ( const VectorR2& );
+ inline void SetRow2 ( const VectorR2& );
+ inline VectorR2 Row1() const;
+ inline VectorR2 Row2() const;
+
+ inline void SetDiagonal( double, double );
+ inline void SetDiagonal( const VectorR2& );
+ inline double Diagonal( int );
+
+ inline void MakeTranspose(); // Transposes it.
+ inline void operator*= (const Matrix2x2& B); // Matrix product
+ inline Matrix2x2& ReNormalize();
+
+ inline void Transform( VectorR2* ) const;
+ inline void Transform( const VectorR2& src, VectorR2* dest) const;
+
+};
+
+inline double NormalizeError( const Matrix2x2& );
+inline VectorR2 operator* ( const Matrix2x2&, const VectorR2& );
+
+ostream& operator<< ( ostream& os, const Matrix2x2& A );
+
+
+// *****************************************
+// LinearMapR2 class *
+// * * * * * * * * * * * * * * * * * * * * *
+
+class LinearMapR2 : public Matrix2x2 {
+
+public:
+
+ LinearMapR2();
+ LinearMapR2( const VectorR2&, const VectorR2& ); // Sets by columns!
+ LinearMapR2( double, double, double, double ); // Sets by columns
+ LinearMapR2 ( const Matrix2x2& );
+
+ inline void Negate();
+ inline LinearMapR2& operator+= (const Matrix2x2& );
+ inline LinearMapR2& operator-= (const Matrix2x2& );
+ inline LinearMapR2& operator*= (double);
+ inline LinearMapR2& operator/= (double);
+ inline LinearMapR2& operator*= (const Matrix2x2& ); // Matrix product
+
+ inline LinearMapR2 Transpose() const;
+ inline double Determinant () const; // Returns the determinant
+ LinearMapR2 Inverse() const; // Returns inverse
+ LinearMapR2& Invert(); // Converts into inverse.
+ VectorR2 Solve(const VectorR2&) const; // Returns solution
+ LinearMapR2 PseudoInverse() const; // Returns pseudo-inverse TO DO
+ VectorR2 PseudoSolve(const VectorR2&); // Finds least squares solution TO DO
+};
+
+inline LinearMapR2 operator+ ( const LinearMapR2&, const LinearMapR2&);
+inline LinearMapR2 operator- ( const Matrix2x2& );
+inline LinearMapR2 operator- ( const LinearMapR2&, const LinearMapR2&);
+inline LinearMapR2 operator* ( const LinearMapR2&, double);
+inline LinearMapR2 operator* ( double, const LinearMapR2& );
+inline LinearMapR2 operator/ ( const LinearMapR2&, double );
+inline LinearMapR2 operator* ( const Matrix2x2&, const LinearMapR2& );
+inline LinearMapR2 operator* ( const LinearMapR2&, const Matrix2x2& );
+inline LinearMapR2 operator* ( const LinearMapR2&, const LinearMapR2& );
+ // Matrix product (composition)
+
+
+// *******************************************
+// RotationMapR2class *
+// * * * * * * * * * * * * * * * * * * * * * *
+
+class RotationMapR2 : public Matrix2x2 {
+
+public:
+
+ RotationMapR2();
+ RotationMapR2( const VectorR2&, const VectorR2& ); // Sets by columns!
+ RotationMapR2( double, double, double, double ); // Sets by columns!
+
+ RotationMapR2& SetZero(); // IT IS AN ERROR TO USE THIS FUNCTION!
+
+ inline RotationMapR2& operator*= (const RotationMapR2& ); // Matrix product
+
+ inline RotationMapR2 Transpose() const;
+ inline RotationMapR2 Inverse() const { return Transpose(); }; // Returns the transpose
+ inline RotationMapR2& Invert() { MakeTranspose(); return *this; }; // Transposes it.
+ inline VectorR2 Invert(const VectorR2&) const; // Returns solution
+};
+
+inline RotationMapR2 operator* ( const RotationMapR2&, const RotationMapR2& );
+ // Matrix product (composition)
+
+
+
+// ***************************************************************
+// * 2-space vector and matrix utilities (prototypes) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+// Returns the angle between vectors u and v.
+// Use AngleUnit if both vectors are unit vectors
+inline double Angle( const VectorR2& u, const VectorR2& v);
+inline double AngleUnit( const VectorR2& u, const VectorR2& v );
+
+// Returns a righthanded orthonormal basis to complement vector u
+// The vector u must be unit.
+inline VectorR2 GetOrtho( const VectorR2& u );
+
+// Projections
+
+inline VectorR2 ProjectToUnit ( const VectorR2& u, const VectorR2& v);
+ // Project u onto v
+inline VectorR2 ProjectPerpUnit ( const VectorR2& u, const VectorR2 & v);
+ // Project perp to v
+// v must be a unit vector.
+
+// Projection maps (LinearMapR2's)
+
+inline LinearMapR2 VectorProjectMap( const VectorR2& u );
+
+inline LinearMapR2 PerpProjectMap ( const VectorR2& u );
+// u - must be unit vector.
+
+// Rotation Maps
+
+inline RotationMapR2 RotateToMap( const VectorR2& fromVec, const VectorR2& toVec);
+// fromVec and toVec should be unit vectors
+
+
+
+// ***************************************************************
+// * Stream Output Routines (Prototypes) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+ostream& operator<< ( ostream& os, const VectorR2& u );
+
+
+// *****************************************************
+// * VectorR2 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+inline VectorR2& VectorR2::Load( const double* v )
+{
+ x = *v;
+ y = *(v+1);
+ return *this;
+}
+
+inline VectorR2& VectorR2::Load( const float* v )
+{
+ x = *v;
+ y = *(v+1);
+ return *this;
+}
+
+inline void VectorR2::Dump( double* v ) const
+{
+ *v = x;
+ *(v+1) = y;
+}
+
+inline void VectorR2::Dump( float* v ) const
+{
+ *v = (float)x;
+ *(v+1) = (float)y;
+}
+
+inline VectorR2& VectorR2::ArrayProd (const VectorR2& v) // Component-wise Product
+{
+ x *= v.x;
+ y *= v.y;
+ return ( *this );
+}
+
+inline VectorR2& VectorR2::MakeUnit () // Convert to unit vector (or leave zero).
+{
+ double nSq = NormSq();
+ if (nSq != 0.0) {
+ *this /= sqrt(nSq);
+ }
+ return *this;
+}
+
+inline VectorR2& VectorR2::ReNormalize() // Convert near unit back to unit
+{
+ double nSq = NormSq();
+ register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor
+ *this *= mFact;
+ return *this;
+}
+
+// Rotate through angle theta
+inline VectorR2& VectorR2::Rotate( double theta )
+{
+ double costheta = cos(theta);
+ double sintheta = sin(theta);
+ double tempx = x*costheta - y*sintheta;
+ y = y*costheta + x*sintheta;
+ x = tempx;
+ return *this;
+}
+
+inline VectorR2& VectorR2::Rotate( double costheta, double sintheta )
+{
+ double tempx = x*costheta + y*sintheta;
+ y = y*costheta - x*sintheta;
+ x = tempx;
+ return *this;
+}
+
+inline double VectorR2::MaxAbs() const
+{
+ register double m;
+ m = (x>=0.0) ? x : -x;
+ if ( y>m ) m=y;
+ else if ( -y >m ) m = -y;
+ return m;
+}
+
+inline VectorR2 operator+( const VectorR2& u, const VectorR2& v )
+{
+ return VectorR2(u.x+v.x, u.y+v.y );
+}
+inline VectorR2 operator-( const VectorR2& u, const VectorR2& v )
+{
+ return VectorR2(u.x-v.x, u.y-v.y );
+}
+inline VectorR2 operator*( const VectorR2& u, register double m)
+{
+ return VectorR2( u.x*m, u.y*m );
+}
+inline VectorR2 operator*( register double m, const VectorR2& u)
+{
+ return VectorR2( u.x*m, u.y*m );
+}
+inline VectorR2 operator/( const VectorR2& u, double m)
+{
+ register double mInv = 1.0/m;
+ return VectorR2( u.x*mInv, u.y*mInv );
+}
+
+inline int operator==( const VectorR2& u, const VectorR2& v )
+{
+ return ( u.x==v.x && u.y==v.y );
+}
+
+inline double operator^ ( const VectorR2& u, const VectorR2& v ) // Dot Product
+{
+ return ( u.x*v.x + u.y*v.y );
+}
+
+inline VectorR2 ArrayProd ( const VectorR2& u, const VectorR2& v )
+{
+ return ( VectorR2( u.x*v.x, u.y*v.y ) );
+}
+
+inline VectorR2& VectorR2::AddScaled( const VectorR2& u, double s )
+{
+ x += s*u.x;
+ y += s*u.y;
+ return(*this);
+}
+
+inline VectorR2::VectorR2( const VectorHgR2& uH )
+: x(uH.x), y(uH.y)
+{
+ *this /= uH.w;
+}
+
+inline double NormalizeError (const VectorR2& u)
+{
+ register double discrepancy;
+ discrepancy = u.x*u.x + u.y*u.y - 1.0;
+ if ( discrepancy < 0.0 ) {
+ discrepancy = -discrepancy;
+ }
+ return discrepancy;
+}
+
+inline double VectorR2::Dist( const VectorR2& u ) const // Distance from u
+{
+ return sqrt( DistSq(u) );
+}
+
+inline double VectorR2::DistSq( const VectorR2& u ) const // Distance from u
+{
+ return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) );
+}
+
+
+// *********************************************************
+// * Matrix2x2 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *****
+
+inline Matrix2x2::Matrix2x2() {}
+
+inline Matrix2x2::Matrix2x2( const VectorR2& u, const VectorR2& v )
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+}
+
+inline Matrix2x2::Matrix2x2( double a11, double a21, double a12, double a22 )
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m21 = a21; // Row 2
+ m22 = a22;
+}
+
+inline void Matrix2x2::SetIdentity ( )
+{
+ m11 = m22 = 1.0;
+ m12 = m21 = 0.0;
+}
+
+inline void Matrix2x2::Set( const VectorR2& u, const VectorR2& v )
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+}
+
+inline void Matrix2x2::Set( double a11, double a21, double a12, double a22 )
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m21 = a21; // Row 2
+ m22 = a22;
+}
+
+inline void Matrix2x2::SetZero( )
+{
+ m11 = m12 = m21 = m22 = 0.0;
+}
+
+inline void Matrix2x2::SetByRows( const VectorR2& u, const VectorR2& v )
+{
+ m11 = u.x; // Row 1
+ m12 = u.y;
+ m21 = v.x; // Row 2
+ m22 = v.y;
+}
+
+inline void Matrix2x2::SetByRows( double a11, double a12, double a21, double a22 )
+ // Values specified in row order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m21 = a21; // Row 2
+ m22 = a22;
+}
+
+inline void Matrix2x2::SetColumn1 ( double x, double y )
+{
+ m11 = x; m21 = y;
+}
+
+inline void Matrix2x2::SetColumn2 ( double x, double y )
+{
+ m12 = x; m22 = y;
+}
+
+inline void Matrix2x2::SetColumn1 ( const VectorR2& u )
+{
+ m11 = u.x; m21 = u.y;
+}
+
+inline void Matrix2x2::SetColumn2 ( const VectorR2& u )
+{
+ m12 = u.x; m22 = u.y;
+}
+
+VectorR2 Matrix2x2::Column1() const
+{
+ return ( VectorR2(m11, m21) );
+}
+
+VectorR2 Matrix2x2::Column2() const
+{
+ return ( VectorR2(m12, m22) );
+}
+
+inline void Matrix2x2::SetRow1 ( double x, double y )
+{
+ m11 = x; m12 = y;
+}
+
+inline void Matrix2x2::SetRow2 ( double x, double y )
+{
+ m21 = x; m22 = y;
+}
+
+inline void Matrix2x2::SetRow1 ( const VectorR2& u )
+{
+ m11 = u.x; m12 = u.y;
+}
+
+inline void Matrix2x2::SetRow2 ( const VectorR2& u )
+{
+ m21 = u.x; m22 = u.y;
+}
+
+VectorR2 Matrix2x2::Row1() const
+{
+ return ( VectorR2(m11, m12) );
+}
+
+VectorR2 Matrix2x2::Row2() const
+{
+ return ( VectorR2(m21, m22) );
+}
+
+inline void Matrix2x2::SetDiagonal( double x, double y )
+{
+ m11 = x;
+ m22 = y;
+}
+
+inline void Matrix2x2::SetDiagonal( const VectorR2& u )
+{
+ SetDiagonal ( u.x, u.y );
+}
+
+inline double Matrix2x2::Diagonal( int i )
+{
+ switch (i) {
+ case 0:
+ return m11;
+ case 1:
+ return m22;
+ default:
+ assert(0);
+ return 0.0;
+ }
+}
+inline void Matrix2x2::MakeTranspose() // Transposes it.
+{
+ register double temp;
+ temp = m12;
+ m12 = m21;
+ m21=temp;
+}
+
+inline void Matrix2x2::operator*= (const Matrix2x2& B) // Matrix product
+{
+ double t1; // temporary value
+
+ t1 = m11*B.m11 + m12*B.m21;
+ m12 = m11*B.m12 + m12*B.m22;
+ m11 = t1;
+
+ t1 = m21*B.m11 + m22*B.m21;
+ m22 = m21*B.m12 + m22*B.m22;
+ m21 = t1;
+}
+
+inline Matrix2x2& Matrix2x2::ReNormalize() // Re-normalizes nearly orthonormal matrix
+{
+ register double alpha = m11*m11+m21*m21; // First column's norm squared
+ register double beta = m12*m12+m22*m22; // Second column's norm squared
+ alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor
+ beta = 1.0 - 0.5*(beta-1.0);
+ m11 *= alpha; // Renormalize first column
+ m21 *= alpha;
+ m12 *= beta; // Renormalize second column
+ m22 *= beta;
+ alpha = m11*m12+m21*m22; // Columns' inner product
+ alpha *= 0.5; // times 1/2
+ register double temp;
+ temp = m11-alpha*m12; // Subtract alpha times other column
+ m12 -= alpha*m11;
+ m11 = temp;
+ temp = m21-alpha*m22;
+ m22 -= alpha*m21;
+ m11 = temp;
+ return *this;
+}
+
+// Gives a measure of how far the matrix is from being normalized.
+// Mostly intended for diagnostic purposes.
+inline double NormalizeError( const Matrix2x2& A)
+{
+ register double discrepancy;
+ register double newdisc;
+ discrepancy = A.m11*A.m11 + A.m21*A.m21 -1.0; // First column - inner product - 1
+ if (discrepancy<0.0) {
+ discrepancy = -discrepancy;
+ }
+ newdisc = A.m12*A.m12 + A.m22*A.m22 - 1.0; // Second column inner product - 1
+ if ( newdisc<0.0 ) {
+ newdisc = -newdisc;
+ }
+ if ( newdisc>discrepancy ) {
+ discrepancy = newdisc;
+ }
+ newdisc = A.m11*A.m12 + A.m21*A.m22; // Inner product of two columns
+ if ( newdisc<0.0 ) {
+ newdisc = -newdisc;
+ }
+ if ( newdisc>discrepancy ) {
+ discrepancy = newdisc;
+ }
+ return discrepancy;
+}
+
+inline VectorR2 operator* ( const Matrix2x2& A, const VectorR2& u)
+{
+ return(VectorR2 ( A.m11*u.x + A.m12*u.y,
+ A.m21*u.x + A.m22*u.y ) );
+}
+
+inline void Matrix2x2::Transform( VectorR2* u ) const {
+ double newX;
+ newX = m11*u->x + m12*u->y;
+ u->y = m21*u->x + m22*u->y;
+ u->x = newX;
+}
+
+inline void Matrix2x2::Transform( const VectorR2& src, VectorR2* dest ) const {
+ dest->x = m11*src.x + m12*src.y;
+ dest->y = m21*src.x + m22*src.y;
+}
+
+
+
+// ******************************************************
+// * LinearMapR2 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline LinearMapR2::LinearMapR2()
+{
+ SetZero();
+ return;
+}
+
+inline LinearMapR2::LinearMapR2( const VectorR2& u, const VectorR2& v )
+:Matrix2x2 ( u, v )
+{ }
+
+inline LinearMapR2::LinearMapR2(double a11, double a21, double a12, double a22)
+ // Values specified in column order!!!
+:Matrix2x2 ( a11, a21, a12, a22 )
+{ }
+
+inline LinearMapR2::LinearMapR2 ( const Matrix2x2& A )
+: Matrix2x2 (A)
+{}
+
+inline void LinearMapR2::Negate ()
+{
+ m11 = -m11;
+ m12 = -m12;
+ m21 = -m21;
+ m22 = -m22;
+}
+
+inline LinearMapR2& LinearMapR2::operator+= (const Matrix2x2& B)
+{
+ m11 += B.m11;
+ m12 += B.m12;
+ m21 += B.m21;
+ m22 += B.m22;
+ return ( *this );
+}
+
+inline LinearMapR2& LinearMapR2::operator-= (const Matrix2x2& B)
+{
+ m11 -= B.m11;
+ m12 -= B.m12;
+ m21 -= B.m21;
+ m22 -= B.m22;
+ return( *this );
+}
+
+inline LinearMapR2 operator+ (const LinearMapR2& A, const LinearMapR2& B)
+{
+ return( LinearMapR2( A.m11+B.m11, A.m21+B.m21,
+ A.m12+B.m12, A.m22+B.m22 ) );
+}
+
+inline LinearMapR2 operator- (const Matrix2x2& A)
+{
+ return( LinearMapR2( -A.m11, -A.m21, -A.m12, -A.m22 ) );
+}
+
+inline LinearMapR2 operator- (const LinearMapR2& A, const LinearMapR2& B)
+{
+ return( LinearMapR2( A.m11-B.m11, A.m21-B.m21,
+ A.m12-B.m12, A.m22-B.m22 ) );
+}
+
+inline LinearMapR2& LinearMapR2::operator*= (register double b)
+{
+ m11 *= b;
+ m12 *= b;
+ m21 *= b;
+ m22 *= b;
+ return ( *this);
+}
+
+inline LinearMapR2 operator* ( const LinearMapR2& A, register double b)
+{
+ return( LinearMapR2( A.m11*b, A.m21*b,
+ A.m12*b, A.m22*b ) );
+}
+
+inline LinearMapR2 operator* ( register double b, const LinearMapR2& A)
+{
+ return( LinearMapR2( A.m11*b, A.m21*b,
+ A.m12*b, A.m22*b ) );
+}
+
+inline LinearMapR2 operator/ ( const LinearMapR2& A, double b)
+{
+ register double bInv = 1.0/b;
+ return ( A*bInv );
+}
+
+inline LinearMapR2& LinearMapR2::operator/= (register double b)
+{
+ register double bInv = 1.0/b;
+ return ( *this *= bInv );
+}
+
+inline LinearMapR2 LinearMapR2::Transpose() const // Returns the transpose
+{
+ return (LinearMapR2( m11, m12, m21, m22 ) );
+}
+
+inline LinearMapR2& LinearMapR2::operator*= (const Matrix2x2& B) // Matrix product
+{
+ (*this).Matrix2x2::operator*=(B);
+
+ return( *this );
+}
+
+inline LinearMapR2 operator* ( const LinearMapR2& A, const Matrix2x2& B)
+{
+ LinearMapR2 AA(A);
+ AA.Matrix2x2::operator*=(B);
+ return AA;
+}
+
+inline LinearMapR2 operator* ( const Matrix2x2& A, const LinearMapR2& B)
+{
+ LinearMapR2 AA(A);
+ AA.Matrix2x2::operator*=(B);
+ return AA;
+}
+
+inline LinearMapR2 operator* ( const LinearMapR2& A, const LinearMapR2& B)
+{
+ LinearMapR2 AA(A);
+ AA.Matrix2x2::operator*=(B);
+ return AA;
+}
+
+inline double LinearMapR2::Determinant () const // Returns the determinant
+{
+ return ( m11*m22 - m12*m21 );
+}
+
+// ******************************************************
+// * RotationMapR2 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline RotationMapR2::RotationMapR2()
+{
+ SetIdentity();
+ return;
+}
+
+inline RotationMapR2::RotationMapR2( const VectorR2& u, const VectorR2& v )
+:Matrix2x2 ( u, v )
+{ }
+
+inline RotationMapR2::RotationMapR2(
+ double a11, double a21, double a12, double a22 )
+ // Values specified in column order!!!
+:Matrix2x2 ( a11, a21, a12, a22 )
+{ }
+
+inline RotationMapR2 RotationMapR2::Transpose() const // Returns the transpose
+{
+ return ( RotationMapR2( m11, m12,
+ m21, m22 ) );
+}
+
+inline VectorR2 RotationMapR2::Invert(const VectorR2& u) const // Returns solution
+{
+ return (VectorR2( m11*u.x + m21*u.y, // Multiply with Transpose
+ m12*u.x + m22*u.y ) );
+}
+
+inline RotationMapR2& RotationMapR2::operator*= (const RotationMapR2& B) // Matrix product
+{
+ (*this).Matrix2x2::operator*=(B);
+
+ return( *this );
+}
+
+inline RotationMapR2 operator* ( const RotationMapR2& A, const RotationMapR2& B)
+{
+ RotationMapR2 AA(A);
+ AA.Matrix2x2::operator*=(B);
+ return AA;
+}
+
+
+// ***************************************************************
+// * 2-space vector and matrix utilities (inlined functions) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+// Returns a righthanded orthonormal basis to complement vector u
+// The vector u must be unit.
+inline VectorR2 GetOrtho( const VectorR2& u )
+{
+ return VectorR2 ( -u.y, u.x );
+}
+
+// Returns the projection of u onto unit v
+inline VectorR2 ProjectToUnit ( const VectorR2& u, const VectorR2& v)
+{
+ return (u^v)*v;
+}
+
+// Returns the projection of u onto the plane perpindicular to the unit vector v
+inline VectorR2 ProjectPerpUnit ( const VectorR2& u, const VectorR2& v)
+{
+ return ( u - ((u^v)*v) );
+}
+
+// Returns the projection of u onto the plane perpindicular to the unit vector v
+// This one is more stable when u and v are nearly equal.
+inline VectorR2 ProjectPerpUnitDiff ( const VectorR2& u, const VectorR2& v)
+{
+ VectorR2 ans = u;
+ ans -= v;
+ ans -= ((ans^v)*v);
+ return ans; // ans = (u-v) - ((u-v)^v)*v
+}
+
+// Returns the solid angle between vectors u and v.
+inline double Angle( const VectorR2& u, const VectorR2& v)
+{
+ double nSqU = u.NormSq();
+ double nSqV = v.NormSq();
+ if ( nSqU==0.0 && nSqV==0.0 ) {
+ return (0.0);
+ }
+ else {
+ return ( AngleUnit( u/sqrt(nSqU), v/sqrt(nSqV) ) );
+ }
+}
+
+inline double AngleUnit( const VectorR2& u, const VectorR2& v )
+{
+ return ( atan2 ( (ProjectPerpUnit(v,u)).Norm(), u^v ) );
+}
+
+// Projection maps (LinearMapR2's)
+
+// VectorProjectMap returns map projecting onto a given vector u.
+// u should be a unit vector (otherwise the returned map is
+// scaled according to the magnitude of u.
+inline LinearMapR2 VectorProjectMap( const VectorR2& u )
+{
+ double xy = u.x*u.y;
+ return( LinearMapR2( u.x*u.x, xy, xy, u.y*u.y ) ) ;
+}
+
+// PlaneProjectMap returns map projecting onto a given plane.
+// The plane is the plane orthognal to u.
+// u must be a unit vector (otherwise the returned map is
+// garbage).
+inline LinearMapR2 PerpProjectMap ( const VectorR2& u )
+{
+ double nxy = -u.x*u.y;
+ return ( LinearMapR2 ( 1.0-u.x*u.x, nxy, nxy, 1.0-u.y*u.y ) );
+}
+
+// fromVec and toVec should be unit vectors
+inline RotationMapR2 RotateToMap( const VectorR2& fromVec, const VectorR2& toVec)
+{
+ double costheta = fromVec.x*toVec.x + fromVec.y*toVec.y;
+ double sintheta = fromVec.x*toVec.y - fromVec.y*toVec.x;
+ return( RotationMapR2( costheta, sintheta, -sintheta, costheta ) );
+}
+
+#endif // LINEAR_R2_H
diff --git a/examples/ThirdPartyLibs/BussIK/LinearR3.cpp b/examples/ThirdPartyLibs/BussIK/LinearR3.cpp
new file mode 100644
index 000000000..c74d2b2b5
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR3.cpp
@@ -0,0 +1,822 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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);
+}
+
diff --git a/examples/ThirdPartyLibs/BussIK/LinearR3.h b/examples/ThirdPartyLibs/BussIK/LinearR3.h
new file mode 100644
index 000000000..4b79d4eed
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR3.h
@@ -0,0 +1,2027 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+//
+// Linear Algebra Classes over R3
+//
+//
+// A. Vector and Position classes
+//
+// A.1. VectorR3: a real column vector of length 3.
+//
+// A.2. VectorHgR3: a column vector of length 4 which is
+// the homogenous representation of a vector in 3-space
+//
+// B. Matrix Classes
+//
+// B.1 LinearMapR3 - arbitrary linear map; 3x3 real matrix
+//
+// B.2 AffineMapR3 - arbitrary affine map; a 3x4 real matrix
+//
+// B.3 RotationMapR3 - orthonormal 3x3 matrix
+//
+// B.4 RigidMapR3 - RotationMapR3 plus displacement
+//
+
+#ifndef LINEAR_R3_H
+#define LINEAR_R3_H
+
+#include
+#include
+#include
+#include "MathMisc.h"
+using namespace std;
+
+class VectorR3; // Space Vector (length 3)
+class VectorHgR3; // Homogenous Space Vector
+class VectorR4; // Space Vector (length 4)
+
+class LinearMapR3; // Linear Map (3x3 Matrix)
+class AffineMapR3; // Affine Map (3x4 Matrix)
+class RotationMapR3; // Rotation (3x3 orthonormal matrix)
+class RigidMapR3; // 3x4 matrix, first 3 columns orthonormal
+
+// Most for internal use:
+class Matrix3x3;
+class Matrix3x4;
+
+class Quaternion;
+
+// **************************************
+// VectorR3 class *
+// * * * * * * * * * * * * * * * * * * **
+
+class VectorR3 {
+
+public:
+ double x, y, z; // The x & y & z coordinates.
+
+ static const VectorR3 Zero;
+ static const VectorR3 UnitX;
+ static const VectorR3 UnitY;
+ static const VectorR3 UnitZ;
+ static const VectorR3 NegUnitX;
+ static const VectorR3 NegUnitY;
+ static const VectorR3 NegUnitZ;
+
+public:
+ VectorR3( ) : x(0.0), y(0.0), z(0.0) {}
+ VectorR3( double xVal, double yVal, double zVal )
+ : x(xVal), y(yVal), z(zVal) {}
+ VectorR3( const VectorHgR3& uH );
+
+ VectorR3& Set( const Quaternion& ); // Convert quat to rotation vector
+ VectorR3& Set( double xx, double yy, double zz )
+ { x=xx; y=yy; z=zz; return *this; }
+ VectorR3& SetFromHg( const VectorR4& ); // Convert homogeneous VectorR4 to VectorR3
+ VectorR3& SetZero() { x=0.0; y=0.0; z=0.0; return *this;}
+ VectorR3& Load( const double* v );
+ VectorR3& Load( const float* v );
+ void Dump( double* v ) const;
+ void Dump( float* v ) const;
+
+ inline double operator[]( int i );
+
+ VectorR3& operator= ( const VectorR3& v )
+ { x=v.x; y=v.y; z=v.z; return(*this);}
+ VectorR3& operator+= ( const VectorR3& v )
+ { x+=v.x; y+=v.y; z+=v.z; return(*this); }
+ VectorR3& operator-= ( const VectorR3& v )
+ { x-=v.x; y-=v.y; z-=v.z; return(*this); }
+ VectorR3& operator*= ( double m )
+ { x*=m; y*=m; z*=m; return(*this); }
+ VectorR3& operator/= ( double m )
+ { register double mInv = 1.0/m;
+ x*=mInv; y*=mInv; z*=mInv;
+ return(*this); }
+ VectorR3 operator- () const { return ( VectorR3(-x, -y, -z) ); }
+ VectorR3& operator*= (const VectorR3& v); // Cross Product
+ VectorR3& ArrayProd(const VectorR3&); // Component-wise product
+
+ VectorR3& AddScaled( const VectorR3& u, double s );
+
+ bool IsZero() const { return ( x==0.0 && y==0.0 && z==0.0 ); }
+ double Norm() const { return ( (double)sqrt( x*x + y*y + z*z ) ); }
+ double NormSq() const { return ( x*x + y*y + z*z ); }
+ double MaxAbs() const;
+ double Dist( const VectorR3& u ) const; // Distance from u
+ double DistSq( const VectorR3& u ) const; // Distance from u squared
+ VectorR3& Negate() { x = -x; y = -y; z = -z; return *this;}
+ VectorR3& Normalize () { *this /= Norm(); return *this;} // No error checking
+ inline VectorR3& MakeUnit(); // Normalize() with error checking
+ inline VectorR3& ReNormalize();
+ bool IsUnit( ) const
+ { register double norm = Norm();
+ return ( 1.000001>=norm && norm>=0.999999 ); }
+ bool IsUnit( double tolerance ) const
+ { register double norm = Norm();
+ return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); }
+ bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );}
+ // tolerance should be non-negative
+
+ double YaxisDistSq() const { return (x*x+z*z); }
+ double YaxisDist() const { return sqrt(x*x+z*z); }
+
+ VectorR3& Rotate( double theta, const VectorR3& u); // rotate around u.
+ VectorR3& RotateUnitInDirection ( const VectorR3& dir); // rotate in direction dir
+ VectorR3& Rotate( const Quaternion& ); // Rotate according to quaternion
+
+ friend ostream& operator<< ( ostream& os, const VectorR3& u );
+
+};
+
+inline VectorR3 operator+( const VectorR3& u, const VectorR3& v );
+inline VectorR3 operator-( const VectorR3& u, const VectorR3& v );
+inline VectorR3 operator*( const VectorR3& u, double m);
+inline VectorR3 operator*( double m, const VectorR3& u);
+inline VectorR3 operator/( const VectorR3& u, double m);
+inline int operator==( const VectorR3& u, const VectorR3& v );
+
+inline double operator^ (const VectorR3& u, const VectorR3& v ); // Dot Product
+inline VectorR3 operator* (const VectorR3& u, const VectorR3& v); // Cross Product
+inline VectorR3 ArrayProd ( const VectorR3& u, const VectorR3& v );
+
+inline double Mag(const VectorR3& u) { return u.Norm(); }
+inline double Dist(const VectorR3& u, const VectorR3& v) { return u.Dist(v); }
+inline double DistSq(const VectorR3& u, const VectorR3& v) { return u.DistSq(v); }
+inline double NormalizeError (const VectorR3& u);
+
+extern const VectorR3 UnitVecIR3;
+extern const VectorR3 UnitVecJR3;
+extern const VectorR3 UnitVecKR3;
+
+inline VectorR3 ToVectorR3( const Quaternion& q )
+ {return VectorR3().Set(q);}
+
+
+// ****************************************
+// VectorHgR3 class *
+// * * * * * * * * * * * * * * * * * * * **
+
+class VectorHgR3 {
+
+public:
+ double x, y, z, w; // The x & y & z & w coordinates.
+
+public:
+ VectorHgR3( ) : x(0.0), y(0.0), z(0.0), w(1.0) {}
+ VectorHgR3( double xVal, double yVal, double zVal )
+ : x(xVal), y(yVal), z(zVal), w(1.0) {}
+ VectorHgR3( double xVal, double yVal, double zVal, double wVal )
+ : x(xVal), y(yVal), z(zVal), w(wVal) {}
+ VectorHgR3 ( const VectorR3& u ) : x(u.x), y(u.y), z(u.z), w(1.0) {}
+};
+
+//
+// Advanced vector and position functions (prototypes)
+//
+
+VectorR3 Interpolate( const VectorR3& start, const VectorR3& end, double a);
+
+// *****************************************
+// Matrix3x3 class *
+// * * * * * * * * * * * * * * * * * * * * *
+
+class Matrix3x3 {
+public:
+
+ double m11, m12, m13, m21, m22, m23, m31, m32, m33;
+
+ // Implements a 3x3 matrix: m_i_j - row-i and column-j entry
+
+ static const Matrix3x3 Identity;
+
+public:
+ inline Matrix3x3();
+ inline Matrix3x3(const VectorR3&, const VectorR3&, const VectorR3&); // Sets by columns!
+ inline Matrix3x3(double, double, double, double, double, double,
+ double, double, double ); // Sets by columns
+
+ inline void SetIdentity (); // Set to the identity map
+ inline void Set ( const Matrix3x3& ); // Set to the matrix.
+ inline void Set3x3 ( const Matrix3x4& ); // Set to the 3x3 part of the matrix.
+ inline void Set( const VectorR3&, const VectorR3&, const VectorR3& );
+ inline void Set( double, double, double,
+ double, double, double,
+ double, double, double );
+ inline void SetByRows( double, double, double, double, double, double,
+ double, double, double );
+ inline void SetByRows( const VectorR3&, const VectorR3&, const VectorR3& );
+
+ inline void SetColumn1 ( double, double, double );
+ inline void SetColumn2 ( double, double, double );
+ inline void SetColumn3 ( double, double, double );
+ inline void SetColumn1 ( const VectorR3& );
+ inline void SetColumn2 ( const VectorR3& );
+ inline void SetColumn3 ( const VectorR3& );
+ inline VectorR3 Column1() const;
+ inline VectorR3 Column2() const;
+ inline VectorR3 Column3() const;
+
+ inline void SetRow1 ( double, double, double );
+ inline void SetRow2 ( double, double, double );
+ inline void SetRow3 ( double, double, double );
+ inline void SetRow1 ( const VectorR3& );
+ inline void SetRow2 ( const VectorR3& );
+ inline void SetRow3 ( const VectorR3& );
+ inline VectorR3 Row1() const;
+ inline VectorR3 Row2() const;
+ inline VectorR3 Row3() const;
+
+ inline void SetDiagonal( double, double, double );
+ inline void SetDiagonal( const VectorR3& );
+ inline double Diagonal( int );
+
+ inline void MakeTranspose(); // Transposes it.
+ Matrix3x3& ReNormalize();
+ VectorR3 Solve(const VectorR3&) const; // Returns solution
+
+ inline void Transform( VectorR3* ) const;
+ inline void Transform( const VectorR3& src, VectorR3* dest) const;
+
+protected:
+ void OperatorTimesEquals( const Matrix3x3& ); // Internal use only
+ void SetZero (); // Set to the zero map
+
+};
+
+inline VectorR3 operator* ( const Matrix3x3&, const VectorR3& );
+
+ostream& operator<< ( ostream& os, const Matrix3x3& A );
+
+
+// *****************************************
+// Matrix3x4 class *
+// * * * * * * * * * * * * * * * * * * * * *
+
+class Matrix3x4
+{
+public:
+ double m11, m12, m13, m21, m22, m23, m31, m32, m33;
+ double m14;
+ double m24;
+ double m34;
+
+ static const Matrix3x4 Identity;
+
+public:
+ // Constructors set by columns!
+ Matrix3x4() {}
+ Matrix3x4(const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& );
+ Matrix3x4(double, double, double, double, double, double,
+ double, double, double, double, double, double ); // Sets by columns
+ Matrix3x4( const Matrix3x3&, const VectorR3& );
+
+ void SetIdentity (); // Set to the identity map
+ void Set ( const Matrix3x4& ); // Set to the matrix.
+ void Set3x3 ( const Matrix3x3& ); // Set linear part to the matrix.
+ void Set ( const Matrix3x3&, const VectorR3& ); // Set to the matrix plus 4th column
+ void Set( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& );
+ void Set( double, double, double,
+ double, double, double,
+ double, double, double,
+ double, double, double ); // Sets by columns
+ void Set3x3( double, double, double,
+ double, double, double,
+ double, double, double ); // Sets by columns
+ void SetByRows( double, double, double, double, double, double,
+ double, double, double, double, double, double );
+
+ void SetColumn1 ( double, double, double );
+ void SetColumn2 ( double, double, double );
+ void SetColumn3 ( double, double, double );
+ void SetColumn4 ( double, double, double );
+ void SetColumn1 ( const VectorR3& );
+ void SetColumn2 ( const VectorR3& );
+ void SetColumn3 ( const VectorR3& );
+ void SetColumn4 ( const VectorR3& );
+ VectorR3 Column1() const;
+ VectorR3 Column2() const;
+ VectorR3 Column3() const;
+ VectorR3 Column4() const;
+ void SetRow1 ( double x, double y, double z, double w );
+ void SetRow2 ( double x, double y, double z, double w );
+ void SetRow3 ( double x, double y, double z, double w );
+ void SetRow4 ( double x, double y, double z, double w );
+
+ Matrix3x4& ApplyTranslationLeft( const VectorR3& u );
+ Matrix3x4& ApplyTranslationRight( const VectorR3& u );
+ Matrix3x4& ApplyYRotationLeft( double theta );
+ Matrix3x4& ApplyYRotationLeft( double costheta, double sintheta );
+
+ Matrix3x4& ReNormalize();
+ VectorR3 Solve(const VectorR3&) const; // Returns solution
+
+ inline void Transform( VectorR3* ) const;
+ inline void Transform3x3( VectorR3* ) const;
+ inline void Transform( const VectorR3& src, VectorR3* dest ) const;
+ inline void Transform3x3( const VectorR3& src, VectorR3* dest ) const;
+ inline void Transform3x3Transpose( VectorR3* dest ) const;
+ inline void Transform3x3Transpose( const VectorR3& src, VectorR3* dest ) const;
+
+protected:
+ void SetZero (); // Set to the zero map
+ void OperatorTimesEquals( const Matrix3x3& ); // Internal use only
+ void OperatorTimesEquals( const Matrix3x4& ); // Internal use only
+};
+
+inline VectorR3 operator* ( const Matrix3x4&, const VectorR3& );
+
+ostream& operator<< ( ostream& os, const Matrix3x4& A );
+
+// *****************************************
+// LinearMapR3 class *
+// * * * * * * * * * * * * * * * * * * * * *
+
+class LinearMapR3 : public Matrix3x3 {
+
+public:
+
+ LinearMapR3();
+ LinearMapR3( const VectorR3&, const VectorR3&, const VectorR3& );
+ LinearMapR3( double, double, double, double, double, double,
+ double, double, double ); // Sets by columns
+ LinearMapR3 ( const Matrix3x3& );
+
+ void SetZero (); // Set to the zero map
+ inline void Negate();
+
+ inline LinearMapR3& operator+= (const Matrix3x3& );
+ inline LinearMapR3& operator-= (const Matrix3x3& );
+ inline LinearMapR3& operator*= (double);
+ inline LinearMapR3& operator/= (double);
+ LinearMapR3& operator*= (const Matrix3x3& ); // Matrix product
+
+ inline LinearMapR3 Transpose() const; // Returns the transpose
+ double Determinant () const; // Returns the determinant
+ LinearMapR3 Inverse() const; // Returns inverse
+ LinearMapR3& Invert(); // Converts into inverse.
+ VectorR3 Solve(const VectorR3&) const; // Returns solution
+ LinearMapR3 PseudoInverse() const; // Returns pseudo-inverse TO DO
+ VectorR3 PseudoSolve(const VectorR3&); // Finds least squares solution TO DO
+
+};
+
+inline LinearMapR3 operator+ (const LinearMapR3&, const Matrix3x3&);
+inline LinearMapR3 operator+ (const Matrix3x3&, const LinearMapR3&);
+inline LinearMapR3 operator- (const LinearMapR3&);
+inline LinearMapR3 operator- (const LinearMapR3&, const Matrix3x3&);
+inline LinearMapR3 operator- (const Matrix3x3&, const LinearMapR3&);
+inline LinearMapR3 operator* ( const LinearMapR3&, double);
+inline LinearMapR3 operator* ( double, const LinearMapR3& );
+inline LinearMapR3 operator/ ( const LinearMapR3&, double );
+LinearMapR3 operator* ( const LinearMapR3&, const LinearMapR3& );
+ // Matrix product (composition)
+
+
+// *****************************************************
+// * AffineMapR3 class *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+class AffineMapR3 : public Matrix3x4 {
+
+public:
+ AffineMapR3();
+ AffineMapR3( double, double, double, double, double, double,
+ double, double, double, double, double, double ); // Sets by columns
+ AffineMapR3 ( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3&);
+ AffineMapR3 ( const LinearMapR3&, const VectorR3& );
+
+ void SetIdentity (); // Set to the identity map
+ void SetZero (); // Set to the zero map
+
+ AffineMapR3& operator+= (const Matrix3x4& );
+ AffineMapR3& operator-= (const Matrix3x4& );
+ AffineMapR3& operator*= (double);
+ AffineMapR3& operator/= (double);
+ AffineMapR3& operator*= (const Matrix3x3& ); // Composition
+ AffineMapR3& operator*= (const Matrix3x4& ); // Composition
+
+ AffineMapR3& ApplyTranslationLeft( const VectorR3& u )
+ { Matrix3x4::ApplyTranslationLeft( u ); return *this; }
+ AffineMapR3& ApplyTranslationRight( const VectorR3& u )
+ { Matrix3x4::ApplyTranslationRight( u ); return *this; }
+ AffineMapR3& ApplyYRotationLeft( double theta )
+ { Matrix3x4::ApplyYRotationLeft( theta ); return *this; }
+ AffineMapR3& ApplyYRotationLeft( double costheta, double sintheta )
+ { Matrix3x4::ApplyYRotationLeft( costheta, sintheta ); return *this; }
+
+ AffineMapR3 Inverse() const; // Returns inverse
+ AffineMapR3& Invert(); // Converts into inverse.
+ VectorR3 Solve(const VectorR3&) const; // Returns solution
+ AffineMapR3 PseudoInverse() const; // Returns pseudo-inverse // TO DO
+ VectorR3 PseudoSolve(const VectorR3&); // Least squares solution // TO DO
+};
+
+inline AffineMapR3 operator+ (const AffineMapR3&, const Matrix3x4&);
+inline AffineMapR3 operator+ (const Matrix3x4&, const AffineMapR3&);
+inline AffineMapR3 operator+ (const AffineMapR3&, const Matrix3x3&);
+inline AffineMapR3 operator+ (const Matrix3x3&, const AffineMapR3&);
+inline AffineMapR3 operator- (const AffineMapR3&, const Matrix3x4&);
+inline AffineMapR3 operator- (const Matrix3x4&, const AffineMapR3&);
+inline AffineMapR3 operator- (const AffineMapR3&, const Matrix3x3&);
+inline AffineMapR3 operator- (const Matrix3x3&, const AffineMapR3&);
+inline AffineMapR3 operator* (const AffineMapR3&, double);
+inline AffineMapR3 operator* (double, const AffineMapR3& );
+inline AffineMapR3 operator/ (const AffineMapR3&, double );
+
+// Composition operators
+AffineMapR3 operator* ( const AffineMapR3&, const AffineMapR3& );
+AffineMapR3 operator* ( const LinearMapR3&, const AffineMapR3& );
+AffineMapR3 operator* ( const AffineMapR3&, const LinearMapR3& );
+
+
+// *******************************************
+// RotationMapR3 class *
+// * * * * * * * * * * * * * * * * * * * * * *
+
+class RotationMapR3 : public Matrix3x3 {
+
+public:
+
+ RotationMapR3();
+ RotationMapR3( const VectorR3&, const VectorR3&, const VectorR3& );
+ RotationMapR3( double, double, double, double, double, double,
+ double, double, double );
+
+ RotationMapR3& Set( const Quaternion& );
+ RotationMapR3& Set( const VectorR3&, double theta ); // Set rotation axis and angle
+ RotationMapR3& Set( const VectorR3&, double sintheta, double costheta );
+
+ RotationMapR3& operator*= (const RotationMapR3& ); // Matrix product
+
+ RotationMapR3 Transpose() const { return Inverse(); }; // Returns the transpose
+ RotationMapR3 Inverse() const; // Returns inverse
+ RotationMapR3& Invert(); // Converts into inverse.
+ VectorR3 Solve(const VectorR3&) const; // Returns solution // Was named Invert
+
+ bool ToAxisAndAngle( VectorR3* u, double* theta ) const; // returns unit vector u and angle
+
+};
+
+RotationMapR3 operator* ( const RotationMapR3&, const RotationMapR3& );
+ // Matrix product (composition)
+
+inline RotationMapR3 ToRotationMapR3( const Quaternion& q )
+ { return( RotationMapR3().Set(q) ); }
+
+ostream& operator<< ( ostream& os, const RotationMapR3& A );
+
+
+
+// ***************************************************************
+// * RigidMapR3 class - prototypes. * *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+class RigidMapR3 : public Matrix3x4
+{
+
+public:
+ RigidMapR3();
+ RigidMapR3( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& );
+ RigidMapR3( double, double, double, double, double, double,
+ double, double, double, double, double, double );
+ RigidMapR3( const Matrix3x3&, const VectorR3& );
+
+ RigidMapR3& Set( const Matrix3x3&, const VectorR3& ); // Set to RotationMap & Vector
+ RigidMapR3& SetTranslationPart( const VectorR3& ); // Set the translation part
+ RigidMapR3& SetTranslationPart( double, double, double ); // Set the translation part
+ RigidMapR3& SetRotationPart( const Matrix3x3& ); // Set the rotation part
+ RigidMapR3& SetRotationPart( const Quaternion& );
+ RigidMapR3& SetRotationPart( const VectorR3&, double theta ); // Set rotation axis and angle
+ RigidMapR3& SetRotationPart( const VectorR3&, double sintheta, double costheta );
+
+ RigidMapR3& ApplyTranslationLeft( const VectorR3& u )
+ {Matrix3x4::ApplyTranslationLeft( u ); return *this;}
+ RigidMapR3& ApplyTranslationRight( const VectorR3& u )
+ {Matrix3x4::ApplyTranslationRight( u ); return *this;}
+ RigidMapR3& ApplyYRotationLeft( double theta )
+ { Matrix3x4::ApplyYRotationLeft( theta ); return *this; }
+ RigidMapR3& ApplyYRotationLeft( double costheta, double sintheta )
+ { Matrix3x4::ApplyYRotationLeft( costheta, sintheta ); return *this; }
+
+ RigidMapR3& operator*=(const RotationMapR3& ); // Composition
+ RigidMapR3& operator*=(const RigidMapR3& ); // Composition
+
+ RigidMapR3 Inverse() const; // Returns inverse
+ RigidMapR3& Invert(); // Converts into inverse.
+
+ bool CalcGlideRotation( VectorR3* u, VectorR3* v,
+ double *glideDist, double *rotation ) const;
+
+ void Transform3x3Inverse( VectorR3* dest ) const
+ { Matrix3x4::Transform3x3Transpose( dest ); }
+ void Transform3x3Inverse( const VectorR3& src, VectorR3* dest ) const
+ { Matrix3x4::Transform3x3Transpose( src, dest ); }
+
+};
+
+
+// ***************************************************************
+// * 3-space vector and matrix utilities (prototypes) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+// Returns the solid angle between vectors v and w.
+inline double SolidAngle( const VectorR3& v, const VectorR3& w);
+
+// Returns a righthanded orthonormal basis to complement unit vector x
+void GetOrtho( const VectorR3& x, VectorR3& y, VectorR3& z);
+// Returns a vector v orthonormal to unit vector x
+void GetOrtho( const VectorR3& x, VectorR3& y );
+
+// Projections
+
+// The next three functions are templated below.
+//inline VectorR3 ProjectToUnit ( const VectorR3& u, const VectorR3& v); // Project u onto v
+//inline VectorR3 ProjectPerpUnit ( const VectorR3& u, const VectorR3 & v); // Project perp to v
+//inline VectorR3 ProjectPerpUnitDiff ( const VectorR3& u, const VectorR3& v)
+// v must be a unit vector.
+
+// Projection maps (LinearMapR3s)
+
+inline LinearMapR3 VectorProjectMap( const VectorR3& u );
+inline LinearMapR3 PlaneProjectMap ( const VectorR3& w );
+inline LinearMapR3 PlaneProjectMap ( const VectorR3& u, const VectorR3 &v );
+// u,v,w - must be unit vector. u and v must be orthonormal and
+// specify the plane they are parallel to. w specifies the plane
+// it is orthogonal to.
+
+// VrRotate is similar to glRotate. Returns a matrix (RotationMapR3)
+// that will perform the rotation. u should be a unit vector.
+RotationMapR3 VrRotate( double theta, const VectorR3& u );
+RotationMapR3 VrRotate( double costheta, double sintheta, const VectorR3& u );
+RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec);
+RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec);
+// fromVec and toVec should be unit vectors for RotateToMap
+
+// ***************************************************************
+// * Stream Output Routines (Prototypes) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+ostream& operator<< ( ostream& os, const VectorR3& u );
+
+
+// *****************************************************
+// * VectorR3 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+inline VectorR3& VectorR3::Load( const double* v )
+{
+ x = *v;
+ y = *(v+1);
+ z = *(v+2);
+ return *this;
+}
+
+inline VectorR3& VectorR3::Load( const float* v )
+{
+ x = *v;
+ y = *(v+1);
+ z = *(v+2);
+ return *this;
+}
+
+inline void VectorR3::Dump( double* v ) const
+{
+ *v = x;
+ *(v+1) = y;
+ *(v+2) = z;
+}
+
+inline void VectorR3::Dump( float* v ) const
+{
+ *v = (float)x;
+ *(v+1) = (float)y;
+ *(v+2) = (float)z;
+}
+
+inline double VectorR3::operator[]( int i )
+{
+ switch (i) {
+ case 0:
+ return x;
+ case 1:
+ return y;
+ case 2:
+ return z;
+ default:
+ assert(0);
+ return 0.0;
+ }
+}
+
+inline VectorR3& VectorR3::MakeUnit () // Convert to unit vector (or leave zero).
+{
+ double nSq = NormSq();
+ if (nSq != 0.0) {
+ *this /= sqrt(nSq);
+ }
+ return *this;
+}
+
+inline VectorR3 operator+( const VectorR3& u, const VectorR3& v )
+{
+ return VectorR3(u.x+v.x, u.y+v.y, u.z+v.z);
+}
+inline VectorR3 operator-( const VectorR3& u, const VectorR3& v )
+{
+ return VectorR3(u.x-v.x, u.y-v.y, u.z-v.z);
+}
+inline VectorR3 operator*( const VectorR3& u, register double m)
+{
+ return VectorR3( u.x*m, u.y*m, u.z*m);
+}
+inline VectorR3 operator*( register double m, const VectorR3& u)
+{
+ return VectorR3( u.x*m, u.y*m, u.z*m);
+}
+inline VectorR3 operator/( const VectorR3& u, double m)
+{
+ register double mInv = 1.0/m;
+ return VectorR3( u.x*mInv, u.y*mInv, u.z*mInv);
+}
+
+inline int operator==( const VectorR3& u, const VectorR3& v )
+{
+ return ( u.x==v.x && u.y==v.y && u.z==v.z );
+}
+
+inline double operator^ ( const VectorR3& u, const VectorR3& v ) // Dot Product
+{
+ return ( u.x*v.x + u.y*v.y + u.z*v.z );
+}
+
+inline VectorR3 operator* (const VectorR3& u, const VectorR3& v) // Cross Product
+{
+ return (VectorR3( u.y*v.z - u.z*v.y,
+ u.z*v.x - u.x*v.z,
+ u.x*v.y - u.y*v.x ) );
+}
+
+inline VectorR3 ArrayProd ( const VectorR3& u, const VectorR3& v )
+{
+ return ( VectorR3( u.x*v.x, u.y*v.y, u.z*v.z ) );
+}
+
+inline VectorR3& VectorR3::operator*= (const VectorR3& v) // Cross Product
+{
+ double tx=x, ty=y;
+ x = y*v.z - z*v.y;
+ y = z*v.x - tx*v.z;
+ z = tx*v.y - ty*v.x;
+ return ( *this );
+}
+
+inline VectorR3& VectorR3::ArrayProd (const VectorR3& v) // Component-wise Product
+{
+ x *= v.x;
+ y *= v.y;
+ z *= v.z;
+ return ( *this );
+}
+
+inline VectorR3& VectorR3::AddScaled( const VectorR3& u, double s )
+{
+ x += s*u.x;
+ y += s*u.y;
+ z += s*u.z;
+ return(*this);
+}
+
+inline VectorR3::VectorR3( const VectorHgR3& uH )
+: x(uH.x), y(uH.y), z(uH.z)
+{
+ *this /= uH.w;
+}
+
+inline VectorR3& VectorR3::ReNormalize() // Convert near unit back to unit
+{
+ double nSq = NormSq();
+ register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor
+ *this *= mFact;
+ return *this;
+}
+
+inline double NormalizeError (const VectorR3& u)
+{
+ register double discrepancy;
+ discrepancy = u.x*u.x + u.y*u.y + u.z*u.z - 1.0;
+ if ( discrepancy < 0.0 ) {
+ discrepancy = -discrepancy;
+ }
+ return discrepancy;
+}
+
+inline double VectorR3::Dist( const VectorR3& u ) const // Distance from u
+{
+ return sqrt( DistSq(u) );
+}
+
+inline double VectorR3::DistSq( const VectorR3& u ) const // Distance from u
+{
+ return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) + (z-u.z)*(z-u.z) );
+}
+
+//
+// Interpolation routines (not just Spherical Interpolation)
+//
+
+// Interpolate(start,end,frac) - linear interpolation
+// - allows overshooting the end points
+inline VectorR3 Interpolate( const VectorR3& start, const VectorR3& end, double a)
+{
+ VectorR3 ret;
+ Lerp( start, end, a, ret );
+ return ret;
+}
+
+
+// ******************************************************
+// * Matrix3x3 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline Matrix3x3::Matrix3x3() {}
+
+inline Matrix3x3::Matrix3x3( const VectorR3& u, const VectorR3& v,
+ const VectorR3& s )
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m13 = s.x; // Column 3
+ m23 = s.y;
+ m33 = s.z;
+}
+
+inline Matrix3x3::Matrix3x3( double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33)
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+}
+
+inline void Matrix3x3::SetIdentity ( )
+{
+ m11 = m22 = m33 = 1.0;
+ m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
+}
+
+inline void Matrix3x3::SetZero( )
+{
+ m11 = m12 = m13 = m21 = m22 = m23 = m31 = m32 = m33 = 0.0;
+}
+
+inline void Matrix3x3::Set ( const Matrix3x3& A ) // Set to the matrix.
+{
+ m11 = A.m11;
+ m21 = A.m21;
+ m31 = A.m31;
+ m12 = A.m12;
+ m22 = A.m22;
+ m32 = A.m32;
+ m13 = A.m13;
+ m23 = A.m23;
+ m33 = A.m33;
+}
+
+inline void Matrix3x3::Set3x3 ( const Matrix3x4& A ) // Set to the 3x3 part of the matrix.
+{
+ m11 = A.m11;
+ m21 = A.m21;
+ m31 = A.m31;
+ m12 = A.m12;
+ m22 = A.m22;
+ m32 = A.m32;
+ m13 = A.m13;
+ m23 = A.m23;
+ m33 = A.m33;
+}
+
+inline void Matrix3x3::Set( const VectorR3& u, const VectorR3& v,
+ const VectorR3& w)
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m13 = w.x; // Column 3
+ m23 = w.y;
+ m33 = w.z;
+}
+
+inline void Matrix3x3::Set( double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33)
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+}
+
+inline void Matrix3x3::SetByRows( double a11, double a12, double a13,
+ double a21, double a22, double a23,
+ double a31, double a32, double a33)
+ // Values specified in row order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+}
+
+inline void Matrix3x3::SetByRows( const VectorR3& u, const VectorR3& v,
+ const VectorR3& s )
+{
+ m11 = u.x; // Row 1
+ m12 = u.y;
+ m13 = u.z;
+ m21 = v.x; // Row 2
+ m22 = v.y;
+ m23 = v.z;
+ m31 = s.x; // Row 3
+ m32 = s.y;
+ m33 = s.z;
+}
+
+inline void Matrix3x3::SetColumn1 ( double x, double y, double z)
+{
+ m11 = x; m21 = y; m31= z;
+}
+
+inline void Matrix3x3::SetColumn2 ( double x, double y, double z)
+{
+ m12 = x; m22 = y; m32= z;
+}
+
+inline void Matrix3x3::SetColumn3 ( double x, double y, double z)
+{
+ m13 = x; m23 = y; m33= z;
+}
+
+inline void Matrix3x3::SetColumn1 ( const VectorR3& u )
+{
+ m11 = u.x; m21 = u.y; m31 = u.z;
+}
+
+inline void Matrix3x3::SetColumn2 ( const VectorR3& u )
+{
+ m12 = u.x; m22 = u.y; m32 = u.z;
+}
+
+inline void Matrix3x3::SetColumn3 ( const VectorR3& u )
+{
+ m13 = u.x; m23 = u.y; m33 = u.z;
+}
+
+inline void Matrix3x3::SetRow1 ( double x, double y, double z )
+{
+ m11 = x;
+ m12 = y;
+ m13 = z;
+}
+
+inline void Matrix3x3::SetRow2 ( double x, double y, double z )
+{
+ m21 = x;
+ m22 = y;
+ m23 = z;
+}
+
+inline void Matrix3x3::SetRow3 ( double x, double y, double z )
+{
+ m31 = x;
+ m32 = y;
+ m33 = z;
+}
+
+
+
+inline VectorR3 Matrix3x3::Column1() const
+{
+ return ( VectorR3(m11, m21, m31) );
+}
+
+inline VectorR3 Matrix3x3::Column2() const
+{
+ return ( VectorR3(m12, m22, m32) );
+}
+
+inline VectorR3 Matrix3x3::Column3() const
+{
+ return ( VectorR3(m13, m23, m33) );
+}
+
+inline VectorR3 Matrix3x3::Row1() const
+{
+ return ( VectorR3(m11, m12, m13) );
+}
+
+inline VectorR3 Matrix3x3::Row2() const
+{
+ return ( VectorR3(m21, m22, m23) );
+}
+
+inline VectorR3 Matrix3x3::Row3() const
+{
+ return ( VectorR3(m31, m32, m33) );
+}
+
+inline void Matrix3x3::SetDiagonal( double x, double y, double z )
+{
+ m11 = x;
+ m22 = y;
+ m33 = z;
+}
+
+inline void Matrix3x3::SetDiagonal( const VectorR3& u )
+{
+ SetDiagonal ( u.x, u.y, u.z );
+}
+
+inline double Matrix3x3::Diagonal( int i )
+{
+ switch (i) {
+ case 0:
+ return m11;
+ case 1:
+ return m22;
+ case 2:
+ return m33;
+ default:
+ assert(0);
+ return 0.0;
+ }
+}
+
+inline void Matrix3x3::MakeTranspose() // Transposes it.
+{
+ register double temp;
+ temp = m12;
+ m12 = m21;
+ m21=temp;
+ temp = m13;
+ m13 = m31;
+ m31 = temp;
+ temp = m23;
+ m23 = m32;
+ m32 = temp;
+}
+
+inline VectorR3 operator* ( const Matrix3x3& A, const VectorR3& u)
+{
+ return( VectorR3( A.m11*u.x + A.m12*u.y + A.m13*u.z,
+ A.m21*u.x + A.m22*u.y + A.m23*u.z,
+ A.m31*u.x + A.m32*u.y + A.m33*u.z ) );
+}
+
+inline void Matrix3x3::Transform( VectorR3* u ) const {
+ double newX, newY;
+ newX = m11*u->x + m12*u->y + m13*u->z;
+ newY = m21*u->x + m22*u->y + m23*u->z;
+ u->z = m31*u->x + m32*u->y + m33*u->z;
+ u->x = newX;
+ u->y = newY;
+}
+
+inline void Matrix3x3::Transform( const VectorR3& src, VectorR3* dest ) const {
+ dest->x = m11*src.x + m12*src.y + m13*src.z;
+ dest->y = m21*src.x + m22*src.y + m23*src.z;
+ dest->z = m31*src.x + m32*src.y + m33*src.z;
+}
+
+
+// ******************************************************
+// * Matrix3x4 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline Matrix3x4::Matrix3x4(const VectorR3& u, const VectorR3& v,
+ const VectorR3& s, const VectorR3& t)
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m13 = s.x; // Column 3
+ m23 = s.y;
+ m33 = s.z;
+ m14 = t.x;
+ m24 = t.y;
+ m34 = t.z;
+}
+
+inline Matrix3x4::Matrix3x4(double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33,
+ double a14, double a24, double a34)
+{ // Values in COLUMN order!
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+}
+
+inline Matrix3x4::Matrix3x4( const Matrix3x3& A, const VectorR3& u )
+{
+ Set(A, u);
+}
+
+inline void Matrix3x4::SetIdentity () // Set to the identity map
+{
+ m11 = m22 = m33 = 1.0;
+ m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
+ m14 = m24 = m34 = 0.0;
+}
+
+inline void Matrix3x4::SetZero () // Set to the zero map
+{
+ m11 = m22 = m33 = 0.0;
+ m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
+ m14 = m24 = m34 = 0.0;
+}
+
+inline void Matrix3x4::Set ( const Matrix3x4& A ) // Set to the matrix.
+{
+ m11 = A.m11;
+ m21 = A.m21;
+ m31 = A.m31;
+ m12 = A.m12;
+ m22 = A.m22;
+ m32 = A.m32;
+ m13 = A.m13;
+ m23 = A.m23;
+ m33 = A.m33;
+ m14 = A.m14;
+ m24 = A.m24;
+ m34 = A.m34;
+}
+
+inline void Matrix3x4::Set ( const Matrix3x3& A, const VectorR3& t ) // Set to the matrix plus 4th column
+{
+ m11 = A.m11;
+ m21 = A.m21;
+ m31 = A.m31;
+ m12 = A.m12;
+ m22 = A.m22;
+ m32 = A.m32;
+ m13 = A.m13;
+ m23 = A.m23;
+ m33 = A.m33;
+ m14 = t.x; // Column 4
+ m24 = t.y;
+ m34 = t.z;
+}
+
+// Set linear part to the matrix
+inline void Matrix3x4::Set3x3 ( const Matrix3x3& A )
+{
+ m11 = A.m11;
+ m21 = A.m21;
+ m31 = A.m31;
+ m12 = A.m12;
+ m22 = A.m22;
+ m32 = A.m32;
+ m13 = A.m13;
+ m23 = A.m23;
+ m33 = A.m33;
+}
+
+inline void Matrix3x4::Set( const VectorR3& u, const VectorR3& v,
+ const VectorR3& w, const VectorR3& t )
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m13 = w.x; // Column 3
+ m23 = w.y;
+ m33 = w.z;
+ m14 = t.x; // Column 4
+ m24 = t.y;
+ m34 = t.z;
+}
+
+inline void Matrix3x4::Set( double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33,
+ double a14, double a24, double a34 )
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+}
+
+inline void Matrix3x4::Set3x3( double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33 )
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+}
+
+inline void Matrix3x4::SetByRows( double a11, double a12, double a13, double a14,
+ double a21, double a22, double a23, double a24,
+ double a31, double a32, double a33, double a34 )
+ // Values specified in row order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+}
+
+inline void Matrix3x4::SetColumn1 ( double x, double y, double z)
+{
+ m11 = x; m21 = y; m31= z;
+}
+
+inline void Matrix3x4::SetColumn2 ( double x, double y, double z)
+{
+ m12 = x; m22 = y; m32= z;
+}
+
+inline void Matrix3x4::SetColumn3 ( double x, double y, double z)
+{
+ m13 = x; m23 = y; m33= z;
+}
+
+inline void Matrix3x4::SetColumn4 ( double x, double y, double z )
+{
+ m14 = x; m24 = y; m34= z;
+}
+
+inline void Matrix3x4::SetColumn1 ( const VectorR3& u )
+{
+ m11 = u.x; m21 = u.y; m31 = u.z;
+}
+
+inline void Matrix3x4::SetColumn2 ( const VectorR3& u )
+{
+ m12 = u.x; m22 = u.y; m32 = u.z;
+}
+
+inline void Matrix3x4::SetColumn3 ( const VectorR3& u )
+{
+ m13 = u.x; m23 = u.y; m33 = u.z;
+}
+
+inline void Matrix3x4::SetColumn4 ( const VectorR3& u )
+{
+ m14 = u.x; m24 = u.y; m34 = u.z;
+}
+
+inline VectorR3 Matrix3x4::Column1() const
+{
+ return ( VectorR3(m11, m21, m31) );
+}
+
+inline VectorR3 Matrix3x4::Column2() const
+{
+ return ( VectorR3(m12, m22, m32) );
+}
+
+inline VectorR3 Matrix3x4::Column3() const
+{
+ return ( VectorR3(m13, m23, m33) );
+}
+
+inline VectorR3 Matrix3x4::Column4() const
+{
+ return ( VectorR3(m14, m24, m34) );
+}
+
+inline void Matrix3x4::SetRow1 ( double x, double y, double z, double w )
+{
+ m11 = x;
+ m12 = y;
+ m13 = z;
+ m14 = w;
+}
+
+inline void Matrix3x4::SetRow2 ( double x, double y, double z, double w )
+{
+ m21 = x;
+ m22 = y;
+ m23 = z;
+ m24 = w;
+}
+
+inline void Matrix3x4::SetRow3 ( double x, double y, double z, double w )
+{
+ m31 = x;
+ m32 = y;
+ m33 = z;
+ m34 = w;
+}
+
+// Left multiply with a translation (so the translation is applied afterwards).
+inline Matrix3x4& Matrix3x4::ApplyTranslationLeft( const VectorR3& u )
+{
+ m14 += u.x;
+ m24 += u.y;
+ m34 += u.z;
+ return *this;
+}
+
+// Right multiply with a translation (so the translation is applied first).
+inline Matrix3x4& Matrix3x4::ApplyTranslationRight( const VectorR3& u )
+{
+ double new14 = m14 + m11*u.x + m12*u.y + m13*u.z;
+ double new24 = m24 + m21*u.x + m22*u.y + m23*u.z;
+ m34 = m34 + m31*u.x + m32*u.y + m33*u.z;
+ m14 = new14;
+ m24 = new24;
+ return *this;
+}
+
+// Left-multiply with a rotation around the y-axis.
+inline Matrix3x4& Matrix3x4::ApplyYRotationLeft( double theta )
+{
+ double costheta = cos(theta);
+ double sintheta = sin(theta);
+ return ApplyYRotationLeft( costheta, sintheta );
+}
+
+inline Matrix3x4& Matrix3x4::ApplyYRotationLeft( double costheta, double sintheta )
+{
+ double tmp;
+ tmp = costheta*m11+sintheta*m31;
+ m31 = costheta*m31-sintheta*m11;
+ m11 = tmp;
+
+ tmp = costheta*m12+sintheta*m32;
+ m32 = costheta*m32-sintheta*m12;
+ m12 = tmp;
+
+ tmp = costheta*m13+sintheta*m33;
+ m33 = costheta*m33-sintheta*m13;
+ m13 = tmp;
+
+ tmp = costheta*m14+sintheta*m34;
+ m34 = costheta*m34-sintheta*m14;
+ m14 = tmp;
+
+ return *this;
+}
+
+inline VectorR3 Matrix3x4::Solve(const VectorR3& u) const // Returns solution
+{
+ Matrix3x3 A;
+ A.Set3x3(*this);
+ return ( A.Solve( VectorR3(m14-u.x, m24-u.y, m34-u.z) ) );
+}
+
+inline void Matrix3x4::Transform( VectorR3* u ) const {
+ double newX, newY;
+ newX = m11*u->x + m12*u->y + m13*u->z + m14;
+ newY = m21*u->x + m22*u->y + m23*u->z + m24;
+ u->z = m31*u->x + m32*u->y + m33*u->z + m34;
+ u->x = newX;
+ u->y = newY;
+}
+
+inline void Matrix3x4::Transform3x3( VectorR3* u ) const {
+ double newX, newY;
+ newX = m11*u->x + m12*u->y + m13*u->z;
+ newY = m21*u->x + m22*u->y + m23*u->z;
+ u->z = m31*u->x + m32*u->y + m33*u->z;
+ u->x = newX;
+ u->y = newY;
+}
+
+inline void Matrix3x4::Transform( const VectorR3& src, VectorR3* dest ) const {
+ dest->x = m11*src.x + m12*src.y + m13*src.z + m14;
+ dest->y = m21*src.x + m22*src.y + m23*src.z + m24;
+ dest->z = m31*src.x + m32*src.y + m33*src.z + m34;
+}
+
+inline void Matrix3x4::Transform3x3( const VectorR3& src, VectorR3* dest ) const {
+ dest->x = m11*src.x + m12*src.y + m13*src.z;
+ dest->y = m21*src.x + m22*src.y + m23*src.z;
+ dest->z = m31*src.x + m32*src.y + m33*src.z;
+}
+
+inline void Matrix3x4::Transform3x3Transpose( VectorR3* u ) const {
+ double newX, newY;
+ newX = m11*u->x + m21*u->y + m31*u->z;
+ newY = m12*u->x + m22*u->y + m32*u->z;
+ u->z = m13*u->x + m23*u->y + m33*u->z;
+ u->x = newX;
+ u->y = newY;
+}
+
+inline void Matrix3x4::Transform3x3Transpose( const VectorR3& src, VectorR3* dest ) const {
+ dest->x = m11*src.x + m21*src.y + m31*src.z;
+ dest->y = m12*src.x + m22*src.y + m32*src.z;
+ dest->z = m13*src.x + m23*src.y + m33*src.z;
+}
+
+inline VectorR3 operator* ( const Matrix3x4& A, const VectorR3& u )
+{
+ return( VectorR3( A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14,
+ A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24,
+ A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34) );
+}
+
+
+// ******************************************************
+// * LinearMapR3 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline LinearMapR3::LinearMapR3()
+{
+ SetZero();
+ return;
+}
+
+inline LinearMapR3::LinearMapR3( const VectorR3& u, const VectorR3& v,
+ const VectorR3& s )
+:Matrix3x3 ( u, v, s )
+{ }
+
+inline LinearMapR3::LinearMapR3(
+ double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33)
+ // Values specified in column order!!!
+:Matrix3x3 ( a11, a21, a31, a12, a22, a32, a13, a23, a33)
+{ }
+
+inline LinearMapR3::LinearMapR3 ( const Matrix3x3& A )
+: Matrix3x3 (A)
+{}
+
+inline void LinearMapR3::SetZero( )
+{
+ Matrix3x3::SetZero();
+}
+
+inline void LinearMapR3::Negate()
+{
+ m11 = -m11; // Row 1
+ m12 = -m12;
+ m13 = -m13;
+ m21 = -m21; // Row 2
+ m22 = -m22;
+ m23 = -m23;
+ m31 = -m31; // Row 3
+ m32 = -m32;
+ m33 = -m33;
+}
+
+
+inline LinearMapR3& LinearMapR3::operator+= (const Matrix3x3& B)
+{
+ m11 += B.m11;
+ m12 += B.m12;
+ m13 += B.m13;
+ m21 += B.m21;
+ m22 += B.m22;
+ m23 += B.m23;
+ m31 += B.m31;
+ m32 += B.m32;
+ m33 += B.m33;
+ return ( *this );
+}
+
+inline LinearMapR3& LinearMapR3::operator-= (const Matrix3x3& B)
+{
+ m11 -= B.m11;
+ m12 -= B.m12;
+ m13 -= B.m13;
+ m21 -= B.m21;
+ m22 -= B.m22;
+ m23 -= B.m23;
+ m31 -= B.m31;
+ m32 -= B.m32;
+ m33 -= B.m33;
+ return( *this );
+}
+
+inline LinearMapR3 operator+ (const LinearMapR3& A, const Matrix3x3& B)
+{
+ return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
+ A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
+ A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) );
+}
+
+inline LinearMapR3 operator+ (const Matrix3x3& A, const LinearMapR3& B)
+{
+ return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
+ A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
+ A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) );
+}
+
+inline LinearMapR3 operator- (const LinearMapR3& A)
+{
+ return( LinearMapR3( -A.m11, -A.m21, -A.m31,
+ -A.m12, -A.m22, -A.m32,
+ -A.m13, -A.m23, -A.m33 ) );
+}
+
+inline LinearMapR3 operator- (const Matrix3x3& A, const LinearMapR3& B)
+{
+ return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
+ A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
+ A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) );
+}
+
+inline LinearMapR3 operator- (const LinearMapR3& A, const Matrix3x3& B)
+{
+ return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
+ A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
+ A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) );
+}
+
+inline LinearMapR3& LinearMapR3::operator*= (register double b)
+{
+ m11 *= b;
+ m12 *= b;
+ m13 *= b;
+ m21 *= b;
+ m22 *= b;
+ m23 *= b;
+ m31 *= b;
+ m32 *= b;
+ m33 *= b;
+ return ( *this);
+}
+
+inline LinearMapR3 operator* ( const LinearMapR3& A, register double b)
+{
+ return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b,
+ A.m12*b, A.m22*b, A.m32*b,
+ A.m13*b, A.m23*b, A.m33*b ) );
+}
+
+inline LinearMapR3 operator* ( register double b, const LinearMapR3& A)
+{
+ return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b,
+ A.m12*b, A.m22*b, A.m32*b,
+ A.m13*b, A.m23*b, A.m33*b ) );
+}
+
+inline LinearMapR3 operator/ ( const LinearMapR3& A, double b)
+{
+ register double bInv = 1.0/b;
+ return( LinearMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv,
+ A.m12*bInv, A.m22*bInv, A.m32*bInv,
+ A.m13*bInv, A.m23*bInv, A.m33*bInv ) );
+}
+
+inline LinearMapR3& LinearMapR3::operator/= (register double b)
+{
+ register double bInv = 1.0/b;
+ return ( *this *= bInv );
+}
+
+inline LinearMapR3& LinearMapR3::operator*= (const Matrix3x3& B) // Matrix product
+{
+ OperatorTimesEquals( B );
+ return( *this );
+}
+
+inline VectorR3 LinearMapR3::Solve(const VectorR3& u) const // Returns solution
+{
+ return ( Matrix3x3::Solve( u ) );
+}
+
+inline LinearMapR3 LinearMapR3::Transpose() const // Returns the transpose
+{
+ return ( LinearMapR3 ( m11, m12, m13, m21, m22, m23, m31, m32, m33) );
+}
+
+// ******************************************************
+// * AffineMapR3 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline AffineMapR3::AffineMapR3( double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33,
+ double a14, double a24, double a34)
+{ // Values in COLUMN order!
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+}
+
+inline AffineMapR3::AffineMapR3 (const VectorR3& u, const VectorR3& v,
+ const VectorR3& w, const VectorR3& t)
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m13 = w.x; // Column 3
+ m23 = w.y;
+ m33 = w.z;
+ m14 = t.x; // Column 4
+ m24 = t.y;
+ m34 = t.z;
+}
+
+inline AffineMapR3::AffineMapR3 (const LinearMapR3& A, const VectorR3& t)
+{
+ m11 = A.m11;
+ m12 = A.m12;
+ m13 = A.m13;
+ m14 = t.x;
+ m21 = A.m21;
+ m22 = A.m22;
+ m23 = A.m23;
+ m24 = t.y;
+ m31 = A.m31;
+ m32 = A.m32;
+ m33 = A.m33;
+ m34 = t.z;
+
+}
+
+inline void AffineMapR3::SetIdentity ( )
+{
+ Matrix3x4::SetIdentity();
+}
+
+inline void AffineMapR3::SetZero( )
+{
+ Matrix3x4::SetZero();
+}
+
+inline VectorR3 AffineMapR3::Solve(const VectorR3& u) const // Returns solution
+{
+ return ( Matrix3x4::Solve( u ) );
+}
+
+
+inline AffineMapR3& AffineMapR3::operator+= (const Matrix3x4& B)
+{
+ m11 += B.m11;
+ m21 += B.m21;
+ m31 += B.m31;
+ m12 += B.m12;
+ m22 += B.m22;
+ m32 += B.m32;
+ m13 += B.m13;
+ m23 += B.m23;
+ m33 += B.m33;
+ m14 += B.m14;
+ m24 += B.m24;
+ m34 += B.m34;
+ return (*this);
+}
+
+inline AffineMapR3& AffineMapR3::operator-= (const Matrix3x4& B)
+{
+ m11 -= B.m11;
+ m21 -= B.m21;
+ m31 -= B.m31;
+ m12 -= B.m12;
+ m22 -= B.m22;
+ m32 -= B.m32;
+ m13 -= B.m13;
+ m23 -= B.m23;
+ m33 -= B.m33;
+ m14 -= B.m14;
+ m24 -= B.m24;
+ m34 -= B.m34;
+ return (*this);
+
+}
+
+inline AffineMapR3 operator+ (const AffineMapR3& A, const AffineMapR3& B)
+{
+ return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
+ A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
+ A.m13+B.m13, A.m23+B.m23, A.m33+B.m33,
+ A.m14+B.m14, A.m23+B.m24, A.m34+B.m34) );
+}
+
+inline AffineMapR3 operator+ (const AffineMapR3& A, const Matrix3x3& B)
+{
+ return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
+ A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
+ A.m13+B.m13, A.m23+B.m23, A.m33+B.m33,
+ A.m14, A.m23, A.m34 ) );
+}
+
+inline AffineMapR3 operator+ (const Matrix3x3& B, const AffineMapR3& A)
+{
+ return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
+ A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
+ A.m13+B.m13, A.m23+B.m23, A.m33+B.m33,
+ A.m14, A.m23, A.m34 ) );
+}
+
+inline AffineMapR3 operator- (const AffineMapR3& A, const AffineMapR3& B)
+{
+ return( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
+ A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
+ A.m13-B.m13, A.m23-B.m23, A.m33-B.m33,
+ A.m14-B.m14, A.m23-B.m24, A.m34-B.m34) );
+}
+
+inline AffineMapR3 operator- (const AffineMapR3& A, const LinearMapR3& B)
+{
+ return ( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
+ A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
+ A.m13-B.m13, A.m23-B.m23, A.m33-B.m33,
+ A.m14, A.m23, A.m34 ) );
+}
+
+inline AffineMapR3 operator- (const LinearMapR3& B, const AffineMapR3& A)
+{
+ return( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
+ A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
+ A.m13-B.m13, A.m23-B.m23, A.m33-B.m33,
+ A.m14, A.m23, A.m34 ) );
+}
+
+
+inline AffineMapR3& AffineMapR3::operator*= (register double b)
+{
+ m11 *= b;
+ m12 *= b;
+ m13 *= b;
+ m21 *= b;
+ m22 *= b;
+ m23 *= b;
+ m31 *= b;
+ m32 *= b;
+ m33 *= b;
+ m14 *= b;
+ m24 *= b;
+ m34 *= b;
+ return (*this);
+}
+
+inline AffineMapR3 operator* (const AffineMapR3& A, register double b)
+{
+ return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b,
+ A.m12*b, A.m22*b, A.m32*b,
+ A.m13*b, A.m23*b, A.m33*b,
+ A.m14*b, A.m24*b, A.m34*b ) );
+}
+
+inline AffineMapR3 operator* (register double b, const AffineMapR3& A)
+{
+ return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b,
+ A.m12*b, A.m22*b, A.m32*b,
+ A.m13*b, A.m23*b, A.m33*b,
+ A.m14*b, A.m24*b, A.m34*b ) );
+}
+
+inline AffineMapR3& AffineMapR3::operator/= (double b)
+{
+ register double bInv = 1.0/b;
+ *this *= bInv;
+ return( *this );
+}
+
+inline AffineMapR3 operator/ (const AffineMapR3& A, double b)
+{
+ register double bInv = 1.0/b;
+ return(AffineMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv,
+ A.m12*bInv, A.m22*bInv, A.m32*bInv,
+ A.m13*bInv, A.m23*bInv, A.m33*bInv,
+ A.m14*bInv, A.m24*bInv, A.m34*bInv ) );
+}
+
+inline AffineMapR3& AffineMapR3::operator*= (const Matrix3x3& B) // Composition
+{
+ OperatorTimesEquals( B );
+ return( *this );
+}
+
+inline AffineMapR3& AffineMapR3::operator*= (const Matrix3x4& B) // Composition
+{
+ OperatorTimesEquals( B );
+ return( *this );
+}
+
+// **************************************************************
+// RotationMapR3 class (inlined functions) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline RotationMapR3::RotationMapR3()
+{
+ SetIdentity();
+ return;
+}
+
+inline RotationMapR3::RotationMapR3( const VectorR3& u, const VectorR3& v,
+ const VectorR3& s )
+:Matrix3x3 ( u, v, s )
+{ }
+
+inline RotationMapR3::RotationMapR3(
+ double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33)
+ // Values specified in column order!!!
+:Matrix3x3 ( a11, a21, a31, a12, a22, a32, a13, a23, a33)
+{ }
+
+inline RotationMapR3 RotationMapR3::Inverse() const // Returns inverse
+{
+ return ( RotationMapR3 ( m11, m12, m13, // In column order!
+ m21, m22, m23,
+ m31, m32, m33 ) );
+}
+
+inline RotationMapR3& RotationMapR3::Invert() // Converts into inverse.
+{
+ register double temp;
+ temp = m12;
+ m12 = m21;
+ m21 = temp;
+ temp = m13;
+ m13 = m31;
+ m31 = temp;
+ temp = m23;
+ m23 = m32;
+ m32 = temp;
+ return ( *this );
+}
+
+inline VectorR3 RotationMapR3::Solve(const VectorR3& u) const // Returns solution
+{
+ return( VectorR3( m11*u.x + m21*u.y + m31*u.z,
+ m12*u.x + m22*u.y + m32*u.z,
+ m13*u.x + m23*u.y + m33*u.z ) );
+}
+
+inline RotationMapR3& RotationMapR3::operator*= (const RotationMapR3& B) // Matrix product
+{
+ OperatorTimesEquals( B );
+ return( *this );
+}
+
+
+// **************************************************************
+// RigidMapR3 class (inlined functions) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline RigidMapR3::RigidMapR3()
+{
+ SetIdentity();
+ return;
+}
+
+inline RigidMapR3::RigidMapR3( const VectorR3& u, const VectorR3& v,
+ const VectorR3& s, const VectorR3& t )
+:Matrix3x4 ( u, v, s, t )
+{}
+
+inline RigidMapR3::RigidMapR3(
+ double a11, double a21, double a31,
+ double a12, double a22, double a32,
+ double a13, double a23, double a33,
+ double a14, double a24, double a34)
+ // Values specified in column order!!!
+:Matrix3x4 ( a11, a21, a31, a12, a22, a32, a13, a23, a33, a14, a24, a34 )
+{}
+
+inline RigidMapR3::RigidMapR3( const Matrix3x3& A, const VectorR3& u ) // Set to RotationMap & Vector
+: Matrix3x4( A, u )
+{}
+
+
+inline RigidMapR3& RigidMapR3::Set( const Matrix3x3& A, const VectorR3& u ) // Set to RotationMap & Vector
+{
+ Matrix3x4::Set( A, u );
+ return *this;
+}
+
+inline RigidMapR3& RigidMapR3::SetTranslationPart( const VectorR3& u) // Set the translation part
+{
+ SetColumn4( u );
+ return *this;
+}
+
+inline RigidMapR3& RigidMapR3::SetTranslationPart( double x, double y, double z) // Set the translation part
+{
+ SetColumn4( x, y, z );
+ return *this;
+}
+
+inline RigidMapR3& RigidMapR3::SetRotationPart( const Matrix3x3& A) // Set the rotation part
+{
+ Matrix3x4::Set3x3( A );
+ return *this;
+}
+
+inline RigidMapR3& RigidMapR3::operator*= (const RotationMapR3& B) // Composition
+{
+ OperatorTimesEquals( B );
+ return( *this );
+}
+
+inline RigidMapR3& RigidMapR3::operator*= (const RigidMapR3& B) // Composition
+{
+ OperatorTimesEquals( B );
+ return( *this );
+}
+
+inline RigidMapR3 RigidMapR3::Inverse() const // Returns inverse
+{
+ double new14 = -(m11*m14 + m21*m24 + m31*m34);
+ double new24 = -(m12*m14 + m22*m24 + m32*m34);
+ double new34 = -(m13*m14 + m23*m24 + m33*m34);
+ return ( RigidMapR3 ( m11, m12, m13, // In column order!
+ m21, m22, m23,
+ m31, m32, m33,
+ new14, new24, new34 ) );
+}
+
+inline RigidMapR3& RigidMapR3::Invert() // Converts into inverse.
+{
+ double new14 = -(m11*m14 + m21*m24 + m31*m34);
+ double new24 = -(m12*m14 + m22*m24 + m32*m34);
+ m34 = -(m13*m14 + m23*m24 + m33*m34);
+ m14 = new14;
+ m24 = new24;
+
+ register double temp;
+ temp = m12;
+ m12 = m21;
+ m21 = temp;
+ temp = m13;
+ m13 = m31;
+ m31 = temp;
+ temp = m23;
+ m23 = m32;
+ m32 = temp;
+ return ( *this );
+}
+
+// ***************************************************************
+// * 3-space vector and matrix utilities (inlined functions) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+// Returns the projection of u onto unit v
+inline VectorR3 ProjectToUnit ( const VectorR3& u, const VectorR3& v)
+{
+ return (u^v)*v;
+}
+
+// Returns the projection of u onto the plane perpindicular to the unit vector v
+inline VectorR3 ProjectPerpUnit ( const VectorR3& u, const VectorR3& v)
+{
+ return ( u - ((u^v)*v) );
+}
+
+// Returns the projection of u onto the plane perpindicular to the unit vector v
+// This one is more stable when u and v are nearly equal.
+inline VectorR3 ProjectPerpUnitDiff ( const VectorR3& u, const VectorR3& v)
+{
+ VectorR3 ans = u;
+ ans -= v;
+ ans -= ((ans^v)*v);
+ return ans; // ans = (u-v) - ((u-v)^v)*v
+}
+
+// VectorProjectMap returns map projecting onto a given vector u.
+// u should be a unit vector (otherwise the returned map is
+// scaled according to the magnitude of u.
+inline LinearMapR3 VectorProjectMap( const VectorR3& u )
+{
+ double a = u.x*u.y;
+ double b = u.x*u.z;
+ double c = u.y*u.z;
+ return( LinearMapR3( u.x*u.x, a, b,
+ a, u.y*u.y, c,
+ b, c, u.z*u.z ) );
+}
+
+// PlaneProjectMap returns map projecting onto a given plane.
+// The plane is the plane orthognal to w.
+// w must be a unit vector (otherwise the returned map is
+// garbage).
+inline LinearMapR3 PlaneProjectMap ( const VectorR3& w )
+{
+ double a = -w.x*w.y;
+ double b = -w.x*w.z;
+ double c = -w.y*w.z;
+ return( LinearMapR3( 1.0-w.x*w.x, a, b,
+ a, 1.0-w.y*w.y, c,
+ b, c, 1.0-w.z*w.z ) );
+}
+
+// PlaneProjectMap returns map projecting onto a given plane.
+// The plane is the plane containing the two orthonormal vectors u,v.
+// If u, v are orthonormal, this is a projection with scaling.
+// If they are not orthonormal, the results are more difficult
+// to interpret.
+inline LinearMapR3 PlaneProjectMap ( const VectorR3& u, const VectorR3 &v )
+{
+ double a = u.x*u.y + v.x*v.y;
+ double b = u.x*u.z + v.x*v.z;
+ double c = u.y*u.z + v.y*v.z;
+ return( LinearMapR3( u.x*u.x+v.x*v.x, a, b,
+ a, u.y*u.y+u.y*u.y, c,
+ b, c, u.z*u.z+v.z*v.z ) );
+}
+
+// Returns the solid angle between unit vectors v and w.
+inline double SolidAngle( const VectorR3& v, const VectorR3& w)
+{
+ return atan2 ( (v*w).Norm(), v^w );
+}
+
+
+#endif
+
+// ******************* End of header material ********************
diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.cpp b/examples/ThirdPartyLibs/BussIK/LinearR4.cpp
new file mode 100644
index 000000000..f662a7d90
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR4.cpp
@@ -0,0 +1,467 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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 "LinearR4.h"
+
+#include
+
+const VectorR4 VectorR4::Zero(0.0, 0.0, 0.0, 0.0);
+const VectorR4 VectorR4::UnitX( 1.0, 0.0, 0.0, 0.0);
+const VectorR4 VectorR4::UnitY( 0.0, 1.0, 0.0, 0.0);
+const VectorR4 VectorR4::UnitZ( 0.0, 0.0, 1.0, 0.0);
+const VectorR4 VectorR4::UnitW( 0.0, 0.0, 0.0, 1.0);
+const VectorR4 VectorR4::NegUnitX(-1.0, 0.0, 0.0, 0.0);
+const VectorR4 VectorR4::NegUnitY( 0.0,-1.0, 0.0, 0.0);
+const VectorR4 VectorR4::NegUnitZ( 0.0, 0.0,-1.0, 0.0);
+const VectorR4 VectorR4::NegUnitW( 0.0, 0.0, 0.0,-1.0);
+
+const Matrix4x4 Matrix4x4::Identity(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+ 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
+
+
+// ******************************************************
+// * VectorR4 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+double VectorR4::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;
+ if ( w>m ) m=w;
+ else if ( -w>m ) m = -w;
+ return m;
+}
+
+// ******************************************************
+// * Matrix4x4 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+
+void Matrix4x4::operator*= (const Matrix4x4& B) // Matrix product
+{
+ double t1, t2, t3; // temporary values
+ t1 = m11*B.m11 + m12*B.m21 + m13*B.m31 + m14*B.m41;
+ t2 = m11*B.m12 + m12*B.m22 + m13*B.m32 + m14*B.m42;
+ t3 = m11*B.m13 + m12*B.m23 + m13*B.m33 + m14*B.m43;
+ m14 = m11*B.m14 + m12*B.m24 + m13*B.m34 + m14*B.m44;
+ m11 = t1;
+ m12 = t2;
+ m13 = t3;
+
+ t1 = m21*B.m11 + m22*B.m21 + m23*B.m31 + m24*B.m41;
+ t2 = m21*B.m12 + m22*B.m22 + m23*B.m32 + m24*B.m42;
+ t3 = m21*B.m13 + m22*B.m23 + m23*B.m33 + m24*B.m43;
+ m24 = m21*B.m14 + m22*B.m24 + m23*B.m34 + m24*B.m44;
+ m21 = t1;
+ m22 = t2;
+ m23 = t3;
+
+ t1 = m31*B.m11 + m32*B.m21 + m33*B.m31 + m34*B.m41;
+ t2 = m31*B.m12 + m32*B.m22 + m33*B.m32 + m34*B.m42;
+ t3 = m31*B.m13 + m32*B.m23 + m33*B.m33 + m34*B.m43;
+ m34 = m31*B.m14 + m32*B.m24 + m33*B.m34 + m34*B.m44;
+ m31 = t1;
+ m32 = t2;
+ m33 = t3;
+
+ t1 = m41*B.m11 + m42*B.m21 + m43*B.m31 + m44*B.m41;
+ t2 = m41*B.m12 + m42*B.m22 + m43*B.m32 + m44*B.m42;
+ t3 = m41*B.m13 + m42*B.m23 + m43*B.m33 + m44*B.m43;
+ m44 = m41*B.m14 + m42*B.m24 + m43*B.m34 + m44*B.m44;
+ m41 = t1;
+ m42 = t2;
+ m43 = t3;
+}
+
+inline void ReNormalizeHelper ( double &a, double &b, double &c, double &d )
+{
+ register double scaleF = a*a+b*b+c*c+d*d; // Inner product of Vector-R4
+ scaleF = 1.0-0.5*(scaleF-1.0);
+ a *= scaleF;
+ b *= scaleF;
+ c *= scaleF;
+ d *= scaleF;
+}
+
+Matrix4x4& Matrix4x4::ReNormalize() {
+ ReNormalizeHelper( m11, m21, m31, m41 ); // Renormalize first column
+ ReNormalizeHelper( m12, m22, m32, m42 ); // Renormalize second column
+ ReNormalizeHelper( m13, m23, m33, m43 ); // Renormalize third column
+ ReNormalizeHelper( m14, m24, m34, m44 ); // Renormalize fourth column
+ double alpha = 0.5*(m11*m12 + m21*m22 + m31*m32 + m41*m42); //1st and 2nd cols
+ double beta = 0.5*(m11*m13 + m21*m23 + m31*m33 + m41*m43); //1st and 3rd cols
+ double gamma = 0.5*(m11*m14 + m21*m24 + m31*m34 + m41*m44); //1st and 4nd cols
+ double delta = 0.5*(m12*m13 + m22*m23 + m32*m33 + m42*m43); //2nd and 3rd cols
+ double eps = 0.5*(m12*m14 + m22*m24 + m32*m34 + m42*m44); //2nd and 4nd cols
+ double phi = 0.5*(m13*m14 + m23*m24 + m33*m34 + m43*m44); //3rd and 4nd cols
+ double temp1, temp2, temp3;
+ temp1 = m11 - alpha*m12 - beta*m13 - gamma*m14;
+ temp2 = m12 - alpha*m11 - delta*m13 - eps*m14;
+ temp3 = m13 - beta*m11 - delta*m12 - phi*m14;
+ m14 -= (gamma*m11 + eps*m12 + phi*m13);
+ m11 = temp1;
+ m12 = temp2;
+ m13 = temp3;
+ temp1 = m21 - alpha*m22 - beta*m23 - gamma*m24;
+ temp2 = m22 - alpha*m21 - delta*m23 - eps*m24;
+ temp3 = m23 - beta*m21 - delta*m22 - phi*m24;
+ m24 -= (gamma*m21 + eps*m22 + phi*m23);
+ m21 = temp1;
+ m22 = temp2;
+ m23 = temp3;
+ temp1 = m31 - alpha*m32 - beta*m33 - gamma*m34;
+ temp2 = m32 - alpha*m31 - delta*m33 - eps*m34;
+ temp3 = m33 - beta*m31 - delta*m32 - phi*m34;
+ m34 -= (gamma*m31 + eps*m32 + phi*m33);
+ m31 = temp1;
+ m32 = temp2;
+ m33 = temp3;
+ temp1 = m41 - alpha*m42 - beta*m43 - gamma*m44;
+ temp2 = m42 - alpha*m41 - delta*m43 - eps*m44;
+ temp3 = m43 - beta*m41 - delta*m42 - phi*m44;
+ m44 -= (gamma*m41 + eps*m42 + phi*m43);
+ m41 = temp1;
+ m42 = temp2;
+ m43 = temp3;
+ return *this;
+}
+
+// ******************************************************
+// * LinearMapR4 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+
+double LinearMapR4::Determinant () const // Returns the determinant
+{
+ double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants
+ double Tbt34C13 = m31*m43-m33*m41;
+ double Tbt34C14 = m31*m44-m34*m41;
+ double Tbt34C23 = m32*m43-m33*m42;
+ double Tbt34C24 = m32*m44-m34*m42;
+ double Tbt34C34 = m33*m44-m34*m43;
+
+ double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants
+ double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
+ double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
+ double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
+
+ return ( m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14 );
+}
+
+LinearMapR4 LinearMapR4::Inverse() const // Returns inverse
+{
+
+ double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants
+ double Tbt34C13 = m31*m43-m33*m41;
+ double Tbt34C14 = m31*m44-m34*m41;
+ double Tbt34C23 = m32*m43-m33*m42;
+ double Tbt34C24 = m32*m44-m34*m42;
+ double Tbt34C34 = m33*m44-m34*m43;
+ double Tbt24C12 = m21*m42-m22*m41; // 2x2 subdeterminants
+ double Tbt24C13 = m21*m43-m23*m41;
+ double Tbt24C14 = m21*m44-m24*m41;
+ double Tbt24C23 = m22*m43-m23*m42;
+ double Tbt24C24 = m22*m44-m24*m42;
+ double Tbt24C34 = m23*m44-m24*m43;
+ double Tbt23C12 = m21*m32-m22*m31; // 2x2 subdeterminants
+ double Tbt23C13 = m21*m33-m23*m31;
+ double Tbt23C14 = m21*m34-m24*m31;
+ double Tbt23C23 = m22*m33-m23*m32;
+ double Tbt23C24 = m22*m34-m24*m32;
+ double Tbt23C34 = m23*m34-m24*m33;
+
+ double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants
+ double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
+ double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
+ double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
+ double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23; // 3x3 subdeterminants
+ double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
+ double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
+ double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
+ double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23; // 3x3 subdeterminants
+ double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
+ double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
+ double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
+ double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23; // 3x3 subdeterminants
+ double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
+ double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
+ double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;
+
+
+ register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14);
+
+ return( LinearMapR4( sd11*detInv, -sd12*detInv, sd13*detInv, -sd14*detInv,
+ -sd21*detInv, sd22*detInv, -sd23*detInv, sd24*detInv,
+ sd31*detInv, -sd32*detInv, sd33*detInv, -sd34*detInv,
+ -sd41*detInv, sd42*detInv, -sd43*detInv, sd44*detInv ) );
+}
+
+LinearMapR4& LinearMapR4::Invert() // Converts into inverse.
+{
+ double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants
+ double Tbt34C13 = m31*m43-m33*m41;
+ double Tbt34C14 = m31*m44-m34*m41;
+ double Tbt34C23 = m32*m43-m33*m42;
+ double Tbt34C24 = m32*m44-m34*m42;
+ double Tbt34C34 = m33*m44-m34*m43;
+ double Tbt24C12 = m21*m42-m22*m41; // 2x2 subdeterminants
+ double Tbt24C13 = m21*m43-m23*m41;
+ double Tbt24C14 = m21*m44-m24*m41;
+ double Tbt24C23 = m22*m43-m23*m42;
+ double Tbt24C24 = m22*m44-m24*m42;
+ double Tbt24C34 = m23*m44-m24*m43;
+ double Tbt23C12 = m21*m32-m22*m31; // 2x2 subdeterminants
+ double Tbt23C13 = m21*m33-m23*m31;
+ double Tbt23C14 = m21*m34-m24*m31;
+ double Tbt23C23 = m22*m33-m23*m32;
+ double Tbt23C24 = m22*m34-m24*m32;
+ double Tbt23C34 = m23*m34-m24*m33;
+
+ double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants
+ double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
+ double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
+ double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
+ double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23; // 3x3 subdeterminants
+ double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
+ double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
+ double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
+ double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23; // 3x3 subdeterminants
+ double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
+ double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
+ double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
+ double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23; // 3x3 subdeterminants
+ double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
+ double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
+ double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;
+
+ register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14);
+
+ m11 = sd11*detInv;
+ m12 = -sd21*detInv;
+ m13 = sd31*detInv;
+ m14 = -sd41*detInv;
+ m21 = -sd12*detInv;
+ m22 = sd22*detInv;
+ m23 = -sd32*detInv;
+ m24 = sd42*detInv;
+ m31 = sd13*detInv;
+ m32 = -sd23*detInv;
+ m33 = sd33*detInv;
+ m34 = -sd43*detInv;
+ m41 = -sd14*detInv;
+ m42 = sd24*detInv;
+ m43 = -sd34*detInv;
+ m44 = sd44*detInv;
+
+ return ( *this );
+}
+
+VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution
+{
+ // Just uses Inverse() for now.
+ return ( Inverse()*u );
+}
+
+// ******************************************************
+// * RotationMapR4 class - math library functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+
+// ***************************************************************
+// * 4-space vector and matrix utilities *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+// Returns u * v^T
+LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v)
+{
+ LinearMapR4 result;
+ TimesTranspose( u, v, result );
+ return result;
+}
+
+// The following routines are use to obtain
+// a righthanded orthonormal basis to complement vectors u,v,w.
+// The vectors u,v,w must be unit and orthonormal.
+// The value is returned in "rotmat" with the first column(s) of
+// rotmat equal to u,v,w as appropriate.
+
+void GetOrtho( const VectorR4& u, RotationMapR4& rotmat )
+{
+ rotmat.SetColumn1(u);
+ GetOrtho( 1, rotmat );
+}
+
+void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat )
+{
+ rotmat.SetColumn1(u);
+ rotmat.SetColumn2(v);
+ GetOrtho( 2, rotmat );
+}
+
+void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& s,
+ RotationMapR4& rotmat )
+{
+ rotmat.SetColumn1(u);
+ rotmat.SetColumn2(v);
+ rotmat.SetColumn3(s);
+ GetOrtho( 3, rotmat );
+}
+
+// This final version of GetOrtho is mainly for internal use.
+// It uses a Gram-Schmidt procedure to extend a partial orthonormal
+// basis to a complete orthonormal basis.
+// j = number of columns of rotmat that have already been set.
+void GetOrtho( int j, RotationMapR4& rotmat)
+{
+ if ( j==0 ) {
+ rotmat.SetIdentity();
+ return;
+ }
+ if ( j==1 ) {
+ rotmat.SetColumn2( -rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31 );
+ j = 2;
+ }
+
+ assert ( rotmat.Column1().Norm()<1.0001 && 0.9999 -0.001 );
+
+ // 2x2 subdeterminants in first 2 columns
+
+ double d12 = rotmat.m11*rotmat.m22-rotmat.m12*rotmat.m21;
+ double d13 = rotmat.m11*rotmat.m32-rotmat.m12*rotmat.m31;
+ double d14 = rotmat.m11*rotmat.m42-rotmat.m12*rotmat.m41;
+ double d23 = rotmat.m21*rotmat.m32-rotmat.m22*rotmat.m31;
+ double d24 = rotmat.m21*rotmat.m42-rotmat.m22*rotmat.m41;
+ double d34 = rotmat.m31*rotmat.m42-rotmat.m32*rotmat.m41;
+ VectorR4 vec3;
+
+ if ( j==2 ) {
+ if ( d12>0.4 || d12<-0.4 || d13>0.4 || d13<-0.4
+ || d23>0.4 || d23<-0.4 ) {
+ vec3.Set( d23, -d13, d12, 0.0);
+ }
+ else if ( d24>0.4 || d24<-0.4 || d14>0.4 || d14<-0.4 ) {
+ vec3.Set( d24, -d14, 0.0, d12 );
+ }
+ else {
+ vec3.Set( d34, 0.0, -d14, d13 );
+ }
+ vec3.Normalize();
+ rotmat.SetColumn3(vec3);
+ }
+
+ // Do the final column
+
+ rotmat.SetColumn4 (
+ -rotmat.m23*d34 + rotmat.m33*d24 - rotmat.m43*d23,
+ rotmat.m13*d34 - rotmat.m33*d14 + rotmat.m43*d13,
+ -rotmat.m13*d24 + rotmat.m23*d14 - rotmat.m43*d12,
+ rotmat.m13*d23 - rotmat.m23*d13 + rotmat.m33*d12 );
+
+ assert ( 0.99 < (((LinearMapR4)rotmat)).Determinant()
+ && (((LinearMapR4)rotmat)).Determinant() < 1.01 );
+
+}
+
+
+
+// *********************************************************************
+// Rotation routines *
+// *********************************************************************
+
+// 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.
+VectorR4& VectorR4::RotateUnitInDirection ( const VectorR4& dir)
+{
+ assert ( this->Norm()<1.0001 && this->Norm()>0.9999 &&
+ (dir^(*this))<0.0001 && (dir^(*this))>-0.0001 );
+
+ double theta = dir.NormSq();
+ if ( theta==0.0 ) {
+ return *this;
+ }
+ else {
+ theta = sqrt(theta);
+ double costheta = cos(theta);
+ double sintheta = sin(theta);
+ VectorR4 dirUnit = dir/theta;
+ *this = costheta*(*this) + sintheta*dirUnit;
+ // this->NormalizeFast();
+ return ( *this );
+ }
+}
+
+// RotateToMap returns a RotationMapR4 that rotates fromVec to toVec,
+// leaving the orthogonal subspace fixed.
+// fromVec and toVec should be unit vectors
+RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec)
+{
+ LinearMapR4 result;
+ result.SetIdentity();
+ LinearMapR4 temp;
+ VectorR4 vPerp = ProjectPerpUnitDiff( toVec, fromVec );
+ double sintheta = vPerp.Norm(); // theta = angle between toVec and fromVec
+ VectorR4 vProj = toVec-vPerp;
+ double costheta = vProj^fromVec;
+ if ( sintheta == 0.0 ) {
+ // The vectors either coincide (return identity) or directly oppose
+ if ( costheta < 0.0 ) {
+ result = -result; // Vectors directly oppose: return -identity.
+ }
+ }
+ else {
+ vPerp /= sintheta; // Normalize
+ VectorProjectMap ( fromVec, temp ); // project in fromVec direction
+ temp *= (costheta-1.0);
+ result += temp;
+ VectorProjectMap ( vPerp, temp ); // Project in vPerp direction
+ temp *= (costheta-1.0);
+ result += temp;
+ TimesTranspose ( vPerp, fromVec, temp ); // temp = (vPerp)*(fromVec^T)
+ temp *= sintheta;
+ result += temp;
+ temp.MakeTranspose();
+ result -= temp; // (-sintheta)*(fromVec)*(vPerp^T)
+ }
+ RotationMapR4 rotationResult;
+ rotationResult.Set(result); // Make explicitly a RotationMapR4
+ return rotationResult;
+}
+
+
+// ***************************************************************
+// Stream Output Routines *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+ostream& operator<< ( ostream& os, const VectorR4& u )
+{
+ return (os << "<" << u.x << "," << u.y << "," << u.z << "," << u.w << ">");
+}
+
+
diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h
new file mode 100644
index 000000000..51f57a666
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h
@@ -0,0 +1,1099 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+//
+// Linear Algebra Classes over R4
+//
+//
+// A. Vector and Position classes
+//
+// A.1. VectorR4: a column vector of length 4
+//
+// B. Matrix Classes
+//
+// B.1 LinearMapR4 - arbitrary linear map; 4x4 real matrix
+//
+// B.2 RotationMapR4 - orthonormal 4x4 matrix
+//
+
+#ifndef LINEAR_R4_H
+#define LINEAR_R4_H
+
+#include
+#include
+#include
+#include "LinearR3.h"
+using namespace std;
+
+class VectorR4; // R4 Vector
+class LinearMapR4; // 4x4 real matrix
+class RotationMapR4; // 4x4 rotation map
+
+// **************************************
+// VectorR4 class *
+// * * * * * * * * * * * * * * * * * * **
+
+class VectorR4 {
+
+public:
+ double x, y, z, w; // The x & y & z & w coordinates.
+
+ static const VectorR4 Zero;
+ static const VectorR4 UnitX;
+ static const VectorR4 UnitY;
+ static const VectorR4 UnitZ;
+ static const VectorR4 UnitW;
+ static const VectorR4 NegUnitX;
+ static const VectorR4 NegUnitY;
+ static const VectorR4 NegUnitZ;
+ static const VectorR4 NegUnitW;
+
+public:
+ VectorR4( ) : x(0.0), y(0.0), z(0.0), w(0.0) {}
+ VectorR4( double xVal, double yVal, double zVal, double wVal )
+ : x(xVal), y(yVal), z(zVal), w(wVal) {}
+ VectorR4( const Quaternion& q); // Definition with Quaternion routines
+
+ VectorR4& SetZero() { x=0.0; y=0.0; z=0.0; w=0.0; return *this;}
+ VectorR4& Set( double xx, double yy, double zz, double ww )
+ { x=xx; y=yy; z=zz; w=ww; return *this;}
+ VectorR4& Set ( const Quaternion& ); // Defined with Quaternion
+ VectorR4& Set ( const VectorHgR3& h ) {x=h.x; y=h.y; z=h.z; w=h.w; return *this; }
+ VectorR4& Load( const double* v );
+ VectorR4& Load( const float* v );
+ void Dump( double* v ) const;
+ void Dump( float* v ) const;
+
+ VectorR4& operator+= ( const VectorR4& v )
+ { x+=v.x; y+=v.y; z+=v.z; w+=v.w; return(*this); }
+ VectorR4& operator-= ( const VectorR4& v )
+ { x-=v.x; y-=v.y; z-=v.z; w-=v.w; return(*this); }
+ VectorR4& operator*= ( double m )
+ { x*=m; y*=m; z*=m; w*=m; return(*this); }
+ VectorR4& operator/= ( double m )
+ { register double mInv = 1.0/m;
+ x*=mInv; y*=mInv; z*=mInv; w*=mInv;
+ return(*this); }
+ VectorR4 operator- () const { return ( VectorR4(-x, -y, -z, -w) ); }
+ VectorR4& ArrayProd(const VectorR4&); // Component-wise product
+ VectorR4& ArrayProd3(const VectorR3&); // Component-wise product
+
+ VectorR4& AddScaled( const VectorR4& u, double s );
+
+ double Norm() const { return ( (double)sqrt( x*x + y*y + z*z +w*w) ); }
+ double NormSq() const { return ( x*x + y*y + z*z + w*w ); }
+ double Dist( const VectorR4& u ) const; // Distance from u
+ double DistSq( const VectorR4& u ) const; // Distance from u
+ double MaxAbs() const;
+ VectorR4& Normalize () { *this /= Norm(); return *this; } // No error checking
+ inline VectorR4& MakeUnit(); // Normalize() with error checking
+ inline VectorR4& ReNormalize();
+ bool IsUnit( ) const
+ { register double norm = Norm();
+ return ( 1.000001>=norm && norm>=0.999999 ); }
+ bool IsUnit( double tolerance ) const
+ { register double norm = Norm();
+ return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); }
+ bool IsZero() const { return ( x==0.0 && y==0.0 && z==0.0 && w==0.0); }
+ bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );}
+ // tolerance should be non-negative
+
+ VectorR4& RotateUnitInDirection ( const VectorR4& dir); // rotate in direction dir
+
+};
+
+inline VectorR4 operator+( const VectorR4& u, const VectorR4& v );
+inline VectorR4 operator-( const VectorR4& u, const VectorR4& v );
+inline VectorR4 operator*( const VectorR4& u, double m);
+inline VectorR4 operator*( double m, const VectorR4& u);
+inline VectorR4 operator/( const VectorR4& u, double m);
+inline int operator==( const VectorR4& u, const VectorR4& v );
+
+inline double operator^ (const VectorR4& u, const VectorR4& v ); // Dot Product
+inline VectorR4 ArrayProd(const VectorR4& u, const VectorR4& v );
+
+inline double Mag(const VectorR4& u) { return u.Norm(); }
+inline double Dist(const VectorR4& u, const VectorR4& v) { return u.Dist(v); }
+inline double DistSq(const VectorR4& u, const VectorR4& v) { return u.DistSq(v); }
+inline double NormalizeError (const VectorR4& u);
+
+// ********************************************************************
+// Matrix4x4 - base class for 4x4 matrices *
+// * * * * * * * * * * * * * * * * * * * * * **************************
+
+class Matrix4x4 {
+
+public:
+ double m11, m12, m13, m14, m21, m22, m23, m24,
+ m31, m32, m33, m34, m41, m42, m43, m44;
+
+ // Implements a 4x4 matrix: m_i_j - row-i and column-j entry
+
+ static const Matrix4x4 Identity;
+
+
+public:
+
+ Matrix4x4();
+ Matrix4x4( const VectorR4&, const VectorR4&,
+ const VectorR4&, const VectorR4& ); // Sets by columns!
+ Matrix4x4( double, double, double, double,
+ double, double, double, double,
+ double, double, double, double,
+ double, double, double, double ); // Sets by columns
+
+ inline void SetIdentity (); // Set to the identity map
+ inline void SetZero (); // Set to the zero map
+ inline void Set ( const Matrix4x4& ); // Set to the matrix.
+ inline void Set( const VectorR4&, const VectorR4&,
+ const VectorR4&, const VectorR4& );
+ inline void Set( double, double, double, double,
+ double, double, double, double,
+ double, double, double, double,
+ double, double, double, double );
+ inline void SetByRows( const VectorR4&, const VectorR4&,
+ const VectorR4&, const VectorR4& );
+ inline void SetByRows( double, double, double, double,
+ double, double, double, double,
+ double, double, double, double,
+ double, double, double, double );
+ inline void SetColumn1 ( double, double, double, double );
+ inline void SetColumn2 ( double, double, double, double );
+ inline void SetColumn3 ( double, double, double, double );
+ inline void SetColumn4 ( double, double, double, double );
+ inline void SetColumn1 ( const VectorR4& );
+ inline void SetColumn2 ( const VectorR4& );
+ inline void SetColumn3 ( const VectorR4& );
+ inline void SetColumn4 ( const VectorR4& );
+ inline VectorR4 Column1() const;
+ inline VectorR4 Column2() const;
+ inline VectorR4 Column3() const;
+ inline VectorR4 Column4() const;
+
+ inline void SetDiagonal( double, double, double, double );
+ inline void SetDiagonal( const VectorR4& );
+ inline double Diagonal( int );
+
+ inline void MakeTranspose(); // Transposes it.
+ void operator*= (const Matrix4x4& B); // Matrix product
+
+ Matrix4x4& ReNormalize();
+};
+
+inline VectorR4 operator* ( const Matrix4x4&, const VectorR4& );
+
+ostream& operator<< ( ostream& os, const Matrix4x4& A );
+
+
+// *****************************************
+// LinearMapR4 class *
+// * * * * * * * * * * * * * * * * * * * * *
+
+class LinearMapR4 : public Matrix4x4 {
+
+public:
+
+ LinearMapR4();
+ LinearMapR4( const VectorR4&, const VectorR4&,
+ const VectorR4&, const VectorR4& ); // Sets by columns!
+ LinearMapR4( double, double, double, double,
+ double, double, double, double,
+ double, double, double, double,
+ double, double, double, double ); // Sets by columns
+ LinearMapR4 ( const Matrix4x4& );
+
+ inline LinearMapR4& operator+= (const LinearMapR4& );
+ inline LinearMapR4& operator-= (const LinearMapR4& );
+ inline LinearMapR4& operator*= (double);
+ inline LinearMapR4& operator/= (double);
+ inline LinearMapR4& operator*= (const Matrix4x4& ); // Matrix product
+
+ inline LinearMapR4 Transpose() const;
+ double Determinant () const; // Returns the determinant
+ LinearMapR4 Inverse() const; // Returns inverse
+ LinearMapR4& Invert(); // Converts into inverse.
+ VectorR4 Solve(const VectorR4&) const; // Returns solution
+ LinearMapR4 PseudoInverse() const; // Returns pseudo-inverse TO DO
+ VectorR4 PseudoSolve(const VectorR4&); // Finds least squares solution TO DO
+};
+
+inline LinearMapR4 operator+ (const LinearMapR4&, const LinearMapR4&);
+inline LinearMapR4 operator- (const LinearMapR4&);
+inline LinearMapR4 operator- (const LinearMapR4&, const LinearMapR4&);
+inline LinearMapR4 operator* ( const LinearMapR4&, double);
+inline LinearMapR4 operator* ( double, const LinearMapR4& );
+inline LinearMapR4 operator/ ( const LinearMapR4&, double );
+inline LinearMapR4 operator* ( const Matrix4x4&, const LinearMapR4& );
+inline LinearMapR4 operator* ( const LinearMapR4&, const Matrix4x4& );
+ // Matrix product (composition)
+
+
+// *******************************************
+// RotationMapR4 class *
+// * * * * * * * * * * * * * * * * * * * * * *
+
+class RotationMapR4 : public Matrix4x4 {
+
+public:
+
+ RotationMapR4();
+ RotationMapR4( const VectorR4&, const VectorR4&,
+ const VectorR4&, const VectorR4& ); // Sets by columns!
+ RotationMapR4( double, double, double, double,
+ double, double, double, double,
+ double, double, double, double,
+ double, double, double, double ); // Sets by columns!
+
+ RotationMapR4& SetZero(); // IT IS AN ERROR TO USE THIS FUNCTION!
+
+ inline RotationMapR4& operator*= (const RotationMapR4& ); // Matrix product
+
+ inline RotationMapR4 Transpose() const;
+ inline RotationMapR4 Inverse() const { return Transpose(); }; // Returns the transpose
+ inline RotationMapR4& Invert() { MakeTranspose(); return *this; }; // Transposes it.
+ inline VectorR4 Invert(const VectorR4&) const; // Returns solution
+};
+
+inline RotationMapR4 operator* ( const RotationMapR4&, const RotationMapR4& );
+ // Matrix product (composition)
+
+
+// ***************************************************************
+// * 4-space vector and matrix utilities (prototypes) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+// Returns the angle between vectors u and v.
+// Use SolidAngleUnit if both vectors are unit vectors
+inline double SolidAngle( const VectorR4& u, const VectorR4& v);
+inline double SolidAngleUnit( const VectorR4 u, const VectorR4 v );
+
+// Returns a righthanded orthonormal basis to complement vectors u,v,w.
+// The vectors u,v,w must be unit and orthonormal.
+void GetOrtho( const VectorR4& u, RotationMapR4& rotmap );
+void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmap );
+void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& w,
+ RotationMapR4& rotmap );
+void GetOrtho( int j, RotationMapR4& rotmap); // Mainly for internal use
+
+// Projections
+
+inline VectorR4 ProjectToUnit ( const VectorR4& u, const VectorR4& v);
+ // Project u onto v
+inline VectorR4 ProjectPerpUnit ( const VectorR4& u, const VectorR4 & v);
+ // Project perp to v
+inline VectorR4 ProjectPerpUnitDiff ( const VectorR4& u, const VectorR4& v);
+// v must be a unit vector.
+
+// Returns the projection of u onto unit v
+inline VectorR4 ProjectToUnit ( const VectorR4& u, const VectorR4& v)
+{
+ return (u^v)*v;
+}
+
+// Returns the projection of u onto the plane perpindicular to the unit vector v
+inline VectorR4 ProjectPerpUnit ( const VectorR4& u, const VectorR4& v)
+{
+ return ( u - ((u^v)*v) );
+}
+
+// Returns the projection of u onto the plane perpindicular to the unit vector v
+// This one is more stable when u and v are nearly equal.
+inline VectorR4 ProjectPerpUnitDiff ( const VectorR4& u, const VectorR4& v)
+{
+ VectorR4 ans = u;
+ ans -= v;
+ ans -= ((ans^v)*v);
+ return ans; // ans = (u-v) - ((u-v)^v)*v
+}
+
+
+// Projection maps (LinearMapR4's)
+
+// VectorProjectMap returns map projecting onto a given vector u.
+// u should be a unit vector (otherwise the returned map is
+// scaled according to the magnitude of u.
+inline void VectorProjectMap( const VectorR4& u, LinearMapR4& M )
+{
+ double a = u.x*u.y;
+ double b = u.x*u.z;
+ double c = u.x*u.w;
+ double d = u.y*u.z;
+ double e = u.y*u.w;
+ double f = u.z*u.w;
+ M.Set( u.x*u.x, a, b, c,
+ a, u.y*u.y, d, e,
+ b, d, u.z*u.z, f,
+ c, e, f, u.w*u.w );
+}
+
+inline LinearMapR4 VectorProjectMap( const VectorR4& u )
+{
+ LinearMapR4 result;
+ VectorProjectMap( u, result );
+ return result;
+}
+
+inline LinearMapR4 PerpProjectMap ( const VectorR4& u );
+// u - must be unit vector.
+
+LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v); // u * v^T.
+inline void TimesTranspose( const VectorR4& u, const VectorR4& v, LinearMapR4& M);
+
+// Rotation Maps
+
+RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec);
+// fromVec and toVec should be unit vectors
+
+
+
+// ***************************************************************
+// * Stream Output Routines (Prototypes) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+ostream& operator<< ( ostream& os, const VectorR4& u );
+
+
+// *****************************************************
+// * VectorR4 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+inline VectorR4& VectorR4::Load( const double* v )
+{
+ x = *v;
+ y = *(v+1);
+ z = *(v+2);
+ w = *(v+3);
+ return *this;
+}
+
+inline VectorR4& VectorR4::Load( const float* v )
+{
+ x = *v;
+ y = *(v+1);
+ z = *(v+2);
+ w = *(v+3);
+ return *this;
+}
+
+inline void VectorR4::Dump( double* v ) const
+{
+ *v = x;
+ *(v+1) = y;
+ *(v+2) = z;
+ *(v+3) = w;
+}
+
+inline void VectorR4::Dump( float* v ) const
+{
+ *v = (float)x;
+ *(v+1) = (float)y;
+ *(v+2) = (float)z;
+ *(v+3) = (float)w;
+}
+
+inline VectorR4& VectorR4::MakeUnit () // Convert to unit vector (or leave zero).
+{
+ double nSq = NormSq();
+ if (nSq != 0.0) {
+ *this /= sqrt(nSq);
+ }
+ return *this;
+}
+
+inline VectorR4 operator+( const VectorR4& u, const VectorR4& v )
+{
+ return VectorR4(u.x+v.x, u.y+v.y, u.z+v.z, u.w+v.w );
+}
+inline VectorR4 operator-( const VectorR4& u, const VectorR4& v )
+{
+ return VectorR4(u.x-v.x, u.y-v.y, u.z-v.z, u.w-v.w);
+}
+inline VectorR4 operator*( const VectorR4& u, register double m)
+{
+ return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m );
+}
+inline VectorR4 operator*( register double m, const VectorR4& u)
+{
+ return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m );
+}
+inline VectorR4 operator/( const VectorR4& u, double m)
+{
+ register double mInv = 1.0/m;
+ return VectorR4( u.x*mInv, u.y*mInv, u.z*mInv, u.w*mInv );
+}
+
+inline int operator==( const VectorR4& u, const VectorR4& v )
+{
+ return ( u.x==v.x && u.y==v.y && u.z==v.z && u.w==v.w );
+}
+
+inline double operator^ ( const VectorR4& u, const VectorR4& v ) // Dot Product
+{
+ return ( u.x*v.x + u.y*v.y + u.z*v.z + u.w*v.w );
+}
+
+inline VectorR4 ArrayProd ( const VectorR4& u, const VectorR4& v )
+{
+ return ( VectorR4( u.x*v.x, u.y*v.y, u.z*v.z, u.w*v.w ) );
+}
+
+inline VectorR4& VectorR4::ArrayProd (const VectorR4& v) // Component-wise Product
+{
+ x *= v.x;
+ y *= v.y;
+ z *= v.z;
+ w *= v.w;
+ return ( *this );
+}
+
+inline VectorR4& VectorR4::ArrayProd3 (const VectorR3& v) // Component-wise Product
+{
+ x *= v.x;
+ y *= v.y;
+ z *= v.z;
+ return ( *this );
+}
+
+inline VectorR4& VectorR4::AddScaled( const VectorR4& u, double s )
+{
+ x += s*u.x;
+ y += s*u.y;
+ z += s*u.z;
+ w += s*u.w;
+ return(*this);
+}
+
+inline VectorR4& VectorR4::ReNormalize() // Convert near unit back to unit
+{
+ double nSq = NormSq();
+ register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor
+ *this *= mFact;
+ return *this;
+}
+
+inline double NormalizeError (const VectorR4& u)
+{
+ register double discrepancy;
+ discrepancy = u.x*u.x + u.y*u.y + u.z*u.z + u.w*u.w - 1.0;
+ if ( discrepancy < 0.0 ) {
+ discrepancy = -discrepancy;
+ }
+ return discrepancy;
+}
+
+inline VectorR3& VectorR3::SetFromHg(const VectorR4& v) {
+ double wInv = 1.0/v.w;
+ x = v.x*wInv;
+ y = v.y*wInv;
+ z = v.z*wInv;
+ return *this;
+}
+
+inline double VectorR4::Dist( const VectorR4& u ) const // Distance from u
+{
+ return sqrt( DistSq(u) );
+}
+
+inline double VectorR4::DistSq( const VectorR4& u ) const // Distance from u
+{
+ return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) + (z-u.z)*(z-u.z) + (w-u.w)*(w-u.w) );
+}
+
+
+// *********************************************************
+// * Matrix4x4 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *****
+
+inline Matrix4x4::Matrix4x4() {}
+
+inline Matrix4x4::Matrix4x4( const VectorR4& u, const VectorR4& v,
+ const VectorR4& s, const VectorR4& t)
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m41 = u.w;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m42 = v.w;
+ m13 = s.x; // Column 3
+ m23 = s.y;
+ m33 = s.z;
+ m43 = s.w;
+ m14 = t.x; // Column 4
+ m24 = t.y;
+ m34 = t.z;
+ m44 = t.w;
+}
+
+inline Matrix4x4::Matrix4x4( double a11, double a21, double a31, double a41,
+ double a12, double a22, double a32, double a42,
+ double a13, double a23, double a33, double a43,
+ double a14, double a24, double a34, double a44)
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+ m41 = a41; // Row 4
+ m42 = a42;
+ m43 = a43;
+ m44 = a44;
+}
+
+/*
+inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A)
+ : m11(A.m11), m12(A.m12), m13(A.m13), m14(A.m14),
+ m21(A.m21), m22(A.m22), m23(A.m23), m24(A.m24),
+ m31(A.m31), m32(A.m32), m33(A.m33), m34(A.m34),
+ m41(A.m41), m42(A.m42), m43(A.m43), m44(A.m44) {} */
+
+inline void Matrix4x4::SetIdentity ( )
+{
+ m11 = m22 = m33 = m44 = 1.0;
+ m12 = m13 = m14 = m21 = m23 = m24 = m13 = m23 = m41= m42 = m43 = 0.0;
+}
+
+inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v,
+ const VectorR4& s, const VectorR4& t )
+{
+ m11 = u.x; // Column 1
+ m21 = u.y;
+ m31 = u.z;
+ m41 = u.w;
+ m12 = v.x; // Column 2
+ m22 = v.y;
+ m32 = v.z;
+ m42 = v.w;
+ m13 = s.x; // Column 3
+ m23 = s.y;
+ m33 = s.z;
+ m43 = s.w;
+ m14 = t.x; // Column 4
+ m24 = t.y;
+ m34 = t.z;
+ m44 = t.w;
+}
+
+inline void Matrix4x4::Set( double a11, double a21, double a31, double a41,
+ double a12, double a22, double a32, double a42,
+ double a13, double a23, double a33, double a43,
+ double a14, double a24, double a34, double a44)
+ // Values specified in column order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+ m41 = a41; // Row 4
+ m42 = a42;
+ m43 = a43;
+ m44 = a44;
+}
+
+inline void Matrix4x4::Set ( const Matrix4x4& M ) // Set to the matrix.
+{
+ m11 = M.m11;
+ m12 = M.m12;
+ m13 = M.m13;
+ m14 = M.m14;
+ m21 = M.m21;
+ m22 = M.m22;
+ m23 = M.m23;
+ m24 = M.m24;
+ m31 = M.m31;
+ m32 = M.m32;
+ m33 = M.m33;
+ m34 = M.m34;
+ m41 = M.m41;
+ m42 = M.m42;
+ m43 = M.m43;
+ m44 = M.m44;
+}
+
+inline void Matrix4x4::SetZero( )
+{
+ m11 = m12 = m13 = m14 = m21 = m22 = m23 = m24
+ = m31 = m32 = m33 = m34 = m41 = m42 = m43 = m44 = 0.0;
+}
+
+inline void Matrix4x4::SetByRows( const VectorR4& u, const VectorR4& v,
+ const VectorR4& s, const VectorR4& t )
+{
+ m11 = u.x; // Row 1
+ m12 = u.y;
+ m13 = u.z;
+ m14 = u.w;
+ m21 = v.x; // Row 2
+ m22 = v.y;
+ m23 = v.z;
+ m24 = v.w;
+ m31 = s.x; // Row 3
+ m32 = s.y;
+ m33 = s.z;
+ m34 = s.w;
+ m41 = t.x; // Row 4
+ m42 = t.y;
+ m43 = t.z;
+ m44 = t.w;
+}
+
+inline void Matrix4x4::SetByRows( double a11, double a12, double a13, double a14,
+ double a21, double a22, double a23, double a24,
+ double a31, double a32, double a33, double a34,
+ double a41, double a42, double a43, double a44 )
+ // Values specified in row order!!!
+{
+ m11 = a11; // Row 1
+ m12 = a12;
+ m13 = a13;
+ m14 = a14;
+ m21 = a21; // Row 2
+ m22 = a22;
+ m23 = a23;
+ m24 = a24;
+ m31 = a31; // Row 3
+ m32 = a32;
+ m33 = a33;
+ m34 = a34;
+ m41 = a41; // Row 4
+ m42 = a42;
+ m43 = a43;
+ m44 = a44;
+}
+
+inline void Matrix4x4::SetColumn1 ( double x, double y, double z, double w)
+{
+ m11 = x; m21 = y; m31= z; m41 = w;
+}
+
+inline void Matrix4x4::SetColumn2 ( double x, double y, double z, double w)
+{
+ m12 = x; m22 = y; m32= z; m42 = w;
+}
+
+inline void Matrix4x4::SetColumn3 ( double x, double y, double z, double w)
+{
+ m13 = x; m23 = y; m33= z; m43 = w;
+}
+
+inline void Matrix4x4::SetColumn4 ( double x, double y, double z, double w)
+{
+ m14 = x; m24 = y; m34= z; m44 = w;
+}
+
+inline void Matrix4x4::SetColumn1 ( const VectorR4& u )
+{
+ m11 = u.x; m21 = u.y; m31 = u.z; m41 = u.w;
+}
+
+inline void Matrix4x4::SetColumn2 ( const VectorR4& u )
+{
+ m12 = u.x; m22 = u.y; m32 = u.z; m42 = u.w;
+}
+
+inline void Matrix4x4::SetColumn3 ( const VectorR4& u )
+{
+ m13 = u.x; m23 = u.y; m33 = u.z; m43 = u.w;
+}
+
+inline void Matrix4x4::SetColumn4 ( const VectorR4& u )
+{
+ m14 = u.x; m24 = u.y; m34 = u.z; m44 = u.w;
+}
+
+VectorR4 Matrix4x4::Column1() const
+{
+ return ( VectorR4(m11, m21, m31, m41) );
+}
+
+VectorR4 Matrix4x4::Column2() const
+{
+ return ( VectorR4(m12, m22, m32, m42) );
+}
+
+VectorR4 Matrix4x4::Column3() const
+{
+ return ( VectorR4(m13, m23, m33, m43) );
+}
+
+VectorR4 Matrix4x4::Column4() const
+{
+ return ( VectorR4(m14, m24, m34, m44) );
+}
+
+inline void Matrix4x4::SetDiagonal( double x, double y,
+ double z, double w)
+{
+ m11 = x;
+ m22 = y;
+ m33 = z;
+ m44 = w;
+}
+
+inline void Matrix4x4::SetDiagonal( const VectorR4& u )
+{
+ SetDiagonal ( u.x, u.y, u.z, u.w );
+}
+
+inline double Matrix4x4::Diagonal( int i )
+{
+ switch (i) {
+ case 0:
+ return m11;
+ case 1:
+ return m22;
+ case 2:
+ return m33;
+ case 3:
+ return m44;
+ default:
+ assert(0);
+ return 0.0;
+ }
+}
+
+inline void Matrix4x4::MakeTranspose() // Transposes it.
+{
+ register double temp;
+ temp = m12;
+ m12 = m21;
+ m21=temp;
+ temp = m13;
+ m13 = m31;
+ m31 = temp;
+ temp = m14;
+ m14 = m41;
+ m41 = temp;
+ temp = m23;
+ m23 = m32;
+ m32 = temp;
+ temp = m24;
+ m24 = m42;
+ m42 = temp;
+ temp = m34;
+ m34 = m43;
+ m43 = temp;
+}
+
+inline VectorR4 operator* ( const Matrix4x4& A, const VectorR4& u)
+{
+ VectorR4 ret;
+ ret.x = A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w;
+ ret.y = A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w;
+ ret.z = A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w;
+ ret.w = A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w;
+ return ret;
+}
+
+
+// ******************************************************
+// * LinearMapR4 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline LinearMapR4::LinearMapR4()
+{
+ SetZero();
+ return;
+}
+
+inline LinearMapR4::LinearMapR4( const VectorR4& u, const VectorR4& v,
+ const VectorR4& s, const VectorR4& t)
+:Matrix4x4 ( u, v, s ,t )
+{ }
+
+inline LinearMapR4::LinearMapR4(
+ double a11, double a21, double a31, double a41,
+ double a12, double a22, double a32, double a42,
+ double a13, double a23, double a33, double a43,
+ double a14, double a24, double a34, double a44)
+ // Values specified in column order!!!
+:Matrix4x4 ( a11, a21, a31, a41, a12, a22, a32, a42,
+ a13, a23, a33, a43, a14, a24, a34, a44 )
+{ }
+
+inline LinearMapR4::LinearMapR4 ( const Matrix4x4& A )
+: Matrix4x4 (A)
+{}
+
+
+inline LinearMapR4& LinearMapR4::operator+= (const LinearMapR4& B)
+{
+ m11 += B.m11;
+ m12 += B.m12;
+ m13 += B.m13;
+ m14 += B.m14;
+ m21 += B.m21;
+ m22 += B.m22;
+ m23 += B.m23;
+ m24 += B.m24;
+ m31 += B.m31;
+ m32 += B.m32;
+ m33 += B.m33;
+ m34 += B.m34;
+ m41 += B.m41;
+ m42 += B.m42;
+ m43 += B.m43;
+ m44 += B.m44;
+ return ( *this );
+}
+
+inline LinearMapR4& LinearMapR4::operator-= (const LinearMapR4& B)
+{
+ m11 -= B.m11;
+ m12 -= B.m12;
+ m13 -= B.m13;
+ m14 -= B.m14;
+ m21 -= B.m21;
+ m22 -= B.m22;
+ m23 -= B.m23;
+ m24 -= B.m24;
+ m31 -= B.m31;
+ m32 -= B.m32;
+ m33 -= B.m33;
+ m34 -= B.m34;
+ m41 -= B.m41;
+ m42 -= B.m42;
+ m43 -= B.m43;
+ m44 -= B.m44;
+ return( *this );
+}
+
+inline LinearMapR4 operator+ (const LinearMapR4& A, const LinearMapR4& B)
+{
+ return( LinearMapR4( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, A.m41+B.m41,
+ A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, A.m42+B.m42,
+ A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, A.m43+B.m43,
+ A.m14+B.m14, A.m24+B.m24, A.m34+B.m34, A.m44+B.m44) );
+}
+
+inline LinearMapR4 operator- (const LinearMapR4& A)
+{
+ return( LinearMapR4( -A.m11, -A.m21, -A.m31, -A.m41,
+ -A.m12, -A.m22, -A.m32, -A.m42,
+ -A.m13, -A.m23, -A.m33, -A.m43,
+ -A.m14, -A.m24, -A.m34, -A.m44 ) );
+}
+
+inline LinearMapR4 operator- (const LinearMapR4& A, const LinearMapR4& B)
+{
+ return( LinearMapR4( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, A.m41-B.m41,
+ A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, A.m42-B.m42,
+ A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, A.m43-B.m43,
+ A.m14-B.m14, A.m24-B.m24, A.m34-B.m34, A.m44-B.m44 ) );
+}
+
+inline LinearMapR4& LinearMapR4::operator*= (register double b)
+{
+ m11 *= b;
+ m12 *= b;
+ m13 *= b;
+ m14 *= b;
+ m21 *= b;
+ m22 *= b;
+ m23 *= b;
+ m24 *= b;
+ m31 *= b;
+ m32 *= b;
+ m33 *= b;
+ m34 *= b;
+ m41 *= b;
+ m42 *= b;
+ m43 *= b;
+ m44 *= b;
+ return ( *this);
+}
+
+inline LinearMapR4 operator* ( const LinearMapR4& A, register double b)
+{
+ return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b,
+ A.m12*b, A.m22*b, A.m32*b, A.m42*b,
+ A.m13*b, A.m23*b, A.m33*b, A.m43*b,
+ A.m14*b, A.m24*b, A.m34*b, A.m44*b) );
+}
+
+inline LinearMapR4 operator* ( register double b, const LinearMapR4& A)
+{
+ return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b,
+ A.m12*b, A.m22*b, A.m32*b, A.m42*b,
+ A.m13*b, A.m23*b, A.m33*b, A.m43*b,
+ A.m14*b, A.m24*b, A.m34*b, A.m44*b ) );
+}
+
+inline LinearMapR4 operator/ ( const LinearMapR4& A, double b)
+{
+ register double bInv = 1.0/b;
+ return ( A*bInv );
+}
+
+inline LinearMapR4& LinearMapR4::operator/= (register double b)
+{
+ register double bInv = 1.0/b;
+ return ( *this *= bInv );
+}
+
+inline VectorR4 operator* ( const LinearMapR4& A, const VectorR4& u)
+{
+ return(VectorR4 ( A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w,
+ A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w,
+ A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w,
+ A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w ) );
+}
+
+inline LinearMapR4 LinearMapR4::Transpose() const // Returns the transpose
+{
+ return (LinearMapR4( m11, m12, m13, m14,
+ m21, m22, m23, m24,
+ m31, m32, m33, m34,
+ m41, m42, m43, m44 ) );
+}
+
+inline LinearMapR4& LinearMapR4::operator*= (const Matrix4x4& B) // Matrix product
+{
+ (*this).Matrix4x4::operator*=(B);
+
+ return( *this );
+}
+
+inline LinearMapR4 operator* ( const LinearMapR4& A, const Matrix4x4& B)
+{
+ LinearMapR4 AA(A);
+ AA.Matrix4x4::operator*=(B);
+ return AA;
+}
+
+inline LinearMapR4 operator* ( const Matrix4x4& A, const LinearMapR4& B)
+{
+ LinearMapR4 AA(A);
+ AA.Matrix4x4::operator*=(B);
+ return AA;
+}
+
+
+
+// ******************************************************
+// * RotationMapR4 class - inlined functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * **
+
+inline RotationMapR4::RotationMapR4()
+{
+ SetIdentity();
+ return;
+}
+
+inline RotationMapR4::RotationMapR4( const VectorR4& u, const VectorR4& v,
+ const VectorR4& s, const VectorR4& t)
+:Matrix4x4 ( u, v, s ,t )
+{ }
+
+inline RotationMapR4::RotationMapR4(
+ double a11, double a21, double a31, double a41,
+ double a12, double a22, double a32, double a42,
+ double a13, double a23, double a33, double a43,
+ double a14, double a24, double a34, double a44)
+ // Values specified in column order!!!
+:Matrix4x4 ( a11, a21, a31, a41, a12, a22, a32, a42,
+ a13, a23, a33, a43, a14, a24, a34, a44 )
+{ }
+
+inline RotationMapR4 RotationMapR4::Transpose() const // Returns the transpose
+{
+ return ( RotationMapR4( m11, m12, m13, m14,
+ m21, m22, m23, m24,
+ m31, m32, m33, m34,
+ m41, m42, m43, m44 ) );
+}
+
+inline VectorR4 RotationMapR4::Invert(const VectorR4& u) const // Returns solution
+{
+ return (VectorR4( m11*u.x + m21*u.y + m31*u.z + m41*u.w,
+ m12*u.x + m22*u.y + m32*u.z + m42*u.w,
+ m13*u.x + m23*u.y + m33*u.z + m43*u.w,
+ m14*u.x + m24*u.y + m34*u.z + m44*u.w ) );
+}
+
+inline RotationMapR4& RotationMapR4::operator*= (const RotationMapR4& B) // Matrix product
+{
+ (*this).Matrix4x4::operator*=(B);
+
+ return( *this );
+}
+
+inline RotationMapR4 operator* ( const RotationMapR4& A, const RotationMapR4& B)
+{
+ RotationMapR4 AA(A);
+ AA.Matrix4x4::operator*=(B);
+ return AA;
+}
+
+
+// ***************************************************************
+// * 4-space vector and matrix utilities (inlined functions) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+inline void TimesTranspose( const VectorR4& u, const VectorR4& v, LinearMapR4& M)
+{
+ M.Set ( v.x*u.x, v.x*u.y, v.x*u.z, v.x*u.w, // Set by columns!
+ v.y*u.x, v.y*u.y, v.y*u.z, v.y*u.w,
+ v.z*u.x, v.z*u.y, v.z*u.z, v.z*u.w,
+ v.w*u.x, v.w*u.y, v.w*u.z, v.w*u.w ) ;
+}
+
+// Returns the solid angle between vectors u and v (not necessarily unit vectors)
+inline double SolidAngle( const VectorR4& u, const VectorR4& v)
+{
+ double nSqU = u.NormSq();
+ double nSqV = v.NormSq();
+ if ( nSqU==0.0 && nSqV==0.0 ) {
+ return (0.0);
+ }
+ else {
+ return ( SolidAngleUnit( u/sqrt(nSqU), v/sqrt(nSqV) ) );
+ }
+}
+
+inline double SolidAngleUnit( const VectorR4 u, const VectorR4 v )
+{
+ return ( atan2 ( ProjectPerpUnit(v,u).Norm(), u^v ) );
+}
+
+
+#endif // LINEAR_R4_H
diff --git a/examples/ThirdPartyLibs/BussIK/MathMisc.h b/examples/ThirdPartyLibs/BussIK/MathMisc.h
new file mode 100644
index 000000000..1738753f8
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/MathMisc.h
@@ -0,0 +1,384 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+#ifndef MATH_MISC_H
+#define MATH_MISC_H
+
+#include
+
+//
+// Commonly used constants
+//
+
+const double PI = 3.1415926535897932384626433832795028841972;
+const double PI2 = 2.0*PI;
+const double PI4 = 4.0*PI;
+const double PISq = PI*PI;
+const double PIhalves = 0.5*PI;
+const double PIthirds = PI/3.0;
+const double PItwothirds = PI2/3.0;
+const double PIfourths = 0.25*PI;
+const double PIsixths = PI/6.0;
+const double PIsixthsSq = PIsixths*PIsixths;
+const double PItwelfths = PI/12.0;
+const double PItwelfthsSq = PItwelfths*PItwelfths;
+const double PIinv = 1.0/PI;
+const double PI2inv = 0.5/PI;
+const double PIhalfinv = 2.0/PI;
+
+const double RadiansToDegrees = 180.0/PI;
+const double DegreesToRadians = PI/180;
+
+const double OneThird = 1.0/3.0;
+const double TwoThirds = 2.0/3.0;
+const double OneSixth = 1.0/6.0;
+const double OneEighth = 1.0/8.0;
+const double OneTwelfth = 1.0/12.0;
+
+const double Root2 = sqrt(2.0);
+const double Root3 = sqrt(3.0);
+const double Root2Inv = 1.0/Root2; // sqrt(2)/2
+const double HalfRoot3 = sqrtf(3)/2.0;
+
+const double LnTwo = log(2.0);
+const double LnTwoInv = 1.0/log(2.0);
+
+// Special purpose constants
+const double OnePlusEpsilon15 = 1.0+1.0e-15;
+const double OneMinusEpsilon15 = 1.0-1.0e-15;
+
+inline double ZeroValue(const double& x)
+{
+ return 0.0;
+}
+
+//
+// Comparisons
+//
+
+template inline T Min ( T x, T y )
+{
+ return (x inline T Max ( T x, T y )
+{
+ return (y inline T ClampRange ( T x, T min, T max)
+{
+ if ( xmax ) {
+ return max;
+ }
+ return x;
+}
+
+template inline bool ClampRange ( T *x, T min, T max)
+{
+ if ( (*x)max ) {
+ (*x) = max;
+ return false;
+ }
+ else {
+ return true;
+ }
+}
+
+template inline bool ClampMin ( T *x, T min)
+{
+ if ( (*x) inline bool ClampMax ( T *x, T max)
+{
+ if ( (*x)>max ) {
+ (*x) = max;
+ return false;
+ }
+ return true;
+}
+
+template inline T& UpdateMin ( const T& x, T& y )
+{
+ if ( x inline T& UpdateMax ( const T& x, T& y )
+{
+ if ( x>y ) {
+ y = x;
+ }
+ return y;
+}
+
+
+template inline bool SameSignNonzero( T x, T y )
+{
+ if ( x<0 ) {
+ return (y<0);
+ }
+ else if ( 0
+inline bool NearEqual( T a, T b, double tolerance ) {
+ a -= b;
+ return ( Mag(a)<=tolerance );
+}
+
+inline bool EqualZeroFuzzy( double x ) {
+ return ( fabs(x)<=1.0e-14 );
+}
+
+inline bool NearZero( double x, double tolerance ) {
+ return ( fabs(x)<=tolerance );
+}
+
+inline bool LessOrEqualFuzzy( double x, double y )
+{
+ if ( x <= y ) {
+ return true;
+ }
+
+ if ( y > 0.0 ) {
+ if ( x>0.0 ) {
+ return ( x*OneMinusEpsilon15 < y*OnePlusEpsilon15 );
+ }
+ else {
+ return ( y<1.0e-15 ); // x==0 in this case
+ }
+ }
+ else if ( y < 0.0 ) {
+ if ( x<0.0 ) {
+ return ( x*OnePlusEpsilon15 < y*OneMinusEpsilon15 );
+ }
+ else {
+ return ( y>-1.0e-15 ); // x==0 in this case
+ }
+ }
+ else {
+ return ( -1.0e-15 *maxabs ) {
+ *maxabs = updateval;
+ return true;
+ }
+ else if ( -updateval > *maxabs ) {
+ *maxabs = -updateval;
+ return true;
+ }
+ else {
+ return false;
+ }
+}
+
+// **********************************************************
+// Combinations and averages. *
+// **********************************************************
+
+template
+void averageOf ( const T& a, const T &b, T&c ) {
+ c = a;
+ c += b;
+ c *= 0.5;
+}
+
+template
+void Lerp( const T& a, const T&b, double alpha, T&c ) {
+ double beta = 1.0-alpha;
+ if ( beta>alpha ) {
+ c = b;
+ c *= alpha/beta;
+ c += a;
+ c *= beta;
+ }
+ else {
+ c = a;
+ c *= beta/alpha;
+ c += b;
+ c *= alpha;
+ }
+}
+
+template
+T Lerp( const T& a, const T&b, double alpha ) {
+ T ret;
+ Lerp( a, b, alpha, ret );
+ return ret;
+}
+
+// **********************************************************
+// Trigonometry *
+// **********************************************************
+
+// TimesCot(x) returns x*cot(x)
+inline double TimesCot ( double x ) {
+ if ( -1.0e-5 < x && x < 1.0e-5 ) {
+ return 1.0+x*OneThird;
+ }
+ else {
+ return ( x*cos(x)/sin(x) );
+ }
+}
+
+// SineOver(x) returns sin(x)/x.
+inline double SineOver( double x ) {
+ if ( -1.0e-5 < x && x < 1.0e-5 ) {
+ return 1.0-x*x*OneSixth;
+ }
+ else {
+ return sin(x)/x;
+ }
+}
+// OverSine(x) returns x/sin(x).
+inline double OverSine( double x ) {
+ if ( -1.0e-5 < x && x < 1.0e-5 ) {
+ return 1.0+x*x*OneSixth;
+ }
+ else {
+ return x/sin(x);
+ }
+}
+
+inline double SafeAsin( double x ) {
+ if ( x <= -1.0 ) {
+ return -PIhalves;
+ }
+ else if ( x >= 1.0 ) {
+ return PIhalves;
+ }
+ else {
+ return asin(x);
+ }
+}
+
+inline double SafeAcos( double x ) {
+ if ( x <= -1.0 ) {
+ return PI;
+ }
+ else if ( x >= 1.0 ) {
+ return 0.0;
+ }
+ else {
+ return acos(x);
+ }
+}
+
+
+// **********************************************************************
+// Roots and powers *
+// **********************************************************************
+
+// Square(x) returns x*x, of course!
+
+template inline T Square ( T x )
+{
+ return (x*x);
+}
+
+// Cube(x) returns x*x*x, of course!
+
+template inline T Cube ( T x )
+{
+ return (x*x*x);
+}
+
+// SafeSqrt(x) = returns sqrt(max(x, 0.0));
+
+inline double SafeSqrt( double x ) {
+ if ( x<=0.0 ) {
+ return 0.0;
+ }
+ else {
+ return sqrt(x);
+ }
+}
+
+
+// SignedSqrt(a, s) returns (sign(s)*sqrt(a)).
+inline double SignedSqrt( double a, double sgn )
+{
+ if ( sgn==0.0 ) {
+ return 0.0;
+ }
+ else {
+ return ( sgn>0.0 ? sqrt(a) : -sqrt(a) );
+ }
+}
+
+
+// Template version of Sign function
+
+template inline int Sign( T x)
+{
+ if ( x<0 ) {
+ return -1;
+ }
+ else if ( x==0 ) {
+ return 0;
+ }
+ else {
+ return 1;
+ }
+}
+
+
+#endif // #ifndef MATH_MISC_H
diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
new file mode 100644
index 000000000..365d90f6c
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
@@ -0,0 +1,984 @@
+
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+//
+// MatrixRmn.cpp: Matrix over reals (Variable dimensional vector)
+//
+// Not very sophisticated yet. Needs more functionality
+// To do: better handling of resizing.
+//
+
+#include "MatrixRmn.h"
+
+MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix
+
+// Fill the diagonal entries with the value d. The rest of the matrix is unchanged.
+void MatrixRmn::SetDiagonalEntries( double d )
+{
+ long diagLen = Min( NumRows, NumCols );
+ double* dPtr = x;
+ for ( ; diagLen>0; diagLen-- ) {
+ *dPtr = d;
+ dPtr += NumRows+1;
+ }
+}
+
+// Fill the diagonal entries with values in vector d. The rest of the matrix is unchanged.
+void MatrixRmn::SetDiagonalEntries( const VectorRn& d )
+{
+ long diagLen = Min( NumRows, NumCols );
+ assert ( d.length == diagLen );
+ double* dPtr = x;
+ double* from = d.x;
+ for ( ; diagLen>0; diagLen-- ) {
+ *dPtr = *(from++);
+ dPtr += NumRows+1;
+ }
+}
+
+// Fill the superdiagonal entries with the value d. The rest of the matrix is unchanged.
+void MatrixRmn::SetSuperDiagonalEntries( double d )
+{
+ long sDiagLen = Min( NumRows, (long)(NumCols-1) );
+ double* to = x + NumRows;
+ for ( ; sDiagLen>0; sDiagLen-- ) {
+ *to = d;
+ to += NumRows+1;
+ }
+}
+
+// Fill the superdiagonal entries with values in vector d. The rest of the matrix is unchanged.
+void MatrixRmn::SetSuperDiagonalEntries( const VectorRn& d )
+{
+ long sDiagLen = Min( (long)(NumRows-1), NumCols );
+ assert ( sDiagLen == d.length );
+ double* to = x + NumRows;
+ double* from = d.x;
+ for ( ; sDiagLen>0; sDiagLen-- ) {
+ *to = *(from++);
+ to += NumRows+1;
+ }
+}
+
+// Fill the subdiagonal entries with the value d. The rest of the matrix is unchanged.
+void MatrixRmn::SetSubDiagonalEntries( double d )
+{
+ long sDiagLen = Min( NumRows, NumCols ) - 1;
+ double* to = x + 1;
+ for ( ; sDiagLen>0; sDiagLen-- ) {
+ *to = d;
+ to += NumRows+1;
+ }
+}
+
+// Fill the subdiagonal entries with values in vector d. The rest of the matrix is unchanged.
+void MatrixRmn::SetSubDiagonalEntries( const VectorRn& d )
+{
+ long sDiagLen = Min( NumRows, NumCols ) - 1;
+ assert ( sDiagLen == d.length );
+ double* to = x + 1;
+ double* from = d.x;
+ for ( ; sDiagLen>0; sDiagLen-- ) {
+ *to = *(from++);
+ to += NumRows+1;
+ }
+}
+
+// Set the i-th column equal to d.
+void MatrixRmn::SetColumn(long i, const VectorRn& d )
+{
+ assert ( NumRows==d.GetLength() );
+ double* to = x+i*NumRows;
+ const double* from = d.x;
+ for ( i=NumRows; i>0; i-- ) {
+ *(to++) = *(from++);
+ }
+}
+
+// Set the i-th column equal to d.
+void MatrixRmn::SetRow(long i, const VectorRn& d )
+{
+ assert ( NumCols==d.GetLength() );
+ double* to = x+i;
+ const double* from = d.x;
+ for ( i=NumRows; i>0; i-- ) {
+ *to = *(from++);
+ to += NumRows;
+ }
+}
+
+// Sets a "linear" portion of the array with the values from a vector d
+// The first row and column position are given by startRow, startCol.
+// Successive positions are found by using the deltaRow, deltaCol values
+// to increment the row and column indices. There is no wrapping around.
+void MatrixRmn::SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol )
+{
+ long length = d.length;
+ assert( startRow>=0 && startRow=0 && startCol=0 && startRow+(length-1)*deltaRow=0 && startCol+(length-1)*deltaCol0; length-- ) {
+ *to = *(from++);
+ to += stride;
+ }
+}
+
+// The matrix A is loaded, in into "this" matrix, based at (0,0).
+// The size of "this" matrix must be large enough to accomodate A.
+// The rest of "this" matrix is left unchanged. It is not filled with zeroes!
+
+void MatrixRmn::LoadAsSubmatrix( const MatrixRmn& A )
+{
+ assert( A.NumRows<=NumRows && A.NumCols<=NumCols );
+ int extraColStep = NumRows - A.NumRows;
+ double *to = x;
+ double *from = A.x;
+ for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, one per time thru loop
+ for ( long j=A.NumRows; j>0; j-- ) { // Copy all elements of this column of A
+ *(to++) = *(from++);
+ }
+ to += extraColStep;
+ }
+}
+
+// The matrix A is loaded, in transposed order into "this" matrix, based at (0,0).
+// The size of "this" matrix must be large enough to accomodate A.
+// The rest of "this" matrix is left unchanged. It is not filled with zeroes!
+void MatrixRmn::LoadAsSubmatrixTranspose( const MatrixRmn& A )
+{
+ assert( A.NumRows<=NumCols && A.NumCols<=NumRows );
+ double* rowPtr = x;
+ double* from = A.x;
+ for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, once per loop
+ double* to = rowPtr;
+ for ( long j=A.NumRows; j>0; j-- ) { // Loop copying values from the column of A
+ *to = *(from++);
+ to += NumRows;
+ }
+ rowPtr ++;
+ }
+}
+
+// Calculate the Frobenius Norm (square root of sum of squares of entries of the matrix)
+double MatrixRmn::FrobeniusNorm() const
+{
+ return sqrt( FrobeniusNormSq() );
+}
+
+// Multiply this matrix by column vector v.
+// Result is column vector "result"
+void MatrixRmn::Multiply( const VectorRn& v, VectorRn& result ) const
+{
+ assert ( v.GetLength()==NumCols && result.GetLength()==NumRows );
+ double* out = result.GetPtr(); // Points to entry in result vector
+ const double* rowPtr = x; // Points to beginning of next row in matrix
+ for ( long j = NumRows; j>0; j-- ) {
+ const double* in = v.GetPtr();
+ const double* m = rowPtr++;
+ *out = 0.0f;
+ for ( long i = NumCols; i>0; i-- ) {
+ *out += (*(in++)) * (*m);
+ m += NumRows;
+ }
+ out++;
+ }
+}
+
+// Multiply transpose of this matrix by column vector v.
+// Result is column vector "result"
+// Equivalent to mult by row vector on left
+void MatrixRmn::MultiplyTranspose( const VectorRn& v, VectorRn& result ) const
+{
+ assert ( v.GetLength()==NumRows && result.GetLength()==NumCols );
+ double* out = result.GetPtr(); // Points to entry in result vector
+ const double* colPtr = x; // Points to beginning of next column in matrix
+ for ( long i=NumCols; i>0; i-- ) {
+ const double* in=v.GetPtr();
+ *out = 0.0f;
+ for ( long j = NumRows; j>0; j-- ) {
+ *out += (*(in++)) * (*(colPtr++));
+ }
+ out++;
+ }
+}
+
+// Form the dot product of a vector v with the i-th column of the array
+double MatrixRmn::DotProductColumn( const VectorRn& v, long colNum ) const
+{
+ assert ( v.GetLength()==NumRows );
+ double* ptrC = x+colNum*NumRows;
+ double* ptrV = v.x;
+ double ret = 0.0;
+ for ( long i = NumRows; i>0; i-- ) {
+ ret += (*(ptrC++))*(*(ptrV++));
+ }
+ return ret;
+}
+
+// Add a constant to each entry on the diagonal
+MatrixRmn& MatrixRmn::AddToDiagonal( double d ) // Adds d to each diagonal entry
+{
+ long diagLen = Min( NumRows, NumCols );
+ double* dPtr = x;
+ for ( ; diagLen>0; diagLen-- ) {
+ *dPtr += d;
+ dPtr += NumRows+1;
+ }
+ return *this;
+}
+
+// Multiply two MatrixRmn's
+MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
+{
+ assert( A.NumCols == B.NumRows && A.NumRows == dst.NumRows && B.NumCols == dst.NumCols );
+ long length = A.NumCols;
+
+ double *bPtr = B.x; // Points to beginning of column in B
+ double *dPtr = dst.x;
+ for ( long i = dst.NumCols; i>0; i-- ) {
+ double *aPtr = A.x; // Points to beginning of row in A
+ for ( long j = dst.NumRows; j>0; j-- ) {
+ *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, 1 );
+ dPtr++;
+ aPtr++;
+ }
+ bPtr += B.NumRows;
+ }
+
+ return dst;
+}
+
+// Multiply two MatrixRmn's, Transpose the first matrix before multiplying
+MatrixRmn& MatrixRmn::TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
+{
+ assert( A.NumRows == B.NumRows && A.NumCols == dst.NumRows && B.NumCols == dst.NumCols );
+ long length = A.NumRows;
+
+ double *bPtr = B.x; // bPtr Points to beginning of column in B
+ double *dPtr = dst.x;
+ for ( long i = dst.NumCols; i>0; i-- ) { // Loop over all columns of dst
+ double *aPtr = A.x; // aPtr Points to beginning of column in A
+ for ( long j = dst.NumRows; j>0; j-- ) { // Loop over all rows of dst
+ *dPtr = DotArray( length, aPtr, 1, bPtr, 1 );
+ dPtr ++;
+ aPtr += A.NumRows;
+ }
+ bPtr += B.NumRows;
+ }
+
+ return dst;
+}
+
+// Multiply two MatrixRmn's. Transpose the second matrix before multiplying
+MatrixRmn& MatrixRmn::MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
+{
+ assert( A.NumCols == B.NumCols && A.NumRows == dst.NumRows && B.NumRows == dst.NumCols );
+ long length = A.NumCols;
+
+ double *bPtr = B.x; // Points to beginning of row in B
+ double *dPtr = dst.x;
+ for ( long i = dst.NumCols; i>0; i-- ) {
+ double *aPtr = A.x; // Points to beginning of row in A
+ for ( long j = dst.NumRows; j>0; j-- ) {
+ *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, B.NumRows );
+ dPtr++;
+ aPtr++;
+ }
+ bPtr++;
+ }
+
+ return dst;
+}
+
+// Solves the equation (*this)*xVec = b;
+// Uses row operations. Assumes *this is square and invertible.
+// No error checking for divide by zero or instability (except with asserts)
+void MatrixRmn::Solve( const VectorRn& b, VectorRn* xVec ) const
+{
+ assert ( NumRows==NumCols && NumCols==xVec->GetLength() && NumRows==b.GetLength() );
+
+ // Copy this matrix and b into an Augmented Matrix
+ MatrixRmn& AugMat = GetWorkMatrix( NumRows, NumCols+1 );
+ AugMat.LoadAsSubmatrix( *this );
+ AugMat.SetColumn( NumRows, b );
+
+ // Put into row echelon form with row operations
+ AugMat.ConvertToRefNoFree();
+
+ // Solve for x vector values using back substitution
+ double* xLast = xVec->x+NumRows-1; // Last entry in xVec
+ double* endRow = AugMat.x+NumRows*NumCols-1; // Last entry in the current row of the coefficient part of Augmented Matrix
+ double* bPtr = endRow+NumRows; // Last entry in augmented matrix (end of last column, in augmented part)
+ for ( long i = NumRows; i>0; i-- ) {
+ double accum = *(bPtr--);
+ // Next loop computes back substitution terms
+ double* rowPtr = endRow; // Points to entries of the current row for back substitution.
+ double* xPtr = xLast; // Points to entries in the x vector (also for back substitution)
+ for ( long j=NumRows-i; j>0; j-- ) {
+ accum -= (*rowPtr)*(*(xPtr--));
+ rowPtr -= NumCols; // Previous entry in the row
+ }
+ assert( *rowPtr != 0.0 ); // Are not supposed to be any free variables in this matrix
+ *xPtr = accum/(*rowPtr);
+ endRow--;
+ }
+}
+
+// ConvertToRefNoFree
+// Converts the matrix (in place) to row echelon form
+// For us, row echelon form allows any non-zero values, not just 1's, in the
+// position for a lead variable.
+// The "NoFree" version operates on the assumption that no free variable will be found.
+// Algorithm uses row operations and row pivoting (only).
+// Augmented matrix is correctly accomodated. Only the first square part participates
+// in the main work of row operations.
+void MatrixRmn::ConvertToRefNoFree()
+{
+ // Loop over all columns (variables)
+ // Find row with most non-zero entry.
+ // Swap to the highest active row
+ // Subtract appropriately from all the lower rows (row op of type 3)
+ long numIters = Min(NumRows,NumCols);
+ double* rowPtr1 = x;
+ const long diagStep = NumRows+1;
+ long lenRowLeft = NumCols;
+ for ( ; numIters>1; numIters-- ) {
+ // Find row with most non-zero entry.
+ double* rowPtr2 = rowPtr1;
+ double maxAbs = fabs(*rowPtr1);
+ double *rowPivot = rowPtr1;
+ long i;
+ for ( i=numIters-1; i>0; i-- ) {
+ const double& newMax = *(++rowPivot);
+ if ( newMax > maxAbs ) {
+ maxAbs = *rowPivot;
+ rowPtr2 = rowPivot;
+ }
+ else if ( -newMax > maxAbs ) {
+ maxAbs = -newMax;
+ rowPtr2 = rowPivot;
+ }
+ }
+ // Pivot step: Swap the row with highest entry to the current row
+ if ( rowPtr1 != rowPtr2 ) {
+ double *to = rowPtr1;
+ for ( long i=lenRowLeft; i>0; i-- ) {
+ double temp = *to;
+ *to = *rowPtr2;
+ *rowPtr2 = temp;
+ to += NumRows;
+ rowPtr2 += NumRows;
+ }
+ }
+ // Subtract this row appropriately from all the lower rows (row operation of type 3)
+ rowPtr2 = rowPtr1;
+ for ( i=numIters-1; i>0; i-- ) {
+ rowPtr2++;
+ double* to = rowPtr2;
+ double* from = rowPtr1;
+ assert( *from != 0.0 );
+ double alpha = (*to)/(*from);
+ *to = 0.0;
+ for ( long j=lenRowLeft-1; j>0; j-- ) {
+ to += NumRows;
+ from += NumRows;
+ *to -= (*from)*alpha;
+ }
+ }
+ // Update for next iteration of loop
+ rowPtr1 += diagStep;
+ lenRowLeft--;
+ }
+
+}
+
+// Calculate the c=cosine and s=sine values for a Givens transformation.
+// The matrix M = ( (c, -s), (s, c) ) in row order transforms the
+// column vector (a, b)^T to have y-coordinate zero.
+void MatrixRmn::CalcGivensValues( double a, double b, double *c, double *s )
+{
+ double denomInv = sqrt(a*a + b*b);
+ if ( denomInv==0.0 ) {
+ *c = 1.0;
+ *s = 0.0;
+ }
+ else {
+ denomInv = 1.0/denomInv;
+ *c = a*denomInv;
+ *s = -b*denomInv;
+ }
+}
+
+// Applies Givens transform to columns i and i+1.
+// Equivalent to postmultiplying by the matrix
+// ( c -s )
+// ( s c )
+// with non-zero entries in rows i and i+1 and columns i and i+1
+void MatrixRmn::PostApplyGivens( double c, double s, long idx )
+{
+ assert ( 0<=idx && idx0; i-- ) {
+ double temp = *colA;
+ *colA = (*colA)*c + (*colB)*s;
+ *colB = (*colB)*c - temp*s;
+ colA++;
+ colB++;
+ }
+}
+
+// Applies Givens transform to columns idx1 and idx2.
+// Equivalent to postmultiplying by the matrix
+// ( c -s )
+// ( s c )
+// with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2
+void MatrixRmn::PostApplyGivens( double c, double s, long idx1, long idx2 )
+{
+ assert ( idx1!=idx2 && 0<=idx1 && idx10; i-- ) {
+ double temp = *colA;
+ *colA = (*colA)*c + (*colB)*s;
+ *colB = (*colB)*c - temp*s;
+ colA++;
+ colB++;
+ }
+}
+
+
+// ********************************************************************************************
+// Singular value decomposition.
+// Return othogonal matrices U and V and diagonal matrix with diagonal w such that
+// (this) = U * Diag(w) * V^T (V^T is V-transpose.)
+// Diagonal entries have all non-zero entries before all zero entries, but are not
+// necessarily sorted. (Someday, I will write ComputedSortedSVD that handles
+// sorting the eigenvalues by magnitude.)
+// ********************************************************************************************
+void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
+{
+ assert ( U.NumRows==NumRows && V.NumCols==NumCols
+ && U.NumRows==U.NumCols && V.NumRows==V.NumCols
+ && w.GetLength()==Min(NumRows,NumCols) );
+
+ double temp=0.0;
+ VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 ); // Some extra work space. Will get passed around.
+
+ // Choose larger of U, V to hold intermediate results
+ // If U is larger than V, use U to store intermediate results
+ // Otherwise use V. In the latter case, we form the SVD of A transpose,
+ // (which is essentially identical to the SVD of A).
+ MatrixRmn* leftMatrix;
+ MatrixRmn* rightMatrix;
+ if ( NumRows >= NumCols ) {
+ U.LoadAsSubmatrix( *this ); // Copy A into U
+ leftMatrix = &U;
+ rightMatrix = &V;
+ }
+ else {
+ V.LoadAsSubmatrixTranspose( *this ); // Copy A-transpose into V
+ leftMatrix = &V;
+ rightMatrix = &U;
+ }
+
+ // Do the actual work to calculate the SVD
+ // Now matrix has at least as many rows as columns
+ CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
+ ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );
+
+}
+
+// ************************************************ CalcBidiagonal **************************
+// Helper routine for SVD computation
+// U is a matrix to be bidiagonalized.
+// On return, U and V are orthonormal and w holds the new diagonal
+// elements and superDiag holds the super diagonal elements.
+
+void MatrixRmn::CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag )
+{
+ assert ( U.NumRows>=V.NumRows );
+
+ // The diagonal and superdiagonal entries of the bidiagonalized
+ // version of the U matrix
+ // are stored in the vectors w and superDiag (temporarily).
+
+ // Apply Householder transformations to U.
+ // Householder transformations come in pairs.
+ // First, on the left, we map a portion of a column to zeros
+ // Second, on the right, we map a portion of a row to zeros
+ const long rowStep = U.NumCols;
+ const long diagStep = U.NumCols+1;
+ double *diagPtr = U.x;
+ double* wPtr = w.x;
+ double* superDiagPtr = superDiag.x;
+ long colLengthLeft = U.NumRows;
+ long rowLengthLeft = V.NumCols;
+ while (true) {
+ // Apply a Householder xform on left to zero part of a column
+ SvdHouseholder( diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr );
+
+ if ( rowLengthLeft==2 ) {
+ *superDiagPtr = *(diagPtr+rowStep);
+ break;
+ }
+ // Apply a Householder xform on the right to zero part of a row
+ SvdHouseholder( diagPtr+rowStep, rowLengthLeft-1, colLengthLeft, rowStep, 1, superDiagPtr );
+
+ rowLengthLeft--;
+ colLengthLeft--;
+ diagPtr += diagStep;
+ wPtr++;
+ superDiagPtr++;
+ }
+
+ int extra = 0;
+ diagPtr += diagStep;
+ wPtr++;
+ if ( colLengthLeft > 2 ) {
+ extra = 1;
+ // Do one last Householder transformation when the matrix is not square
+ colLengthLeft--;
+ SvdHouseholder( diagPtr, colLengthLeft, 1, 1, 0, wPtr );
+ }
+ else {
+ *wPtr = *diagPtr;
+ }
+
+ // Form U and V from the Householder transformations
+ V.ExpandHouseholders( V.NumCols-2, 1, U.x+U.NumRows, U.NumRows, 1 );
+ U.ExpandHouseholders( V.NumCols-1+extra, 0, U.x, 1, U.NumRows );
+
+ // Done with bidiagonalization
+ return;
+}
+
+// Helper routine for CalcBidiagonal
+// Performs a series of Householder transformations on a matrix
+// Stores results compactly into the matrix: The Householder vector u (normalized)
+// is stored into the first row/column being transformed.
+// The leading term of that row (= plus/minus its magnitude is returned
+// separately into "retFirstEntry"
+void MatrixRmn::SvdHouseholder( double* basePt,
+ long colLength, long numCols, long colStride, long rowStride,
+ double* retFirstEntry )
+{
+
+ // Calc norm of vector u
+ double* cPtr = basePt;
+ double norm = 0.0;
+ long i;
+ for ( i=colLength; i>0 ; i-- ) {
+ norm += Square( *cPtr );
+ cPtr += colStride;
+ }
+ norm = sqrt(norm); // Norm of vector to reflect to axis e_1
+
+ // Handle sign issues
+ double imageVal; // Choose sign to maximize distance
+ if ( (*basePt) < 0.0 ) {
+ imageVal = norm;
+ norm = 2.0*norm*(norm-(*basePt));
+ }
+ else {
+ imageVal = -norm;
+ norm = 2.0*norm*(norm+(*basePt));
+ }
+ norm = sqrt(norm); // Norm is norm of reflection vector
+
+ if ( norm==0.0 ) { // If the vector being transformed is equal to zero
+ // Force to zero in case of roundoff errors
+ cPtr = basePt;
+ for ( i=colLength; i>0; i-- ) {
+ *cPtr = 0.0;
+ cPtr += colStride;
+ }
+ *retFirstEntry = 0.0;
+ return;
+ }
+
+ *retFirstEntry = imageVal;
+
+ // Set up the normalized Householder vector
+ *basePt -= imageVal; // First component changes. Rest stay the same.
+ // Normalize the vector
+ norm = 1.0/norm; // Now it is the inverse norm
+ cPtr = basePt;
+ for ( i=colLength; i>0 ; i-- ) {
+ *cPtr *= norm;
+ cPtr += colStride;
+ }
+
+ // Transform the rest of the U matrix with the Householder transformation
+ double *rPtr = basePt;
+ for ( long j=numCols-1; j>0; j-- ) {
+ rPtr += rowStride;
+ // Calc dot product with Householder transformation vector
+ double dotP = DotArray( colLength, basePt, colStride, rPtr, colStride );
+ // Transform with I - 2*dotP*(Householder vector)
+ AddArrayScale( colLength, basePt, colStride, rPtr, colStride, -2.0*dotP );
+ }
+}
+
+// ********************************* ExpandHouseholders ********************************************
+// The matrix will be square.
+// numXforms = number of Householder transformations to concatenate
+// Each Householder transformation is represented by a unit vector
+// Each successive Householder transformation starts one position later
+// and has one more implied leading zero
+// basePt = beginning of the first Householder transform
+// colStride, rowStride: Householder xforms are stored in "columns"
+// numZerosSkipped is the number of implicit zeros on the front each
+// Householder transformation vector (only values supported are 0 and 1).
+void MatrixRmn::ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride )
+{
+ // Number of applications of the last Householder transform
+ // (That are not trivial!)
+ long numToTransform = NumCols-numXforms+1-numZerosSkipped;
+ assert( numToTransform>0 );
+
+ if ( numXforms==0 ) {
+ SetIdentity();
+ return;
+ }
+
+ // Handle the first one separately as a special case,
+ // "this" matrix will be treated to simulate being preloaded with the identity
+ long hDiagStride = rowStride+colStride;
+ const double* hBase = basePt + hDiagStride*(numXforms-1); // Pointer to the last Householder vector
+ const double* hDiagPtr = hBase + colStride*(numToTransform-1); // Pointer to last entry in that vector
+ long i;
+ double* diagPtr = x+NumCols*NumRows-1; // Last entry in matrix (points to diagonal entry)
+ double* colPtr = diagPtr-(numToTransform-1); // Pointer to column in matrix
+ for ( i=numToTransform; i>0; i-- ) {
+ CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hDiagPtr) );
+ *diagPtr += 1.0; // Add back in 1 to the diagonal entry (since xforming the identity)
+ diagPtr -= (NumRows+1); // Next diagonal entry in this matrix
+ colPtr -= NumRows; // Next column in this matrix
+ hDiagPtr -= colStride;
+ }
+
+ // Now handle the general case
+ // A row of zeros must be in effect added to the top of each old column (in each loop)
+ double* colLastPtr = x + NumRows*NumCols - numToTransform - 1;
+ for ( i = numXforms-1; i>0; i-- ) {
+ numToTransform++; // Number of non-trivial applications of this Householder transformation
+ hBase -= hDiagStride; // Pointer to the beginning of the Householder transformation
+ colPtr = colLastPtr;
+ for ( long j = numToTransform-1; j>0; j-- ) {
+ // Get dot product
+ double dotProd2N = -2.0*DotArray( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1 );
+ *colPtr = dotProd2N*(*hBase); // Adding onto zero at initial point
+ AddArrayScale( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1, dotProd2N );
+ colPtr -= NumRows;
+ }
+ // Do last one as a special case (may overwrite the Householder vector)
+ CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hBase) );
+ *colPtr += 1.0; // Add back one one as identity
+ // Done with this Householder transformation
+ colLastPtr --;
+ }
+
+ if ( numZerosSkipped!=0 ) {
+ assert( numZerosSkipped==1 );
+ // Fill first row and column with identity (More generally: first numZerosSkipped many rows and columns)
+ double* d = x;
+ *d = 1;
+ double* d2 = d;
+ for ( i=NumRows-1; i>0; i-- ) {
+ *(++d) = 0;
+ *(d2+=NumRows) = 0;
+ }
+ }
+}
+
+// **************** ConvertBidiagToDiagonal ***********************************************
+// Do the iterative transformation from bidiagonal form to diagonal form using
+// Givens transformation. (Golub-Reinsch)
+// U and V are square. Size of U less than or equal to that of U.
+void MatrixRmn::ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const
+{
+ // These two index into the last bidiagonal block (last in the matrix, it will be
+ // first one handled.
+ long lastBidiagIdx = V.NumRows-1;
+ long firstBidiagIdx = 0;
+ double eps = 1.0e-15 * Max(w.MaxAbs(), superDiag.MaxAbs());
+
+ while ( true ) {
+ bool workLeft = UpdateBidiagIndices( &firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps );
+ if ( !workLeft ) {
+ break;
+ }
+
+ // Get ready for first Givens rotation
+ // Push non-zero to M[2,1] with Givens transformation
+ double* wPtr = w.x+firstBidiagIdx;
+ double* sdPtr = superDiag.x+firstBidiagIdx;
+ double extraOffDiag=0.0;
+ if ( (*wPtr)==0.0 ) {
+ ClearRowWithDiagonalZero( firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps );
+ if ( firstBidiagIdx>0 ) {
+ if ( NearZero( *(--sdPtr), eps ) ) {
+ *sdPtr = 0.0;
+ }
+ else {
+ ClearColumnWithDiagonalZero( firstBidiagIdx, V, wPtr, sdPtr, eps );
+ }
+ }
+ continue;
+ }
+
+ // Estimate an eigenvalue from bottom four entries of M
+ // This gives a lambda value which will shift the Givens rotations
+ // Last four entries of M^T * M are ( ( A, B ), ( B, C ) ).
+ double A;
+ A = (firstBidiagIdx C ) {
+ lambda = -lambda;
+ }
+ lambda += (A+C)*0.5; // Now lambda equals the estimate for the last eigenvalue
+ double t11 = Square(w[firstBidiagIdx]);
+ double t12 = w[firstBidiagIdx]*superDiag[firstBidiagIdx];
+
+ double c, s;
+ CalcGivensValues( t11-lambda, t12, &c, &s );
+ ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, wPtr+1 );
+ V.PostApplyGivens( c, -s, firstBidiagIdx );
+ long i;
+ for ( i=firstBidiagIdx; i 0 ) {
+ if ( NearZero( *wPtr, eps ) ) { // If this diagonal entry (near) zero
+ *wPtr = 0.0;
+ break;
+ }
+ if ( NearZero(*(--sdPtr), eps) ) { // If the entry above the diagonal entry is (near) zero
+ *sdPtr = 0.0;
+ break;
+ }
+ wPtr--;
+ firstIdx--;
+ }
+ *firstBidiagIdx = firstIdx;
+ return true;
+}
+
+
+// ******************************************DEBUG STUFFF
+
+bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const
+{
+ // Special SVD test code
+
+ MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
+ IV.SetIdentity();
+ MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
+ MatrixRmn::TransposeMultiply( V, V, VTV );
+ IV -= VTV;
+ double error = IV.FrobeniusNorm();
+
+ MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
+ IU.SetIdentity();
+ MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
+ MatrixRmn::TransposeMultiply( U, U, UTU );
+ IU -= UTU;
+ error += IU.FrobeniusNorm();
+
+ MatrixRmn Diag( U.GetNumRows(), V.GetNumRows() );
+ Diag.SetZero();
+ Diag.SetDiagonalEntries( w );
+ MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
+ MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
+ MatrixRmn::Multiply( U, Diag, B );
+ MatrixRmn::MultiplyTranspose( B, V, C );
+ C -= *this;
+ error += C.FrobeniusNorm();
+
+ bool ret = ( fabs(error)<=1.0e-13*w.MaxAbs() );
+ assert ( ret );
+ return ret;
+}
+
+bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
+{
+ // Special SVD test code
+
+ MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
+ IV.SetIdentity();
+ MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
+ MatrixRmn::TransposeMultiply( V, V, VTV );
+ IV -= VTV;
+ double error = IV.FrobeniusNorm();
+
+ MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
+ IU.SetIdentity();
+ MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
+ MatrixRmn::TransposeMultiply( U, U, UTU );
+ IU -= UTU;
+ error += IU.FrobeniusNorm();
+
+ MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() );
+ DiagAndSuper.SetZero();
+ DiagAndSuper.SetDiagonalEntries( w );
+ if ( this->GetNumRows()>=this->GetNumColumns() ) {
+ DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 );
+ }
+ else {
+ DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 );
+ }
+ MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
+ MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
+ MatrixRmn::Multiply( U, DiagAndSuper, B );
+ MatrixRmn::MultiplyTranspose( B, V, C );
+ C -= *this;
+ error += C.FrobeniusNorm();
+
+ bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) );
+ assert ( ret );
+ return ret;
+}
+
diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h
new file mode 100644
index 000000000..544dd921e
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h
@@ -0,0 +1,402 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+//
+// MatrixRmn: Matrix over reals (Variable dimensional vector)
+//
+// Not very sophisticated yet. Needs more functionality
+// To do: better handling of resizing.
+//
+
+#ifndef MATRIX_RMN_H
+#define MATRIX_RMN_H
+
+#include
+#include
+#include "LinearR3.h"
+#include "VectorRn.h"
+
+class MatrixRmn {
+
+public:
+ MatrixRmn(); // Null constructor
+ MatrixRmn( long numRows, long numCols ); // Constructor with length
+ ~MatrixRmn(); // Destructor
+
+ void SetSize( long numRows, long numCols );
+ long GetNumRows() const { return NumRows; }
+ long GetNumColumns() const { return NumCols; }
+ void SetZero();
+
+ // Return entry in row i and column j.
+ double Get( long i, long j ) const;
+ void GetTriple( long i, long j, VectorR3 *retValue ) const;
+
+ // Use GetPtr to get pointer into the array (efficient)
+ // Is friendly in that anyone can change the array contents (be careful!)
+ // The entries are in column order!!!
+ // Use this with care. You may call GetRowStride and GetColStride to navigate
+ // within the matrix. I do not expect these values to ever change.
+ const double* GetPtr() const;
+ double* GetPtr();
+ const double* GetPtr( long i, long j ) const;
+ double* GetPtr( long i, long j );
+ const double* GetColumnPtr( long j ) const;
+ double* GetColumnPtr( long j );
+ const double* GetRowPtr( long i ) const;
+ double* GetRowPtr( long i );
+ long GetRowStride() const { return NumRows; } // Step size (stride) along a row
+ long GetColStride() const { return 1; } // Step size (stide) along a column
+
+ void Set( long i, long j, double val );
+ void SetTriple( long i, long c, const VectorR3& u );
+
+ void SetIdentity();
+ void SetDiagonalEntries( double d );
+ void SetDiagonalEntries( const VectorRn& d );
+ void SetSuperDiagonalEntries( double d );
+ void SetSuperDiagonalEntries( const VectorRn& d );
+ void SetSubDiagonalEntries( double d );
+ void SetSubDiagonalEntries( const VectorRn& d );
+ void SetColumn(long i, const VectorRn& d );
+ void SetRow(long i, const VectorRn& d );
+ void SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol );
+
+ // Loads matrix in as a sub-matrix. (i,j) is the base point. Defaults to (0,0).
+ // The "Tranpose" versions load the transpose of A.
+ void LoadAsSubmatrix( const MatrixRmn& A );
+ void LoadAsSubmatrix( long i, long j, const MatrixRmn& A );
+ void LoadAsSubmatrixTranspose( const MatrixRmn& A );
+ void LoadAsSubmatrixTranspose( long i, long j, const MatrixRmn& A );
+
+ // Norms
+ double FrobeniusNormSq() const;
+ double FrobeniusNorm() const;
+
+ // Operations on VectorRn's
+ void Multiply( const VectorRn& v, VectorRn& result ) const; // result = (this)*(v)
+ void MultiplyTranspose( const VectorRn& v, VectorRn& result ) const; // Equivalent to mult by row vector on left
+ double DotProductColumn( const VectorRn& v, long colNum ) const; // Returns dot product of v with i-th column
+
+ // Operations on MatrixRmn's
+ MatrixRmn& operator*=( double );
+ MatrixRmn& operator/=( double d ) { assert(d!=0.0); *this *= (1.0/d); return *this; }
+ MatrixRmn& AddScaled( const MatrixRmn& B, double factor );
+ MatrixRmn& operator+=( const MatrixRmn& B );
+ MatrixRmn& operator-=( const MatrixRmn& B );
+ static MatrixRmn& Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ); // Sets dst = A*B.
+ static MatrixRmn& MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ); // Sets dst = A*(B-tranpose).
+ static MatrixRmn& TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ); // Sets dst = (A-transpose)*B.
+
+ // Miscellaneous operation
+ MatrixRmn& AddToDiagonal( double d ); // Adds d to each diagonal
+
+ // Solving systems of linear equations
+ void Solve( const VectorRn& b, VectorRn* x ) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible.
+
+ // Row Echelon Form and Reduced Row Echelon Form routines
+ // Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables.
+ void ConvertToRefNoFree(); // Converts the matrix in place to row echelon form -- assumption is no free variables will be found
+ void ConvertToRef( int numVars); // Converts the matrix in place to row echelon form -- numVars is number of columns to work with.
+ void ConvertToRef( int numVars, double eps); // Same, but eps is the measure of closeness to zero
+
+ // Givens transformation
+ static void CalcGivensValues( double a, double b, double *c, double *s );
+ void PostApplyGivens( double c, double s, long idx ); // Applies Givens transform to columns idx and idx+1.
+ void PostApplyGivens( double c, double s, long idx1, long idx2 ); // Applies Givens transform to columns idx1 and idx2.
+
+ // Singular value decomposition
+ void ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const;
+ // Good for debugging SVD computations (I recommend this be used for any new application to check for bugs/instability).
+ bool DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const;
+
+ // Some useful routines for experts who understand the inner workings of these classes.
+ inline static double DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB );
+ inline static void CopyArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale );
+ inline static void AddArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale );
+
+private:
+ long NumRows; // Number of rows
+ long NumCols; // Number of columns
+ double *x; // Array of vector entries - stored in column order
+ long AllocSize; // Allocated size of the x array
+
+ static MatrixRmn WorkMatrix; // Temporary work matrix
+ static MatrixRmn& GetWorkMatrix() { return WorkMatrix; }
+ static MatrixRmn& GetWorkMatrix(long numRows, long numCols) { WorkMatrix.SetSize( numRows, numCols ); return WorkMatrix; }
+
+ // Internal helper routines for SVD calculations
+ static void CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag );
+ void ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const;
+ static void SvdHouseholder( double* basePt,
+ long colLength, long numCols, long colStride, long rowStride,
+ double* retFirstEntry );
+ void ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride );
+ static bool UpdateBidiagIndices( long *firstDiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps );
+ static void ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d );
+ static void ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c,
+ double d, double *e, double *f );
+ static void ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx,
+ MatrixRmn& U, double *wPtr, double *sdPtr, double eps );
+ static void ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps );
+ bool DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const;
+};
+
+inline MatrixRmn::MatrixRmn()
+{
+ NumRows = 0;
+ NumCols = 0;
+ x = 0;
+ AllocSize = 0;
+}
+
+inline MatrixRmn::MatrixRmn( long numRows, long numCols )
+{
+ NumRows = 0;
+ NumCols = 0;
+ x = 0;
+ AllocSize = 0;
+ SetSize( numRows, numCols );
+}
+
+inline MatrixRmn::~MatrixRmn()
+{
+ delete x;
+}
+
+// Resize.
+// If the array space is decreased, the information about the allocated length is lost.
+inline void MatrixRmn::SetSize( long numRows, long numCols )
+{
+ assert ( numRows>0 && numCols>0 );
+ long newLength = numRows*numCols;
+ if ( newLength>AllocSize ) {
+ delete x;
+ AllocSize = Max(newLength, AllocSize<<1);
+ x = new double[AllocSize];
+ }
+ NumRows = numRows;
+ NumCols = numCols;
+}
+
+// Zero out the entire vector
+inline void MatrixRmn::SetZero()
+{
+ double* target = x;
+ for ( long i=NumRows*NumCols; i>0; i-- ) {
+ *(target++) = 0.0;
+ }
+}
+
+// Return entry in row i and column j.
+inline double MatrixRmn::Get( long i, long j ) const {
+ assert ( iLoad( x+j*NumRows + ii );
+}
+
+// Get a pointer to the (0,0) entry.
+// The entries are in column order.
+// This version gives read-only pointer
+inline const double* MatrixRmn::GetPtr( ) const
+{
+ return x;
+}
+
+// Get a pointer to the (0,0) entry.
+// The entries are in column order.
+inline double* MatrixRmn::GetPtr( )
+{
+ return x;
+}
+
+// Get a pointer to the (i,j) entry.
+// The entries are in column order.
+// This version gives read-only pointer
+inline const double* MatrixRmn::GetPtr( long i, long j ) const
+{
+ assert ( 0<=i && i0; i-- ) {
+ (*(aPtr++)) *= mult;
+ }
+ return (*this);
+}
+
+inline MatrixRmn& MatrixRmn::AddScaled( const MatrixRmn& B, double factor )
+{
+ assert (NumRows==B.NumRows && NumCols==B.NumCols);
+ double* aPtr = x;
+ double* bPtr = B.x;
+ for ( long i=NumRows*NumCols; i>0; i-- ) {
+ (*(aPtr++)) += (*(bPtr++))*factor;
+ }
+ return (*this);
+}
+
+inline MatrixRmn& MatrixRmn::operator+=( const MatrixRmn& B )
+{
+ assert (NumRows==B.NumRows && NumCols==B.NumCols);
+ double* aPtr = x;
+ double* bPtr = B.x;
+ for ( long i=NumRows*NumCols; i>0; i-- ) {
+ (*(aPtr++)) += *(bPtr++);
+ }
+ return (*this);
+}
+
+inline MatrixRmn& MatrixRmn::operator-=( const MatrixRmn& B )
+{
+ assert (NumRows==B.NumRows && NumCols==B.NumCols);
+ double* aPtr = x;
+ double* bPtr = B.x;
+ for ( long i=NumRows*NumCols; i>0; i-- ) {
+ (*(aPtr++)) -= *(bPtr++);
+ }
+ return (*this);
+}
+
+inline double MatrixRmn::FrobeniusNormSq() const
+{
+ double* aPtr = x;
+ double result = 0.0;
+ for ( long i=NumRows*NumCols; i>0; i-- ) {
+ result += Square( *(aPtr++) );
+ }
+ return result;
+}
+
+// Helper routine to calculate dot product
+inline double MatrixRmn::DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB )
+{
+ double result = 0.0;
+ for ( ; length>0 ; length-- ) {
+ result += (*ptrA)*(*ptrB);
+ ptrA += strideA;
+ ptrB += strideB;
+ }
+ return result;
+}
+
+// Helper routine: copies and scales an array (src and dest may be equal, or overlap)
+inline void MatrixRmn::CopyArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale )
+{
+ for ( ; length>0; length-- ) {
+ *to = (*from)*scale;
+ from += fromStride;
+ to += toStride;
+ }
+}
+
+// Helper routine: adds a scaled array
+// fromArray = toArray*scale.
+inline void MatrixRmn::AddArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale )
+{
+ for ( ; length>0; length-- ) {
+ *to += (*from)*scale;
+ from += fromStride;
+ to += toStride;
+ }
+}
+
+
+
+#endif MATRIX_RMN_H
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp
new file mode 100644
index 000000000..6aa696d55
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp
@@ -0,0 +1,153 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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
+#include "LinearR3.h"
+
+#include "../OpenGLWindow/OpenGLInclude.h"
+
+/****************************************************************
+ Axes
+*****************************************************************/
+static float xx[] = {
+ 0., 1., 0., 1.
+ };
+
+static float xy[] = {
+ -.5, .5, .5, -.5
+ };
+
+static int xorder[] = {
+ 1, 2, -3, 4
+ };
+
+
+static float yx[] = {
+ 0., 0., -.5, .5
+ };
+
+static float yy[] = {
+ 0.f, .6f, 1.f, 1.f
+ };
+
+static int yorder[] = {
+ 1, 2, 3, -2, 4
+ };
+
+
+static float zx[] = {
+ 1., 0., 1., 0., .25, .75
+ };
+
+static float zy[] = {
+ .5, .5, -.5, -.5, 0., 0.
+ };
+
+static int zorder[] = {
+ 1, 2, 3, 4, -5, 6
+ };
+
+#define LENFRAC 0.10
+#define BASEFRAC 1.10
+
+
+/****************************************************************
+ Arrow
+*****************************************************************/
+
+/* size of wings as fraction of length: */
+
+#define WINGS 0.10
+
+
+/* axes: */
+
+#define X 1
+#define Y 2
+#define Z 3
+
+
+/* x, y, z, axes: */
+
+static float axx[3] = { 1., 0., 0. };
+static float ayy[3] = { 0., 1., 0. };
+static float azz[3] = { 0., 0., 1. };
+
+
+
+/* function declarations: */
+
+void cross( float [3], float [3], float [3] );
+float dot( float [3], float [3] );
+float unit( float [3], float [3] );
+
+
+
+
+
+float dot( float v1[3], float v2[3] )
+{
+ return( v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] );
+}
+
+
+
+void
+cross( float v1[3], float v2[3], float vout[3] )
+{
+ float tmp[3];
+
+ tmp[0] = v1[1]*v2[2] - v2[1]*v1[2];
+ tmp[1] = v2[0]*v1[2] - v1[0]*v2[2];
+ tmp[2] = v1[0]*v2[1] - v2[0]*v1[1];
+
+ vout[0] = tmp[0];
+ vout[1] = tmp[1];
+ vout[2] = tmp[2];
+}
+
+
+
+float
+unit( float vin[3], float vout[3] )
+{
+ float dist, f ;
+
+ dist = vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2];
+
+ if( dist > 0.0 )
+ {
+ dist = sqrt( dist );
+ f = 1. / dist;
+ vout[0] = f * vin[0];
+ vout[1] = f * vin[1];
+ vout[2] = f * vin[2];
+ }
+ else
+ {
+ vout[0] = vin[0];
+ vout[1] = vin[1];
+ vout[2] = vin[2];
+ }
+
+ return( dist );
+}
diff --git a/examples/ThirdPartyLibs/BussIK/Node.cpp b/examples/ThirdPartyLibs/BussIK/Node.cpp
new file mode 100644
index 000000000..612cc42d7
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Node.cpp
@@ -0,0 +1,90 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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
+
+
+#include "LinearR3.h"
+#include "MathMisc.h"
+#include "Node.h"
+
+extern int RotAxesOn;
+
+Node::Node(const VectorR3& attach, const VectorR3& v, double size, Purpose purpose, double minTheta, double maxTheta, double restAngle)
+{
+ Node::freezed = false;
+ Node::size = size;
+ Node::purpose = purpose;
+ seqNumJoint = -1;
+ seqNumEffector = -1;
+ Node::attach = attach; // Global attachment point when joints are at zero angle
+ r.Set(0.0, 0.0, 0.0); // r will be updated when this node is inserted into tree
+ Node::v = v; // Rotation axis when joints at zero angles
+ theta = 0.0;
+ Node::minTheta = minTheta;
+ Node::maxTheta = maxTheta;
+ Node::restAngle = restAngle;
+ left = right = realparent = 0;
+}
+
+// Compute the global position of a single node
+void Node::ComputeS(void)
+{
+ Node* y = this->realparent;
+ Node* w = this;
+ s = r; // Initialize to local (relative) position
+ while ( y ) {
+ s.Rotate( y->theta, y->v );
+ y = y->realparent;
+ w = w->realparent;
+ s += w->r;
+ }
+}
+
+// Compute the global rotation axis of a single node
+void Node::ComputeW(void)
+{
+ Node* y = this->realparent;
+ w = v; // Initialize to local rotation axis
+ while (y) {
+ w.Rotate(y->theta, y->v);
+ y = y->realparent;
+ }
+}
+
+
+
+void Node::PrintNode()
+{
+ cerr << "Attach : (" << attach << ")\n";
+ cerr << "r : (" << r << ")\n";
+ cerr << "s : (" << s << ")\n";
+ cerr << "w : (" << w << ")\n";
+ cerr << "realparent : " << realparent->seqNumJoint << "\n";
+}
+
+
+void Node::InitNode()
+{
+ theta = 0.0;
+}
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/Node.h b/examples/ThirdPartyLibs/BussIK/Node.h
new file mode 100644
index 000000000..5a49d278a
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Node.h
@@ -0,0 +1,101 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+#ifndef _CLASS_NODE
+#define _CLASS_NODE
+
+#include "LinearR3.h"
+
+enum Purpose {JOINT, EFFECTOR};
+
+class VectorR3;
+
+class Node {
+
+ friend class Tree;
+
+public:
+ Node(const VectorR3&, const VectorR3&, double, Purpose, double minTheta=-PI, double maxTheta=PI, double restAngle=0.);
+
+
+ void PrintNode();
+ void InitNode();
+
+ const VectorR3& GetAttach() const { return attach; }
+
+ double GetTheta() const { return theta; }
+ double AddToTheta( double& delta ) {
+ double orgTheta = theta;
+ theta += delta;
+#if 0
+ if (theta < minTheta)
+ theta = minTheta;
+ if (theta > maxTheta)
+ theta = maxTheta;
+ double actualDelta = theta - orgTheta;
+ delta = actualDelta;
+#endif
+ return theta; }
+
+ const VectorR3& GetS() const { return s; }
+ const VectorR3& GetW() const { return w; }
+
+ double GetMinTheta() const { return minTheta; }
+ double GetMaxTheta() const { return maxTheta; }
+ double GetRestAngle() const { return restAngle; } ;
+ void SetTheta(double newTheta) { theta = newTheta; }
+ void ComputeS(void);
+ void ComputeW(void);
+
+ bool IsEffector() const { return purpose==EFFECTOR; }
+ bool IsJoint() const { return purpose==JOINT; }
+ int GetEffectorNum() const { return seqNumEffector; }
+ int GetJointNum() const { return seqNumJoint; }
+
+ bool IsFrozen() const { return freezed; }
+ void Freeze() { freezed = true; }
+ void UnFreeze() { freezed = false; }
+
+//private:
+ bool freezed; // Is this node frozen?
+ int seqNumJoint; // sequence number if this node is a joint
+ int seqNumEffector; // sequence number if this node is an effector
+ double size; // size
+ Purpose purpose; // joint / effector / both
+ VectorR3 attach; // attachment point
+ VectorR3 r; // relative position vector
+ VectorR3 v; // rotation axis
+ double theta; // joint angle (radian)
+ double minTheta; // lower limit of joint angle
+ double maxTheta; // upper limit of joint angle
+ double restAngle; // rest position angle
+ VectorR3 s; // GLobal Position
+ VectorR3 w; // Global rotation axis
+ Node* left; // left child
+ Node* right; // right sibling
+ Node* realparent; // pointer to real parent
+
+
+};
+
+#endif
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/Spherical.h b/examples/ThirdPartyLibs/BussIK/Spherical.h
new file mode 100644
index 000000000..5db1862d3
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Spherical.h
@@ -0,0 +1,298 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+
+//
+// Spherical Operations Classes
+//
+//
+// B. SphericalInterpolator
+//
+// OrientationR3
+//
+// A. Quaternion
+//
+// B. RotationMapR3 // Elsewhere
+//
+// C. EulerAnglesR3 // TO DO
+//
+//
+// Functions for spherical operations
+// A. Many routines for rotation and averaging on a sphere
+//
+
+#ifndef SPHERICAL_H
+#define SPHERICAL_H
+
+#include "LinearR3.h"
+#include "LinearR4.h"
+#include "MathMisc.h"
+
+class SphericalInterpolator; // Spherical linear interpolation of vectors
+class SphericalBSpInterpolator; // Spherical Bspline interpolation of vector
+class Quaternion; // Quaternion (x,y,z,w) values.
+class EulerAnglesR3; // Euler Angles
+
+// *****************************************************
+// SphericalInterpolator class *
+// - Does linear interpolation (slerp-ing) *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+
+class SphericalInterpolator {
+
+private:
+ VectorR3 startDir, endDir; // Unit vectors (starting and ending)
+ double startLen, endLen; // Magnitudes of the vectors
+ double rotRate; // Angle between start and end vectors
+
+public:
+ SphericalInterpolator( const VectorR3& u, const VectorR3& v );
+
+ VectorR3 InterValue ( double frac ) const;
+};
+
+
+// ***************************************************************
+// * Quaternion class - prototypes *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+class Quaternion {
+
+public:
+ double x, y, z, w;
+
+public:
+ Quaternion() :x(0.0), y(0.0), z(0.0), w(1.0) {};
+ Quaternion( double, double, double, double );
+
+ inline Quaternion& Set( double xx, double yy, double zz, double ww );
+ inline Quaternion& Set( const VectorR4& );
+ Quaternion& Set( const EulerAnglesR3& );
+ Quaternion& Set( const RotationMapR3& );
+ Quaternion& SetRotate( const VectorR3& );
+
+ Quaternion& SetIdentity(); // Set to the identity map
+ Quaternion Inverse() const; // Return the Inverse
+ Quaternion& Invert(); // Invert this quaternion
+
+ double Angle(); // Angle of rotation
+ double Norm(); // Norm of x,y,z component
+
+ Quaternion& operator*=(const Quaternion&);
+
+};
+
+Quaternion operator*(const Quaternion&, const Quaternion&);
+
+inline Quaternion ToQuat( const VectorR4& v)
+{
+ return Quaternion(v.x,v.y,v.z,v.w);
+}
+
+inline double Quaternion::Norm()
+{
+ return sqrt( x*x + y*y + z*z );
+}
+
+inline double Quaternion::Angle ()
+{
+ double halfAngle = asin(Norm());
+ return halfAngle+halfAngle;
+}
+
+
+// ****************************************************************
+// Solid Geometry Routines *
+// ****************************************************************
+
+// Compute the angle formed by two geodesics on the unit sphere.
+// Three unit vectors u,v,w specify the geodesics u-v and v-w which
+// meet at vertex uv. The angle from v-w to v-u is returned. This
+// is always in the range [0, 2PI).
+double SphereAngle( const VectorR3& u, const VectorR3& v, const VectorR3& w );
+
+// Compute the area of a triangle on the unit sphere. Three unit vectors
+// specify the corners of the triangle in COUNTERCLOCKWISE order.
+inline double SphericalTriangleArea(
+ const VectorR3& u, const VectorR3& v, const VectorR3& w )
+{
+ double AngleA = SphereAngle( u,v,w );
+ double AngleB = SphereAngle( v,w,u );
+ double AngleC = SphereAngle( w,u,v );
+ return ( AngleA+AngleB+AngleC - PI );
+}
+
+
+// ****************************************************************
+// Spherical Mean routines *
+// ****************************************************************
+
+// Weighted sum of vectors
+VectorR3 WeightedSum( long Num, const VectorR3 vv[], const double weights[] );
+VectorR4 WeightedSum( long Num, const VectorR4 vv[], const double weights[] );
+
+// Weighted average of vectors on the sphere.
+// Sum of weights should equal one (but no checking is done)
+VectorR3 ComputeMeanSphere( long Num, const VectorR3 vv[], const double weights[],
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+VectorR3 ComputeMeanSphere( long Num, const VectorR3 vv[], const double weights[],
+ const VectorR3& InitialVec,
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+VectorR4 ComputeMeanSphere( long Num, const VectorR4 vv[], const double weights[],
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+Quaternion ComputeMeanQuat( long Num, const Quaternion qq[], const double weights[],
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+
+// Next functions mostly for internal use.
+// It takes an initial estimate InitialVec (and a flag for
+// indicating quaternions).
+VectorR4 ComputeMeanSphere( long Num, const VectorR4 vv[], const double weights[],
+ const VectorR4& InitialVec, int QuatFlag=0,
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+const int SPHERICAL_NOTQUAT=0;
+const int SPHERICAL_QUAT=1;
+
+// Slow version, mostly for testing
+VectorR3 ComputeMeanSphereSlow( long Num, const VectorR3 vv[], const double weights[],
+ double tolerance = 1.0e-16, double bkuptolerance = 5.0e-16 );
+VectorR4 ComputeMeanSphereSlow( long Num, const VectorR4 vv[], const double weights[],
+ double tolerance = 1.0e-16, double bkuptolerance = 5.0e-16 );
+VectorR3 ComputeMeanSphereOld( long Num, const VectorR3 vv[], const double weights[],
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+VectorR4 ComputeMeanSphereOld( long Num, const VectorR4 vv[], const double weights[],
+ const VectorR4& InitialVec, int QuatFlag,
+ double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 );
+
+// Solves a system of spherical-mean equalities, where the system is
+// given as a tridiagonal matrix.
+void SolveTriDiagSphere ( int numPoints,
+ const double* tridiagvalues, const VectorR3* c,
+ VectorR3* p,
+ double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 );
+void SolveTriDiagSphere ( int numPoints,
+ const double* tridiagvalues, const VectorR4* c,
+ VectorR4* p,
+ double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 );
+
+// The "Slow" version uses a simpler but slower iteration with a linear rate of
+// convergence. The base version uses a Newton iteration with a quadratic
+// rate of convergence.
+void SolveTriDiagSphereSlow ( int numPoints,
+ const double* tridiagvalues, const VectorR3* c,
+ VectorR3* p,
+ double accuracy=1.0e-15, double bkupaccuracy=5.0e-15 );
+void SolveTriDiagSphereSlow ( int numPoints,
+ const double* tridiagvalues, const VectorR4* c,
+ VectorR4* p,
+ double accuracy=1.0e-15, double bkupaccuracy=5.0e-15 );
+
+// The "Unstable" version probably shouldn't be used except for very short sequences
+// of knots. Mostly it's used for testing purposes now.
+void SolveTriDiagSphereUnstable ( int numPoints,
+ const double* tridiagvalues, const VectorR3* c,
+ VectorR3* p,
+ double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 );
+void SolveTriDiagSphereHelperUnstable ( int numPoints,
+ const double* tridiagvalues, const VectorR3 *c,
+ const VectorR3& p0value,
+ VectorR3 *p,
+ double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 );
+
+
+
+// ***************************************************************
+// * Quaternion class - inlined member functions *
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+inline VectorR4::VectorR4 ( const Quaternion& q )
+: x(q.x), y(q.y), z(q.z), w(q.w)
+{}
+
+inline VectorR4& VectorR4::Set ( const Quaternion& q )
+{
+ x = q.x; y = q.y; z = q.z; w = q.w;
+ return *this;
+}
+
+inline Quaternion::Quaternion( double xx, double yy, double zz, double ww)
+: x(xx), y(yy), z(zz), w(ww)
+{}
+
+inline Quaternion& Quaternion::Set( double xx, double yy, double zz, double ww )
+{
+ x = xx;
+ y = yy;
+ z = zz;
+ w = ww;
+ return *this;
+}
+
+inline Quaternion& Quaternion::Set( const VectorR4& u)
+{
+ x = u.x;
+ y = u.y;
+ z = u.z;
+ w = u.w;
+ return *this;
+}
+
+inline Quaternion& Quaternion::SetIdentity()
+{
+ x = y = z = 0.0;
+ w = 1.0;
+ return *this;
+}
+
+inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2)
+{
+ Quaternion q(q1);
+ q *= q2;
+ return q;
+}
+
+inline Quaternion& Quaternion::operator*=( const Quaternion& q )
+{
+ double wnew = w*q.w - (x*q.x + y*q.y + z*q.z);
+ double xnew = w*q.x + q.w*x + (y*q.z - z*q.y);
+ double ynew = w*q.y + q.w*y + (z*q.x - x*q.z);
+ z = w*q.z + q.w*z + (x*q.y - y*q.x);
+ w = wnew;
+ x = xnew;
+ y = ynew;
+ return *this;
+}
+
+inline Quaternion Quaternion::Inverse() const // Return the Inverse
+{
+ return ( Quaternion( x, y, z, -w ) );
+}
+
+inline Quaternion& Quaternion::Invert() // Invert this quaternion
+{
+ w = -w;
+ return *this;
+}
+
+
+#endif // SPHERICAL_H
diff --git a/examples/ThirdPartyLibs/BussIK/Tree.cpp b/examples/ThirdPartyLibs/BussIK/Tree.cpp
new file mode 100644
index 000000000..6891f0601
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Tree.cpp
@@ -0,0 +1,217 @@
+/*
+*
+* Inverse Kinematics software, with several solvers including
+* Selectively Damped Least Squares Method
+* Damped Least Squares Method
+* Pure Pseudoinverse Method
+* Jacobian Transpose Method
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
+*
+*
+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.
+*
+*
+*/
+
+//
+// VectorRn: Vector over Rn (Variable length vector)
+//
+
+#include
+using namespace std;
+
+
+
+
+#include "LinearR3.h"
+#include "Tree.h"
+#include "Node.h"
+
+Tree::Tree()
+{
+ root = 0;
+ nNode = nEffector = nJoint = 0;
+}
+
+void Tree::SetSeqNum(Node* node)
+{
+ switch (node->purpose) {
+ case JOINT:
+ node->seqNumJoint = nJoint++;
+ node->seqNumEffector = -1;
+ break;
+ case EFFECTOR:
+ node->seqNumJoint = -1;
+ node->seqNumEffector = nEffector++;
+ break;
+ }
+}
+
+void Tree::InsertRoot(Node* root)
+{
+ assert( nNode==0 );
+ nNode++;
+ Tree::root = root;
+ root->r = root->attach;
+ assert( !(root->left || root->right) );
+ SetSeqNum(root);
+}
+
+void Tree::InsertLeftChild(Node* parent, Node* child)
+{
+ assert(parent);
+ nNode++;
+ parent->left = child;
+ child->realparent = parent;
+ child->r = child->attach - child->realparent->attach;
+ assert( !(child->left || child->right) );
+ SetSeqNum(child);
+}
+
+void Tree::InsertRightSibling(Node* parent, Node* child)
+{
+ assert(parent);
+ nNode++;
+ parent->right = child;
+ child->realparent = parent->realparent;
+ child->r = child->attach - child->realparent->attach;
+ assert( !(child->left || child->right) );
+ SetSeqNum(child);
+}
+
+// Search recursively below "node" for the node with index value.
+Node* Tree::SearchJoint(Node* node, int index)
+{
+ Node* ret;
+ if (node != 0) {
+ if (node->seqNumJoint == index) {
+ return node;
+ } else {
+ if (ret = SearchJoint(node->left, index)) {
+ return ret;
+ }
+ if (ret = SearchJoint(node->right, index)) {
+ return ret;
+ }
+ return NULL;
+ }
+ }
+ else {
+ return NULL;
+ }
+}
+
+
+// Get the joint with the index value
+Node* Tree::GetJoint(int index)
+{
+ return SearchJoint(root, index);
+}
+
+// Search recursively below node for the end effector with the index value
+Node* Tree::SearchEffector(Node* node, int index)
+{
+ Node* ret;
+ if (node != 0) {
+ if (node->seqNumEffector == index) {
+ return node;
+ } else {
+ if (ret = SearchEffector(node->left, index)) {
+ return ret;
+ }
+ if (ret = SearchEffector(node->right, index)) {
+ return ret;
+ }
+ return NULL;
+ }
+ } else {
+ return NULL;
+ }
+}
+
+
+// Get the end effector for the index value
+Node* Tree::GetEffector(int index)
+{
+ return SearchEffector(root, index);
+}
+
+// Returns the global position of the effector.
+const VectorR3& Tree::GetEffectorPosition(int index)
+{
+ Node* effector = GetEffector(index);
+ assert(effector);
+ return (effector->s);
+}
+
+void Tree::ComputeTree(Node* node)
+{
+ if (node != 0) {
+ node->ComputeS();
+ node->ComputeW();
+ ComputeTree(node->left);
+ ComputeTree(node->right);
+ }
+}
+
+void Tree::Compute(void)
+{
+ ComputeTree(root);
+}
+
+
+void Tree::PrintTree(Node* node)
+{
+ if (node != 0) {
+ node->PrintNode();
+ PrintTree(node->left);
+ PrintTree(node->right);
+ }
+}
+
+void Tree::Print(void)
+{
+ PrintTree(root);
+ cout << "\n";
+}
+
+// Recursively initialize tree below the node
+void Tree::InitTree(Node* node)
+{
+ if (node != 0) {
+ node->InitNode();
+ InitTree(node->left);
+ InitTree(node->right);
+ }
+}
+
+// Initialize all nodes in the tree
+void Tree::Init(void)
+{
+ InitTree(root);
+}
+
+void Tree::UnFreezeTree(Node* node)
+{
+ if (node != 0) {
+ node->UnFreeze();
+ UnFreezeTree(node->left);
+ UnFreezeTree(node->right);
+ }
+}
+
+void Tree::UnFreeze(void)
+{
+ UnFreezeTree(root);
+}
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/Tree.h b/examples/ThirdPartyLibs/BussIK/Tree.h
new file mode 100644
index 000000000..673f41c9e
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/Tree.h
@@ -0,0 +1,92 @@
+/*
+*
+* Inverse Kinematics software, with several solvers including
+* Selectively Damped Least Squares Method
+* Damped Least Squares Method
+* Pure Pseudoinverse Method
+* Jacobian Transpose Method
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
+*
+*
+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 "LinearR3.h"
+#include "Node.h"
+
+#ifndef _CLASS_TREE
+#define _CLASS_TREE
+
+class Tree {
+
+public:
+ Tree();
+
+ int GetNumNode() const { return nNode; }
+ int GetNumEffector() const { return nEffector; }
+ int GetNumJoint() const { return nJoint; }
+ void InsertRoot(Node*);
+ void InsertLeftChild(Node* parent, Node* child);
+ void InsertRightSibling(Node* parent, Node* child);
+
+ // Accessors based on node numbers
+ Node* GetJoint(int);
+ Node* GetEffector(int);
+ const VectorR3& GetEffectorPosition(int);
+
+ // Accessors for tree traversal
+ Node* GetRoot() const { return root; }
+ Node* GetSuccessor ( const Node* ) const;
+ Node* GetParent( const Node* node ) const { return node->realparent; }
+
+ void Compute();
+
+ void Print();
+ void Init();
+ void UnFreeze();
+
+private:
+ Node* root;
+ int nNode; // nNode = nEffector + nJoint
+ int nEffector;
+ int nJoint;
+ void SetSeqNum(Node*);
+ Node* SearchJoint(Node*, int);
+ Node* SearchEffector(Node*, int);
+ void ComputeTree(Node*);
+
+ void PrintTree(Node*);
+ void InitTree(Node*);
+ void UnFreezeTree(Node*);
+};
+
+inline Node* Tree::GetSuccessor ( const Node* node ) const
+{
+ if ( node->left ) {
+ return node->left;
+ }
+ while ( true ) {
+ if ( node->right ) {
+ return ( node->right );
+ }
+ node = node->realparent;
+ if ( !node ) {
+ return 0; // Back to root, finished traversal
+ }
+ }
+}
+
+#endif
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/VectorRn.cpp b/examples/ThirdPartyLibs/BussIK/VectorRn.cpp
new file mode 100644
index 000000000..5d2b563e9
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/VectorRn.cpp
@@ -0,0 +1,46 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+
+//
+// VectorRn: Vector over Rn (Variable length vector)
+//
+
+#include "VectorRn.h"
+
+VectorRn VectorRn::WorkVector;
+
+double VectorRn::MaxAbs () const
+{
+ double result = 0.0;
+ double* t = x;
+ for ( long i = length; i>0; i-- ) {
+ if ( (*t) > result ) {
+ result = *t;
+ }
+ else if ( -(*t) > result ) {
+ result = -(*t);
+ }
+ t++;
+ }
+ return result;
+}
\ No newline at end of file
diff --git a/examples/ThirdPartyLibs/BussIK/VectorRn.h b/examples/ThirdPartyLibs/BussIK/VectorRn.h
new file mode 100644
index 000000000..8357f9997
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/VectorRn.h
@@ -0,0 +1,238 @@
+/*
+*
+* Mathematics Subpackage (VrMath)
+*
+*
+* Author: Samuel R. Buss, sbuss@ucsd.edu.
+* 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.
+*
+*
+*/
+
+//
+// VectorRn: Vector over Rn (Variable length vector)
+//
+// Not very sophisticated yet. Needs more functionality
+// To do: better handling of resizing.
+//
+
+#ifndef VECTOR_RN_H
+#define VECTOR_RN_H
+
+#include
+#include
+#include "LinearR3.h"
+
+class VectorRn {
+ friend class MatrixRmn;
+
+public:
+ VectorRn(); // Null constructor
+ VectorRn( long length ); // Constructor with length
+ ~VectorRn(); // Destructor
+
+ void SetLength( long newLength );
+ long GetLength() const { return length; }
+
+ void SetZero();
+ void Fill( double d );
+ void Load( const double* d );
+ void LoadScaled( const double* d, double scaleFactor );
+ void Set ( const VectorRn& src );
+
+ // Two access methods identical in functionality
+ // Subscripts are ZERO-BASED!!
+ const double& operator[]( long i ) const { assert ( 0<=i && i0 );
+ if ( newLength>AllocLength ) {
+ delete x;
+ AllocLength = Max( newLength, AllocLength<<1 );
+ x = new double[AllocLength];
+ }
+ length = newLength;
+}
+
+// Zero out the entire vector
+inline void VectorRn::SetZero()
+{
+ double* target = x;
+ for ( long i=length; i>0; i-- ) {
+ *(target++) = 0.0;
+ }
+}
+
+// Set the value of the i-th triple of entries in the vector
+inline void VectorRn::SetTriple( long i, const VectorR3& u )
+{
+ long j = 3*i;
+ assert ( 0<=j && j+2 < length );
+ u.Dump( x+j );
+}
+
+inline void VectorRn::Fill( double d )
+{
+ double *to = x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) = d;
+ }
+}
+
+inline void VectorRn::Load( const double* d )
+{
+ double *to = x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) = *(d++);
+ }
+}
+
+inline void VectorRn::LoadScaled( const double* d, double scaleFactor )
+{
+ double *to = x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) = (*(d++))*scaleFactor;
+ }
+}
+
+inline void VectorRn::Set( const VectorRn& src )
+{
+ assert ( src.length == this->length );
+ double* to = x;
+ double* from = src.x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) = *(from++);
+ }
+}
+
+inline VectorRn& VectorRn::operator+=( const VectorRn& src )
+{
+ assert ( src.length == this->length );
+ double* to = x;
+ double* from = src.x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) += *(from++);
+ }
+ return *this;
+}
+
+inline VectorRn& VectorRn::operator-=( const VectorRn& src )
+{
+ assert ( src.length == this->length );
+ double* to = x;
+ double* from = src.x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) -= *(from++);
+ }
+ return *this;
+}
+
+inline void VectorRn::AddScaled (const VectorRn& src, double scaleFactor )
+{
+ assert ( src.length == this->length );
+ double* to = x;
+ double* from = src.x;
+ for ( long i=length; i>0; i-- ) {
+ *(to++) += (*(from++))*scaleFactor;
+ }
+}
+
+inline VectorRn& VectorRn::operator*=( double f )
+{
+ double* target = x;
+ for ( long i=length; i>0; i-- ) {
+ *(target++) *= f;
+ }
+ return *this;
+}
+
+inline double VectorRn::NormSq() const
+{
+ double* target = x;
+ double res = 0.0;
+ for ( long i=length; i>0; i-- ) {
+ res += (*target)*(*target);
+ target++;
+ }
+ return res;
+}
+
+inline double Dot( const VectorRn& u, const VectorRn& v )
+{
+ assert ( u.GetLength() == v.GetLength() );
+ double res = 0.0;
+ const double* p = u.GetPtr();
+ const double* q = v.GetPtr();
+ for ( long i = u.GetLength(); i>0; i-- ) {
+ res += (*(p++))*(*(q++));
+ }
+ return res;
+}
+
+#endif VECTOR_RN_H
\ No newline at end of file
diff --git a/examples/TinyRenderer/tgaimage.h b/examples/TinyRenderer/tgaimage.h
index 7a4dbfae9..9e2cbf877 100644
--- a/examples/TinyRenderer/tgaimage.h
+++ b/examples/TinyRenderer/tgaimage.h
@@ -24,8 +24,10 @@ struct TGAColor {
unsigned char bgra[4];
unsigned char bytespp;
- TGAColor() : bgra(), bytespp(1) {
- for (int i=0; i<4; i++) bgra[i] = 0;
+ TGAColor() : bytespp(1)
+ {
+ for (int i=0; i<4; i++)
+ bgra[i] = 0;
}
TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) {
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 86c7d3a74..ebc1042b4 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -23,9 +23,9 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
- m_desiredVelocity(desiredVelocity),
- m_desiredPosition(0),
- m_kd(1.),
+ m_desiredVelocity(desiredVelocity),
+ m_desiredPosition(0),
+ m_kd(1.),
m_kp(0)
{
@@ -54,10 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
- m_desiredVelocity(desiredVelocity),
- m_desiredPosition(0),
- m_kd(1.),
- m_kp(0)
+ m_desiredVelocity(desiredVelocity),
+ m_desiredPosition(0),
+ m_kd(1.),
+ m_kp(0)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -119,14 +119,14 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof];
- btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
- btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
- btScalar velocityError = (m_desiredVelocity - currentVelocity);
- btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
-
+ btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+ btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+ btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
+ btScalar velocityError = (m_desiredVelocity - currentVelocity);
+ btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
+
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);
constraintRow.m_orgConstraint = this;