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..b0c542249
--- /dev/null
+++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h
@@ -0,0 +1,1102 @@
+/*
+*
+* 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 ( )
+{
+ m12 = m13 = m14 =
+ m21 = m23 = m24 =
+ m31 = m32 = m34 =
+ m41 = m42 = m43 = 0.0;
+ m11 = m22 = m33 = m44 = 1.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..1d3eda690
--- /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
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..99bd67da5
--- /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
diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
index 8479992d5..16b0522ea 100644
--- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
@@ -366,6 +366,8 @@ std::string LoadMtl (
continue;
}
+ linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
+
// Skip leading space.
const char* token = linebuf.c_str();
token += strspn(token, " \t");
diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp
index 650948a1a..b81ce57ed 100644
--- a/examples/TinyRenderer/TinyRenderer.cpp
+++ b/examples/TinyRenderer/TinyRenderer.cpp
@@ -87,13 +87,34 @@ struct Shader : public IShader {
}
};
-
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
+m_segmentationMaskBufferPtr(0),
m_model(0),
m_userData(0),
-m_userIndex(-1)
+m_userIndex(-1),
+m_objectIndex(-1)
+{
+ Vec3f eye(1,1,3);
+ Vec3f center(0,0,0);
+ Vec3f up(0,0,1);
+ m_lightDirWorld.setValue(0,0,0);
+ m_localScaling.setValue(1,1,1);
+ m_modelMatrix = Matrix::identity();
+
+}
+
+
+
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* segmentationMaskBuffer, int objectIndex)
+:m_rgbColorBuffer(rgbColorBuffer),
+m_depthBuffer(depthBuffer),
+m_segmentationMaskBufferPtr(segmentationMaskBuffer),
+m_model(0),
+m_userData(0),
+m_userIndex(-1),
+m_objectIndex(objectIndex)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
@@ -247,6 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer;
+ int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0);
TGAImage& frame = renderData.m_rgbColorBuffer;
@@ -264,7 +286,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
for (int j=0; j<3; j++) {
shader.vertex(i, j);
}
- triangle(shader.varying_tri, shader, frame, &zbuffer[0], renderData.m_viewportMatrix);
+ triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}
diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h
index c6f4f9821..1819790a6 100644
--- a/examples/TinyRenderer/TinyRenderer.h
+++ b/examples/TinyRenderer/TinyRenderer.h
@@ -27,9 +27,11 @@ struct TinyRenderObjectData
//Output
TGAImage& m_rgbColorBuffer;
- b3AlignedObjectArray& m_depthBuffer;
-
- TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer);
+ b3AlignedObjectArray& m_depthBuffer;//required, hence a reference
+ b3AlignedObjectArray* m_segmentationMaskBufferPtr;//optional, hence a pointer
+
+ TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer);
+ TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* segmentationMaskBuffer,int objectIndex);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);
@@ -41,6 +43,7 @@ struct TinyRenderObjectData
void* m_userData;
int m_userIndex;
+ int m_objectIndex;
};
diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp
index 4f23b73e7..639db9b0b 100644
--- a/examples/TinyRenderer/our_gl.cpp
+++ b/examples/TinyRenderer/our_gl.cpp
@@ -59,7 +59,12 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) {
return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator
}
-void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) {
+void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix, int objectIndex)
+{
+ triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0);
+}
+
+void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
@@ -100,6 +105,10 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
bool discard = shader.fragment(bc_clip, color);
if (!discard) {
zbuffer[P.x+P.y*image.get_width()] = frag_depth;
+ if (segmentationMaskBuffer)
+ {
+ segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
+ }
image.set(P.x, P.y, color);
}
}
diff --git a/examples/TinyRenderer/our_gl.h b/examples/TinyRenderer/our_gl.h
index d80156904..378324cbd 100644
--- a/examples/TinyRenderer/our_gl.h
+++ b/examples/TinyRenderer/our_gl.h
@@ -16,7 +16,7 @@ struct IShader {
virtual bool fragment(Vec3f bar, TGAColor &color) = 0;
};
-//void triangle(Vec4f *pts, IShader &shader, TGAImage &image, float *zbuffer);
void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix);
+void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex);
#endif //__OUR_GL_H__
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/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp
index 6802726d9..7482c302d 100644
--- a/examples/Utils/b3Clock.cpp
+++ b/examples/Utils/b3Clock.cpp
@@ -36,6 +36,7 @@ const T& b3ClockMin(const T& a, const T& b)
#else //_WIN32
#include
+#include
#endif //_WIN32
@@ -227,3 +228,21 @@ double b3Clock::getTimeInSeconds()
return double(getTimeMicroseconds()/1.e6);
}
+void b3Clock::usleep(int microSeconds)
+{
+#ifdef _WIN32
+ int millis = microSeconds/1000;
+ if (millis < 1)
+ {
+ millis = 1;
+ }
+ Sleep(millis);
+#else
+
+ usleep(microSeconds);
+ //struct timeval tv;
+ //tv.tv_sec = microSeconds/1000000L;
+ //tv.tv_usec = microSeconds%1000000L;
+ //return select(0, 0, 0, 0, &tv);
+#endif
+}
diff --git a/examples/Utils/b3Clock.h b/examples/Utils/b3Clock.h
index cb1c0c29d..10a36686a 100644
--- a/examples/Utils/b3Clock.h
+++ b/examples/Utils/b3Clock.h
@@ -28,6 +28,10 @@ public:
/// the Clock was created.
double getTimeInSeconds();
+ ///Sleep for 'microSeconds', to yield to other threads and not waste 100% CPU cycles.
+ ///Note that some operating systems may sleep a longer time.
+ static void usleep(int microSeconds);
+
private:
struct b3ClockData* m_data;
};
diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt
index 0d983ac70..e58cabb58 100644
--- a/examples/pybullet/CMakeLists.txt
+++ b/examples/pybullet/CMakeLists.txt
@@ -8,61 +8,60 @@ INCLUDE_DIRECTORIES(
SET(pybullet_SRCS
pybullet.c
- ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
- ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
- ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
- ../../examples/OpenGLWindow/SimpleCamera.cpp
- ../../examples/OpenGLWindow/SimpleCamera.h
- ../../examples/TinyRenderer/geometry.cpp
- ../../examples/TinyRenderer/model.cpp
- ../../examples/TinyRenderer/tgaimage.cpp
- ../../examples/TinyRenderer/our_gl.cpp
- ../../examples/TinyRenderer/TinyRenderer.cpp
- ../../examples/SharedMemory/InProcessMemory.cpp
- ../../examples/SharedMemory/PhysicsClient.cpp
- ../../examples/SharedMemory/PhysicsClient.h
- ../../examples/SharedMemory/PhysicsServer.cpp
- ../../examples/SharedMemory/PhysicsServer.h
- ../../examples/SharedMemory/PhysicsServerExample.cpp
- ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
- ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
- ../../examples/SharedMemory/PhysicsServerSharedMemory.h
- ../../examples/SharedMemory/PhysicsDirect.cpp
- ../../examples/SharedMemory/PhysicsDirect.h
- ../../examples/SharedMemory/PhysicsDirectC_API.cpp
- ../../examples/SharedMemory/PhysicsDirectC_API.h
- ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
- ../../examples/SharedMemory/PhysicsServerCommandProcessor.h
- ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
- ../../examples/SharedMemory/PhysicsClientSharedMemory.h
- ../../examples/SharedMemory/PhysicsClientC_API.cpp
- ../../examples/SharedMemory/PhysicsClientC_API.h
- ../../examples/SharedMemory/Win32SharedMemory.cpp
- ../../examples/SharedMemory/Win32SharedMemory.h
- ../../examples/SharedMemory/PosixSharedMemory.cpp
- ../../examples/SharedMemory/PosixSharedMemory.h
- ../../examples/Utils/b3ResourcePath.cpp
- ../../examples/Utils/b3ResourcePath.h
- ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
- ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
- ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
- ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
- ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
- ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
- ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
- ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
- ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
- ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
- ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
- ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
- ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
- ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
- ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
- ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
- ../../examples/MultiThreading/b3PosixThreadSupport.cpp
- ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
- ../../examples/MultiThreading/b3ThreadSupportInterface.cpp
-
+ ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+ ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+ ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+ ../../examples/OpenGLWindow/SimpleCamera.cpp
+ ../../examples/OpenGLWindow/SimpleCamera.h
+ ../../examples/TinyRenderer/geometry.cpp
+ ../../examples/TinyRenderer/model.cpp
+ ../../examples/TinyRenderer/tgaimage.cpp
+ ../../examples/TinyRenderer/our_gl.cpp
+ ../../examples/TinyRenderer/TinyRenderer.cpp
+ ../../examples/SharedMemory/InProcessMemory.cpp
+ ../../examples/SharedMemory/PhysicsClient.cpp
+ ../../examples/SharedMemory/PhysicsClient.h
+ ../../examples/SharedMemory/PhysicsServer.cpp
+ ../../examples/SharedMemory/PhysicsServer.h
+ ../../examples/SharedMemory/PhysicsServerExample.cpp
+ ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
+ ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
+ ../../examples/SharedMemory/PhysicsServerSharedMemory.h
+ ../../examples/SharedMemory/PhysicsDirect.cpp
+ ../../examples/SharedMemory/PhysicsDirect.h
+ ../../examples/SharedMemory/PhysicsDirectC_API.cpp
+ ../../examples/SharedMemory/PhysicsDirectC_API.h
+ ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+ ../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+ ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
+ ../../examples/SharedMemory/PhysicsClientSharedMemory.h
+ ../../examples/SharedMemory/PhysicsClientC_API.cpp
+ ../../examples/SharedMemory/PhysicsClientC_API.h
+ ../../examples/SharedMemory/Win32SharedMemory.cpp
+ ../../examples/SharedMemory/Win32SharedMemory.h
+ ../../examples/SharedMemory/PosixSharedMemory.cpp
+ ../../examples/SharedMemory/PosixSharedMemory.h
+ ../../examples/Utils/b3ResourcePath.cpp
+ ../../examples/Utils/b3ResourcePath.h
+ ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+ ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+ ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+ ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+ ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+ ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+ ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+ ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+ ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+ ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+ ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+ ../../examples/MultiThreading/b3PosixThreadSupport.cpp
+ ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
+ ../../examples/MultiThreading/b3ThreadSupportInterface.cpp
)
IF(WIN32)
diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua
index 39092b889..cacb7218e 100644
--- a/examples/pybullet/premake4.lua
+++ b/examples/pybullet/premake4.lua
@@ -1,18 +1,16 @@
project ("pybullet")
-
language "C++"
kind "SharedLib"
targetsuffix ("")
targetprefix ("")
- targetextension (".so")
includedirs {"../../src", "../../examples",
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
- links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+ links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
initOpenGL()
initGlew()
@@ -20,10 +18,8 @@ project ("pybullet")
".",
"../../src",
"../ThirdPartyLibs",
- "/usr/include/python2.7",
}
-
if os.is("MacOSX") then
links{"Cocoa.framework","Python"}
end
@@ -40,8 +36,69 @@ project ("pybullet")
files {
"pybullet.c",
- "../../examples/ExampleBrowser/ExampleEntries.cpp",
+ "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
+ "../../examples/OpenGLWindow/SimpleCamera.cpp",
+ "../../examples/OpenGLWindow/SimpleCamera.h",
+ "../../examples/TinyRenderer/geometry.cpp",
+ "../../examples/TinyRenderer/model.cpp",
+ "../../examples/TinyRenderer/tgaimage.cpp",
+ "../../examples/TinyRenderer/our_gl.cpp",
+ "../../examples/TinyRenderer/TinyRenderer.cpp",
+ "../../examples/SharedMemory/InProcessMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClient.cpp",
+ "../../examples/SharedMemory/PhysicsClient.h",
+ "../../examples/SharedMemory/PhysicsServer.cpp",
+ "../../examples/SharedMemory/PhysicsServer.h",
+ "../../examples/SharedMemory/PhysicsServerExample.cpp",
+ "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsDirect.cpp",
+ "../../examples/SharedMemory/PhysicsDirect.h",
+ "../../examples/SharedMemory/PhysicsDirectC_API.cpp",
+ "../../examples/SharedMemory/PhysicsDirectC_API.h",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsClientC_API.cpp",
+ "../../examples/SharedMemory/PhysicsClientC_API.h",
+ "../../examples/SharedMemory/Win32SharedMemory.cpp",
+ "../../examples/SharedMemory/Win32SharedMemory.h",
+ "../../examples/SharedMemory/PosixSharedMemory.cpp",
+ "../../examples/SharedMemory/PosixSharedMemory.h",
+ "../../examples/Utils/b3ResourcePath.cpp",
+ "../../examples/Utils/b3ResourcePath.h",
+ "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+ "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../../examples/MultiThreading/b3PosixThreadSupport.cpp",
+ "../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
+ "../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
}
+
+ includedirs {
+ _OPTIONS["python_include_dir"],
+ }
+ libdirs {
+ _OPTIONS["python_lib_dir"]
+ }
+
if os.is("Linux") then
initX11()
end
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index be5994c16..915de51f1 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -294,27 +294,27 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
- //todo(erwincoumans): set max forces, kp, kd
-
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
+ double targetPosition=0;
+ double targetVelocity=0;
double maxForce=100000;
- double gains=0.1;
+ double kp=0.1;
+ double kd=1.0;
int valid = 0;
-
-
+
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
-
+
size= PySequence_Size(args);
-
if (size==4)
{
-
+ // for CONTROL_MODE_VELOCITY targetValue -> velocity
+ // for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@@ -324,7 +324,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==5)
{
-
+ // for CONTROL_MODE_VELOCITY targetValue -> velocity
+ // for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@@ -334,18 +335,38 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==6)
{
-
- if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
+ if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
-
-
+ if (size==8)
+ {
+ // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
+ if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd))
+ {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+ }
+
+ if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD)
+ {
+ PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD");
+ return NULL;
+ }
+ if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8)
+ {
+ PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity");
+ return NULL;
+ }
+
if (valid)
{
+
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -357,7 +378,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
-
+
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
@@ -365,19 +386,18 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
PyErr_SetString(SpamError, "Illegral control mode.");
return NULL;
}
-
+
commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
-
+
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
-
+
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
- double kd = gains;
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
- b3JointControlSetKd(commandHandle,info.m_uIndex,kd);
- b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
@@ -386,31 +406,30 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
break;
}
-
+
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
- double kp = gains;
- b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
- b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
- b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
+ b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
+ b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+ b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
default:
{
}
};
-
+
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
-
+
Py_INCREF(Py_None);
return Py_None;
}
- PyErr_SetString(SpamError, "error in setJointControl.");
+ PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
return NULL;
}
-
-
static PyObject *
pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
{
@@ -1022,6 +1041,8 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal, fov) -
// set resolution and initialize camera based on camera position, target
// position, camera up, fulstrum near/far values and camera field of view.
+// renderImage(w, h, targetPos, distance, yaw, pitch, upAxisIndex, nearVal, farVal, fov)
+
//
// Note if the (w,h) is too small, the objects may not appear based on
// where the camera has been set
@@ -1132,6 +1153,36 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
aspect = width/height;
b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal);
}
+ }
+ else if (size==11)
+ {
+ int upAxisIndex=1;
+ float camDistance,yaw,pitch,roll;
+
+ //sometimes more arguments are better :-)
+ if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov))
+ {
+
+ b3RequestCameraImageSetPixelResolution(command,width,height);
+ if (pybullet_internalSetVector(objTargetPos, targetPos))
+ {
+ //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov);
+
+ b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,roll,upAxisIndex);
+ aspect = width/height;
+ b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal);
+ } else
+ {
+ PyErr_SetString(SpamError, "Error parsing camera target pos");
+ }
+ } else
+ {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ }
+
+
+
+
}
else
{
@@ -1143,6 +1194,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
+
+ //b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL);
+
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
@@ -1151,11 +1205,13 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
PyObject *pylistRGB;
PyObject* pylistDep;
+ PyObject* pylistSeg;
+
int i, j, p;
b3GetCameraImageData(sm, &imageData);
//TODO(hellojas): error handling if image size is 0
- pyResultList = PyTuple_New(4);
+ pyResultList = PyTuple_New(5);
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
@@ -1167,15 +1223,23 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
pylistRGB = PyTuple_New(num);
pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
-
+ pylistSeg = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
for (i=0;igetWorldTransform().getOrigin();
- btScalar maxPen = btScalar(0.0);
+// btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_manifoldArray.resize(0);
@@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject);
- btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject);
+ btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
+
+ if (!needsCollision(obj0, obj1))
+ continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btScalar dist = pt.getDistance();
- if (dist < 0.0)
+ if (dist < -m_maxPenetrationDepth)
{
- if (dist < maxPen)
- {
- maxPen = dist;
- m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+ // TODO: cause problems on slopes, not sure if it is needed
+ //if (dist < maxPen)
+ //{
+ // maxPen = dist;
+ // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
- }
+ //}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
@@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
+ btScalar stepHeight = 0.0f;
+ if (m_verticalVelocity < 0.0)
+ stepHeight = m_stepHeight;
+
// phase 1: up
btTransform start, end;
- m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
- start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
+ start.setOrigin(m_currentPosition);
+
+ m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
+ m_currentPosition = m_targetPosition;
+
end.setOrigin (m_targetPosition);
- btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+ start.setRotation(m_currentOrientation);
+ end.setRotation(m_targetOrientation);
+
+ btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
}
else
{
- world->convexSweepTest (m_convexShape, start, end, callback);
+ world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
-
- if (callback.hasHit())
+
+ if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
- if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+ if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
- m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+ m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
+
+ btTransform& xform = m_ghostObject->getWorldTransform();
+ xform.setOrigin(m_currentPosition);
+ m_ghostObject->setWorldTransform(xform);
+
+ // fix penetration if we hit a ceiling for example
+ int numPenetrationLoops = 0;
+ m_touchingContact = false;
+ while (recoverFromPenetration(world))
+ {
+ numPenetrationLoops++;
+ m_touchingContact = true;
+ if (numPenetrationLoops > 4)
+ {
+ //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+ break;
+ }
+ }
+ m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
+ m_currentPosition = m_targetPosition;
+
+ if (m_verticalOffset > 0)
+ {
+ m_verticalOffset = 0.0;
+ m_verticalVelocity = 0.0;
+ m_currentStepOffset = m_stepHeight;
+ }
} else {
- m_currentStepOffset = m_stepHeight;
+ m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
+bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+ bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
+ collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
+ return collides;
+}
+
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
@@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
+
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
@@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
- if (m_touchingContact)
- {
- if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
- {
- //interferes with step movement
- //updateTargetPositionBasedOnCollision (m_touchingNormal);
- }
- }
-
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
@@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
+ start.setRotation(m_currentOrientation);
+ end.setRotation(m_targetOrientation);
+
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
-
- if (m_useGhostObjectSweepTest)
+ if (!(start == end))
{
- m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- } else
- {
- collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ if (m_useGhostObjectSweepTest)
+ {
+ m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
+ else
+ {
+ collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
}
-
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
- if (callback.hasHit())
+ if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we moved only a fraction
//btScalar hitDistance;
@@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
}
} else {
- // we moved whole way
- m_currentPosition = m_targetPosition;
+// if (!m_wasJumping)
+ // we moved whole way
+ m_currentPosition = m_targetPosition;
}
+ m_currentPosition = m_targetPosition;
// if (callback.m_closestHitFraction == 0.f)
// break;
@@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+ btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
- btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
+ btVector3 gravity_drop = m_up * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+ if (m_verticalVelocity > 0.0)
+ return;
+
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+ btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
- btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+ btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
- btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+ btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
+ start.setRotation(m_currentOrientation);
+ end.setRotation(m_targetOrientation);
+
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
@@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- if (!callback.hasHit())
+ if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -471,30 +529,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- if (!callback.hasHit())
- {
- //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
- collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- }
+ if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
+ {
+ //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+ collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
if (bounce_fix == true)
- has_hit = callback.hasHit() || callback2.hasHit();
+ has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
- has_hit = callback2.hasHit();
+ has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
- if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+ btScalar stepHeight = 0.0f;
+ if (m_verticalVelocity < 0.0)
+ stepHeight = m_stepHeight;
+
+ if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
- downVelocity = m_stepHeight;
+ downVelocity = stepHeight;
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+ btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
- if (callback.hasHit() || runonce == true)
+ if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we dropped a fraction of the height -> hit floor
-
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,10 +574,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
- else
- //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+ else
+ //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@@ -528,7 +589,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_wasJumping = false;
} else {
// we dropped the full height
-
+
full_drop = true;
if (bounce_fix == true)
@@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
- step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+ step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
@@ -579,21 +640,63 @@ btScalar timeInterval
m_velocityTimeInterval += timeInterval;
}
+void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
+{
+ m_AngVel = velocity;
+}
+
+const btVector3& btKinematicCharacterController::getAngularVelocity() const
+{
+ return m_AngVel;
+}
+
+void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
+{
+ m_walkDirection = velocity;
+
+ // HACK: if we are moving in the direction of the up, treat it as a jump :(
+ if (m_walkDirection.length2() > 0)
+ {
+ btVector3 w = velocity.normalized();
+ btScalar c = w.dot(m_up);
+ if (c != 0)
+ {
+ //there is a component in walkdirection for vertical velocity
+ btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
+ m_walkDirection -= upComponent;
+ m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
+
+ if (c > 0.0f)
+ {
+ m_wasJumping = true;
+ m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+ }
+ }
+ }
+ else
+ m_verticalVelocity = 0.0f;
+}
+
+btVector3 btKinematicCharacterController::getLinearVelocity() const
+{
+ return m_walkDirection + (m_verticalVelocity * m_up);
+}
+
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
{
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
- m_wasOnGround = false;
- m_wasJumping = false;
- m_walkDirection.setValue(0,0,0);
- m_velocityTimeInterval = 0.0;
+ m_verticalVelocity = 0.0;
+ m_verticalOffset = 0.0;
+ m_wasOnGround = false;
+ m_wasJumping = false;
+ m_walkDirection.setValue(0,0,0);
+ m_velocityTimeInterval = 0.0;
- //clear pair cache
- btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
- while (cache->getOverlappingPairArray().size() > 0)
- {
- cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
- }
+ //clear pair cache
+ btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
+ while (cache->getOverlappingPairArray().size() > 0)
+ {
+ cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+ }
}
void btKinematicCharacterController::warp (const btVector3& origin)
@@ -607,62 +710,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
-
- int numPenetrationLoops = 0;
- m_touchingContact = false;
- while (recoverFromPenetration (collisionWorld))
- {
- numPenetrationLoops++;
- m_touchingContact = true;
- if (numPenetrationLoops > 4)
- {
- //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
- break;
- }
- }
-
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
+
+ m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+ m_targetOrientation = m_currentOrientation;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
-
-
}
-#include
-
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
+ if (m_AngVel.length2() > 0.0f)
+ {
+ m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
+ }
+
+ // integrate for angular velocity
+ if (m_AngVel.length2() > 0.0f)
+ {
+ btTransform xform;
+ xform = m_ghostObject->getWorldTransform();
+
+ btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
+
+ btQuaternion orn = rot * xform.getRotation();
+
+ xform.setRotation(orn);
+ m_ghostObject->setWorldTransform(xform);
+
+ m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+ m_targetPosition = m_currentPosition;
+ m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+ m_targetOrientation = m_currentOrientation;
+ }
+
// quick check...
- if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
+ if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
+ //btVector3 lvel = m_walkDirection;
+ btScalar c = 0.0f;
+
+ if (m_walkDirection.length2() > 0)
+ {
+ // apply damping
+ m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
+ }
+
+ m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
+
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
- if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+ if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
- if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+ if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
-
btTransform xform;
- xform = m_ghostObject->getWorldTransform ();
+ xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
- stepUp (collisionWorld);
+ stepUp(collisionWorld);
+ //todo: Experimenting with behavior of controller when it hits a ceiling..
+ //bool hitUp = stepUp (collisionWorld);
+ //if (hitUp)
+ //{
+ // m_verticalVelocity -= m_gravity * dt;
+ // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+ // {
+ // m_verticalVelocity = m_jumpSpeed;
+ // }
+ // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+ // {
+ // m_verticalVelocity = -btFabs(m_fallSpeed);
+ // }
+ // m_verticalOffset = m_verticalVelocity * dt;
+
+ // xform = m_ghostObject->getWorldTransform();
+ //}
+
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
@@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
stepDown (collisionWorld, dt);
+ //todo: Experimenting with max jump height
+ //if (m_wasJumping)
+ //{
+ // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
+ // if (ds > m_maxJumpHeight)
+ // {
+ // // substract the overshoot
+ // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
+
+ // // max height was reached, so potential energy is at max
+ // // and kinematic energy is 0, thus velocity is 0.
+ // if (m_verticalVelocity > 0.0)
+ // m_verticalVelocity = 0.0;
+ // }
+ //}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
+
+ int numPenetrationLoops = 0;
+ m_touchingContact = false;
+ while (recoverFromPenetration(collisionWorld))
+ {
+ numPenetrationLoops++;
+ m_touchingContact = true;
+ if (numPenetrationLoops > 4)
+ {
+ //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+ break;
+ }
+ }
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
+ m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const
return onGround();
}
-void btKinematicCharacterController::jump ()
+void btKinematicCharacterController::jump(const btVector3& v)
{
- if (!canJump())
- return;
-
+ m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
+ m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
+
+ m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+
#if 0
currently no jumping.
btTransform xform;
@@ -727,14 +898,16 @@ void btKinematicCharacterController::jump ()
#endif
}
-void btKinematicCharacterController::setGravity(btScalar gravity)
+void btKinematicCharacterController::setGravity(const btVector3& gravity)
{
- m_gravity = gravity;
+ if (gravity.length2() > 0) setUpVector(-gravity);
+
+ m_gravity = gravity.length();
}
-btScalar btKinematicCharacterController::getGravity() const
+btVector3 btKinematicCharacterController::getGravity() const
{
- return m_gravity;
+ return -m_gravity * m_up;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
return m_maxSlopeRadians;
}
-bool btKinematicCharacterController::onGround () const
+void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
{
- return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+ m_maxPenetrationDepth = d;
}
+btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
+{
+ return m_maxPenetrationDepth;
+}
+
+bool btKinematicCharacterController::onGround () const
+{
+ return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
+}
+
+void btKinematicCharacterController::setStepHeight(btScalar h)
+{
+ m_stepHeight = h;
+}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
@@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}
+
+void btKinematicCharacterController::setUp(const btVector3& up)
+{
+ if (up.length2() > 0 && m_gravity > 0.0f)
+ {
+ setGravity(-m_gravity * up.normalized());
+ return;
+ }
+
+ setUpVector(up);
+}
+
+void btKinematicCharacterController::setUpVector(const btVector3& up)
+{
+ if (m_up == up)
+ return;
+
+ btVector3 u = m_up;
+
+ if (up.length2() > 0)
+ m_up = up.normalized();
+ else
+ m_up = btVector3(0.0, 0.0, 0.0);
+
+ if (!m_ghostObject) return;
+ btQuaternion rot = getRotation(m_up, u);
+
+ //set orientation with new up
+ btTransform xform;
+ xform = m_ghostObject->getWorldTransform();
+ btQuaternion orn = rot.inverse() * xform.getRotation();
+ xform.setRotation(orn);
+ m_ghostObject->setWorldTransform(xform);
+}
+
+btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
+{
+ if (v0.length2() == 0.0f || v1.length2() == 0.0f)
+ {
+ btQuaternion q;
+ return q;
+ }
+
+ return shortestArcQuatNormalize2(v0, v1);
+}
+
diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h
index add6f30a6..3d677e647 100644
--- a/src/BulletDynamics/Character/btKinematicCharacterController.h
+++ b/src/BulletDynamics/Character/btKinematicCharacterController.h
@@ -43,10 +43,12 @@ protected:
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+ btScalar m_maxPenetrationDepth;
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
+ btScalar m_SetjumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@@ -61,24 +63,34 @@ protected:
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
+ btVector3 m_AngVel;
+
+ btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
+ btQuaternion m_currentOrientation;
+ btQuaternion m_targetOrientation;
+
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
+ btScalar m_linearDamping;
+ btScalar m_angularDamping;
+
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
- int m_upAxis;
+ btVector3 m_up;
+ btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
@@ -94,11 +106,18 @@ protected:
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
+
+ virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
+
+ void setUpVector(const btVector3& up);
+
+ btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
+
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
- btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
+ btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
@@ -112,14 +131,9 @@ public:
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
- void setUpAxis (int axis)
- {
- if (axis < 0)
- axis = 0;
- if (axis > 2)
- axis = 2;
- m_upAxis = axis;
- }
+ void setUp(const btVector3& up);
+
+ const btVector3& getUp() { return m_up; }
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
@@ -136,27 +150,47 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
+ virtual void setAngularVelocity(const btVector3& velocity);
+ virtual const btVector3& getAngularVelocity() const;
+
+ virtual void setLinearVelocity(const btVector3& velocity);
+ virtual btVector3 getLinearVelocity() const;
+
+ void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+ btScalar getLinearDamping() const { return m_linearDamping; }
+ void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+ btScalar getAngularDamping() const { return m_angularDamping; }
+
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
+ void setStepHeight(btScalar h);
+ btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
+ btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
+ btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
- void jump ();
+ void jump(const btVector3& v = btVector3());
- void setGravity(btScalar gravity);
- btScalar getGravity() const;
+ void applyImpulse(const btVector3& v) { jump(v); }
+
+ void setGravity(const btVector3& gravity);
+ btVector3 getGravity() const;
/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
+ void setMaxPenetrationDepth(btScalar d);
+ btScalar getMaxPenetrationDepth() const;
+
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index 738d52ce8..57b4e1983 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -319,14 +319,6 @@ protected:
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
- static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
- static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
-
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -489,6 +481,14 @@ public:
//If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
virtual btScalar getParam(int num, int axis = -1) const;
+
+ static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+ static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
};
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index c683a179e..edef315b3 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -299,7 +299,7 @@ void btMultiBody::setupPlanar(int i,
m_links[i].m_eVector = parentComToThisComOffset;
//
- static btVector3 vecNonParallelToRotAxis(1, 0, 0);
+ btVector3 vecNonParallelToRotAxis(1, 0, 0);
if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
vecNonParallelToRotAxis.setValue(0, 1, 0);
//
@@ -466,6 +466,16 @@ btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
}
}
+btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
+{
+ btMatrix3x3 result = local_frame;
+ btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
+ btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
+ btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
+ result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
+ return result;
+}
+
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
{
int num_links = getNumLinks();
@@ -714,15 +724,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
btScalar * Y = &scratch_r[0];
//
//aux variables
- static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
- static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
- static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
- static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
- static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
- static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
- static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
- static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
- static btSpatialTransformationMatrix fromWorld;
+ btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
+ btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
+ btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+ btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+ btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+ btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
+ btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
+ btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
+ btSpatialTransformationMatrix fromWorld;
fromWorld.m_trnVec.setZero();
/////////////////
@@ -919,8 +929,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::eSpherical:
case btMultibodyLink::ePlanar:
{
- static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
- static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+ btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+ btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
//unroll the loop?
for(int row = 0; row < 3; ++row)
@@ -1323,11 +1333,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
btScalar * Y = r_ptr;
////////////////
//aux variables
- static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
- static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
- static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
- static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
- static btSpatialTransformationMatrix fromParent;
+ btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+ btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+ btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+ btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
+ btSpatialTransformationMatrix fromParent;
/////////////////
// First 'upward' loop.
@@ -1522,8 +1532,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
//
- static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
- static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+ btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+ btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
pBaseQuat[0] = baseQuat.x();
pBaseQuat[1] = baseQuat.y();
@@ -1557,8 +1567,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
case btMultibodyLink::eSpherical:
{
- static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
- static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+ btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+ btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
break;
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index f2251a1e3..a676c0227 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -272,7 +272,11 @@ public:
btVector3 localDirToWorld(int i, const btVector3 &vec) const;
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
-
+
+ //
+ // transform a frame in local coordinate to a frame in world coordinate
+ //
+ btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
//
// calculate kinetic energy and angular momentum
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 12997d2e3..119a24c60 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -53,323 +53,359 @@ void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala
}
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
- btMultiBodyJacobianData& data,
- btScalar* jacOrgA, btScalar* jacOrgB,
- const btVector3& contactNormalOnB,
- const btVector3& posAworld, const btVector3& posBworld,
- btScalar posError,
- const btContactSolverInfo& infoGlobal,
- btScalar lowerLimit, btScalar upperLimit,
- btScalar relaxation,
- bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+ btMultiBodyJacobianData& data,
+ btScalar* jacOrgA, btScalar* jacOrgB,
+ const btVector3& constraintNormalAng,
+ const btVector3& constraintNormalLin,
+ const btVector3& posAworld, const btVector3& posBworld,
+ btScalar posError,
+ const btContactSolverInfo& infoGlobal,
+ btScalar lowerLimit, btScalar upperLimit,
+ bool angConstraint,
+ btScalar relaxation,
+ bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
-
-
- solverConstraint.m_multiBodyA = m_bodyA;
- solverConstraint.m_multiBodyB = m_bodyB;
- solverConstraint.m_linkA = m_linkA;
- solverConstraint.m_linkB = m_linkB;
-
- btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
- btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-
- btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
- btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
-
- btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
- btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-
- btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
- if (bodyA)
- rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
- if (bodyB)
- rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
-
- if (multiBodyA)
- {
- if (solverConstraint.m_linkA<0)
- {
- rel_pos1 = posAworld - multiBodyA->getBasePos();
- } else
- {
- rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
- }
-
- const int ndofA = multiBodyA->getNumDofs() + 6;
-
- solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
-
- if (solverConstraint.m_deltaVelAindex <0)
- {
- solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
- multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
- data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
- } else
- {
- btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
- }
-
- //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
- //resize..
- solverConstraint.m_jacAindex = data.m_jacobians.size();
- data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
- //copy/determine
- if(jacOrgA)
- {
- for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
- }
-
- //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
- //resize..
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
- btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- //determine..
- multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-
- btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
- solverConstraint.m_relpos1CrossNormal = torqueAxis0;
- solverConstraint.m_contactNormal1 = contactNormalOnB;
- }
- else //if(rb0)
- {
- btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
- solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
- solverConstraint.m_relpos1CrossNormal = torqueAxis0;
- solverConstraint.m_contactNormal1 = contactNormalOnB;
- }
-
- if (multiBodyB)
- {
- if (solverConstraint.m_linkB<0)
- {
- rel_pos2 = posBworld - multiBodyB->getBasePos();
- } else
- {
- rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
- }
-
- const int ndofB = multiBodyB->getNumDofs() + 6;
-
- solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
- if (solverConstraint.m_deltaVelBindex <0)
- {
- solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
- multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
- data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
- }
-
- //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
- //resize..
- solverConstraint.m_jacBindex = data.m_jacobians.size();
- data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
- //copy/determine..
- if(jacOrgB)
- {
- for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
- }
-
- //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
- //resize..
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
- btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
- //determine..
- multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-
- btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
- solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
- solverConstraint.m_contactNormal2 = -contactNormalOnB;
-
- }
- else //if(rb1)
- {
- btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
- solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
- solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
- solverConstraint.m_contactNormal2 = -contactNormalOnB;
- }
- {
-
- btVector3 vec;
- btScalar denom0 = 0.f;
- btScalar denom1 = 0.f;
- btScalar* jacB = 0;
- btScalar* jacA = 0;
- btScalar* deltaVelA = 0;
- btScalar* deltaVelB = 0;
- int ndofA = 0;
- //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
- if (multiBodyA)
- {
- ndofA = multiBodyA->getNumDofs() + 6;
- jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
- deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- for (int i = 0; i < ndofA; ++i)
- {
- btScalar j = jacA[i] ;
- btScalar l = deltaVelA[i];
- denom0 += j*l;
- }
- }
- else if(rb0)
- {
- vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
- denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
- }
- //
- if (multiBodyB)
- {
- const int ndofB = multiBodyB->getNumDofs() + 6;
- jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
- deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
- for (int i = 0; i < ndofB; ++i)
- {
- btScalar j = jacB[i] ;
- btScalar l = deltaVelB[i];
- denom1 += j*l;
- }
-
- }
- else if(rb1)
- {
- vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
- denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
- }
-
- //
- btScalar d = denom0+denom1;
- if (d>SIMD_EPSILON)
- {
- solverConstraint.m_jacDiagABInv = relaxation/(d);
- }
- else
- {
- //disable the constraint row to handle singularity/redundant constraint
- solverConstraint.m_jacDiagABInv = 0.f;
- }
- }
-
-
- //compute rhs and remaining solverConstraint fields
- btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
-
- btScalar rel_vel = 0.f;
- int ndofA = 0;
- int ndofB = 0;
- {
- btVector3 vel1,vel2;
- if (multiBodyA)
- {
- ndofA = multiBodyA->getNumDofs() + 6;
- btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
- for (int i = 0; i < ndofA ; ++i)
- rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
- }
- else if(rb0)
- {
- rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
- }
- if (multiBodyB)
- {
- ndofB = multiBodyB->getNumDofs() + 6;
- btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
- for (int i = 0; i < ndofB ; ++i)
- rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
- }
- else if(rb1)
- {
- rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
- }
-
- solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
- }
-
-
- ///warm starting (or zero if disabled)
- /*
- if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
- {
- solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
- if (solverConstraint.m_appliedImpulse)
- {
- if (multiBodyA)
- {
- btScalar impulse = solverConstraint.m_appliedImpulse;
- btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- multiBodyA->applyDeltaVee(deltaV,impulse);
- applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
- } else
- {
- if (rb0)
+ solverConstraint.m_multiBodyA = m_bodyA;
+ solverConstraint.m_multiBodyB = m_bodyB;
+ solverConstraint.m_linkA = m_linkA;
+ solverConstraint.m_linkB = m_linkB;
+
+ btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+ btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+ btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+ btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+
+ btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+ btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
+ btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
+ if (bodyA)
+ rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
+ if (bodyB)
+ rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
+
+ if (multiBodyA)
+ {
+ if (solverConstraint.m_linkA<0)
+ {
+ rel_pos1 = posAworld - multiBodyA->getBasePos();
+ } else
+ {
+ rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofA = multiBodyA->getNumDofs() + 6;
+
+ solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+ if (solverConstraint.m_deltaVelAindex <0)
+ {
+ solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+ multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+ data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+ } else
+ {
+ btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+ }
+
+ //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+ //resize..
+ solverConstraint.m_jacAindex = data.m_jacobians.size();
+ data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+ //copy/determine
+ if(jacOrgA)
+ {
+ for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+ multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+ }
+
+ //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+ //resize..
+ data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+ btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+ btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ //determine..
+ multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+
+ btVector3 torqueAxis0;
+ if (angConstraint) {
+ torqueAxis0 = constraintNormalAng;
+ }
+ else {
+ torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+
+ }
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = constraintNormalLin;
+ }
+ else //if(rb0)
+ {
+ btVector3 torqueAxis0;
+ if (angConstraint) {
+ torqueAxis0 = constraintNormalAng;
+ }
+ else {
+ torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+ }
+ solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = constraintNormalLin;
+ }
+
+ if (multiBodyB)
+ {
+ if (solverConstraint.m_linkB<0)
+ {
+ rel_pos2 = posBworld - multiBodyB->getBasePos();
+ } else
+ {
+ rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofB = multiBodyB->getNumDofs() + 6;
+
+ solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+ if (solverConstraint.m_deltaVelBindex <0)
+ {
+ solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+ multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+ data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+ }
+
+ //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+ //resize..
+ solverConstraint.m_jacBindex = data.m_jacobians.size();
+ data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+ //copy/determine..
+ if(jacOrgB)
+ {
+ for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+ multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+ }
+
+ //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+ //resize..
+ data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+ btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+ btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ //determine..
+ multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+
+ btVector3 torqueAxis1;
+ if (angConstraint) {
+ torqueAxis1 = constraintNormalAng;
+ }
+ else {
+ torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+ }
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -constraintNormalLin;
+ }
+ else //if(rb1)
+ {
+ btVector3 torqueAxis1;
+ if (angConstraint) {
+ torqueAxis1 = constraintNormalAng;
+ }
+ else {
+ torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+ }
+ solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -constraintNormalLin;
+ }
+ {
+
+ btVector3 vec;
+ btScalar denom0 = 0.f;
+ btScalar denom1 = 0.f;
+ btScalar* jacB = 0;
+ btScalar* jacA = 0;
+ btScalar* deltaVelA = 0;
+ btScalar* deltaVelB = 0;
+ int ndofA = 0;
+ //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
+ if (multiBodyA)
+ {
+ ndofA = multiBodyA->getNumDofs() + 6;
+ jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+ deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ for (int i = 0; i < ndofA; ++i)
+ {
+ btScalar j = jacA[i] ;
+ btScalar l = deltaVelA[i];
+ denom0 += j*l;
+ }
+ }
+ else if(rb0)
+ {
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ if (angConstraint) {
+ denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
+ }
+ else {
+ denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
+ }
+ }
+ //
+ if (multiBodyB)
+ {
+ const int ndofB = multiBodyB->getNumDofs() + 6;
+ jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+ deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ for (int i = 0; i < ndofB; ++i)
+ {
+ btScalar j = jacB[i] ;
+ btScalar l = deltaVelB[i];
+ denom1 += j*l;
+ }
+
+ }
+ else if(rb1)
+ {
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ if (angConstraint) {
+ denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
+ }
+ else {
+ denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
+ }
+ }
+
+ //
+ btScalar d = denom0+denom1;
+ if (d>SIMD_EPSILON)
+ {
+ solverConstraint.m_jacDiagABInv = relaxation/(d);
+ }
+ else
+ {
+ //disable the constraint row to handle singularity/redundant constraint
+ solverConstraint.m_jacDiagABInv = 0.f;
+ }
+ }
+
+
+ //compute rhs and remaining solverConstraint fields
+ btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
+
+ btScalar rel_vel = 0.f;
+ int ndofA = 0;
+ int ndofB = 0;
+ {
+ btVector3 vel1,vel2;
+ if (multiBodyA)
+ {
+ ndofA = multiBodyA->getNumDofs() + 6;
+ btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+ for (int i = 0; i < ndofA ; ++i)
+ rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+ }
+ else if(rb0)
+ {
+ rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+ }
+ if (multiBodyB)
+ {
+ ndofB = multiBodyB->getNumDofs() + 6;
+ btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+ for (int i = 0; i < ndofB ; ++i)
+ rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+ }
+ else if(rb1)
+ {
+ rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+ }
+
+ solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+ }
+
+
+ ///warm starting (or zero if disabled)
+ /*
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+ {
+ solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+
+ if (solverConstraint.m_appliedImpulse)
+ {
+ if (multiBodyA)
+ {
+ btScalar impulse = solverConstraint.m_appliedImpulse;
+ btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ multiBodyA->applyDeltaVee(deltaV,impulse);
+ applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
+ } else
+ {
+ if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
- }
- if (multiBodyB)
- {
- btScalar impulse = solverConstraint.m_appliedImpulse;
- btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
- multiBodyB->applyDeltaVee(deltaV,impulse);
- applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
- } else
- {
- if (rb1)
+ }
+ if (multiBodyB)
+ {
+ btScalar impulse = solverConstraint.m_appliedImpulse;
+ btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ multiBodyB->applyDeltaVee(deltaV,impulse);
+ applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
+ } else
+ {
+ if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
- }
- }
- } else
- */
-
- solverConstraint.m_appliedImpulse = 0.f;
- solverConstraint.m_appliedPushImpulse = 0.f;
-
- {
-
- btScalar positionalError = 0.f;
- btScalar velocityError = desiredVelocity - rel_vel;// * damping;
-
-
- btScalar erp = infoGlobal.m_erp2;
- if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
- {
- erp = infoGlobal.m_erp;
- }
-
- positionalError = -penetration * erp/infoGlobal.m_timeStep;
-
- btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
- btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-
- if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
- {
- //combine position and velocity into rhs
- solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
- solverConstraint.m_rhsPenetration = 0.f;
-
- } else
- {
- //split position and velocity into rhs and m_rhsPenetration
- solverConstraint.m_rhs = velocityImpulse;
- solverConstraint.m_rhsPenetration = penetrationImpulse;
- }
-
- solverConstraint.m_cfm = 0.f;
- solverConstraint.m_lowerLimit = lowerLimit;
- solverConstraint.m_upperLimit = upperLimit;
- }
-
- return rel_vel;
-
+ }
+ }
+ } else
+ */
+
+ solverConstraint.m_appliedImpulse = 0.f;
+ solverConstraint.m_appliedPushImpulse = 0.f;
+
+ {
+
+ btScalar positionalError = 0.f;
+ btScalar velocityError = desiredVelocity - rel_vel;// * damping;
+
+
+ btScalar erp = infoGlobal.m_erp2;
+ if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+ {
+ erp = infoGlobal.m_erp;
+ }
+
+ positionalError = -penetration * erp/infoGlobal.m_timeStep;
+
+ btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+ btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
+ if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+ {
+ //combine position and velocity into rhs
+ solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+ solverConstraint.m_rhsPenetration = 0.f;
+
+ } else
+ {
+ //split position and velocity into rhs and m_rhsPenetration
+ solverConstraint.m_rhs = velocityImpulse;
+ solverConstraint.m_rhsPenetration = penetrationImpulse;
+ }
+
+ solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_lowerLimit = lowerLimit;
+ solverConstraint.m_upperLimit = upperLimit;
+ }
+
+ return rel_vel;
+
}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 137b34d87..74c6f5a81 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -66,15 +66,19 @@ protected:
btAlignedObjectArray m_data;
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
+
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
- btScalar* jacOrgA, btScalar* jacOrgB,
- const btVector3& contactNormalOnB,
+ btScalar* jacOrgA, btScalar* jacOrgB,
+ const btVector3& constraintNormalAng,
+
+ const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
- btScalar lowerLimit, btScalar upperLimit,
+ btScalar lowerLimit, btScalar upperLimit,
+ bool angConstraint = false,
+
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
new file mode 100644
index 000000000..0f0d9f67b
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -0,0 +1,211 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
+
+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.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyFixedConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBFIXEDCONSTRAINT_DIM 6
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+ :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(bodyB),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB)
+{
+ m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
+}
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+ :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(0),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB)
+{
+ m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
+}
+
+void btMultiBodyFixedConstraint::finalizeMultiDof()
+{
+ //not implemented yet
+ btAssert(0);
+}
+
+btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
+{
+}
+
+
+int btMultiBodyFixedConstraint::getIslandIdA() const
+{
+ if (m_rigidBodyA)
+ return m_rigidBodyA->getIslandTag();
+
+ if (m_bodyA)
+ {
+ btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+ if (col)
+ return col->getIslandTag();
+ for (int i=0;igetNumLinks();i++)
+ {
+ if (m_bodyA->getLink(i).m_collider)
+ return m_bodyA->getLink(i).m_collider->getIslandTag();
+ }
+ }
+ return -1;
+}
+
+int btMultiBodyFixedConstraint::getIslandIdB() const
+{
+ if (m_rigidBodyB)
+ return m_rigidBodyB->getIslandTag();
+ if (m_bodyB)
+ {
+ btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+ if (col)
+ return col->getIslandTag();
+
+ for (int i=0;igetNumLinks();i++)
+ {
+ col = m_bodyB->getLink(i).m_collider;
+ if (col)
+ return col->getIslandTag();
+ }
+ }
+ return -1;
+}
+
+void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+ int numDim = BTMBFIXEDCONSTRAINT_DIM;
+ for (int i=0;igetCompanionId();
+ pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
+ frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
+
+ } else
+ {
+ if (m_bodyA) {
+ pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+ frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+ }
+ }
+ btVector3 pivotBworld = m_pivotInB;
+ btMatrix3x3 frameBworld = m_frameInB;
+ if (m_rigidBodyB)
+ {
+ constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+ pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
+ frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
+
+ } else
+ {
+ if (m_bodyB) {
+ pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+ frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
+ }
+ }
+
+ btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
+ btVector3 angleDiff;
+ btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
+
+ btVector3 constraintNormalLin(0,0,0);
+ btVector3 constraintNormalAng(0,0,0);
+ btScalar posError = 0.0;
+ if (i < 3) {
+ constraintNormalLin[i] = -1;
+ posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+ constraintNormalLin, pivotAworld, pivotBworld,
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse
+ );
+ }
+ else { //i>=3
+ constraintNormalAng = frameAworld.getColumn(i%3);
+ posError = angleDiff[i%3];
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+ constraintNormalLin, pivotAworld, pivotBworld,
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse, true
+ );
+ }
+ }
+}
+
+void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+ btTransform tr;
+ tr.setIdentity();
+
+ if (m_rigidBodyA)
+ {
+ btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+ tr.setOrigin(pivot);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_bodyA)
+ {
+ btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+ tr.setOrigin(pivotAworld);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_rigidBodyB)
+ {
+ // that ideally should draw the same frame
+ btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+ tr.setOrigin(pivot);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_bodyB)
+ {
+ btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+ tr.setOrigin(pivotBworld);
+ drawer->drawTransform(tr, 0.1);
+ }
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
new file mode 100644
index 000000000..26e28a74e
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
@@ -0,0 +1,94 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
+
+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.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
+#define BT_MULTIBODY_FIXED_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyFixedConstraint : public btMultiBodyConstraint
+{
+protected:
+
+ btRigidBody* m_rigidBodyA;
+ btRigidBody* m_rigidBodyB;
+ btVector3 m_pivotInA;
+ btVector3 m_pivotInB;
+ btMatrix3x3 m_frameInA;
+ btMatrix3x3 m_frameInB;
+
+public:
+
+ btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+ btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+ virtual ~btMultiBodyFixedConstraint();
+
+ virtual void finalizeMultiDof();
+
+ virtual int getIslandIdA() const;
+ virtual int getIslandIdB() const;
+
+ virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+ btMultiBodyJacobianData& data,
+ const btContactSolverInfo& infoGlobal);
+
+ const btVector3& getPivotInA() const
+ {
+ return m_pivotInA;
+ }
+
+ void setPivotInA(const btVector3& pivotInA)
+ {
+ m_pivotInA = pivotInA;
+ }
+
+ const btVector3& getPivotInB() const
+ {
+ return m_pivotInB;
+ }
+
+ void setPivotInB(const btVector3& pivotInB)
+ {
+ m_pivotInB = pivotInB;
+ }
+
+ const btMatrix3x3& getFrameInA() const
+ {
+ return m_frameInA;
+ }
+
+ void setFrameInA(const btMatrix3x3& frameInA)
+ {
+ m_frameInA = frameInA;
+ }
+
+ const btMatrix3x3& getFrameInB() const
+ {
+ return m_frameInB;
+ }
+
+ void setFrameInB(const btMatrix3x3& frameInB)
+ {
+ m_frameInB = frameInB;
+ }
+
+ virtual void debugDraw(class btIDebugDraw* drawer);
+
+};
+
+#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 3f05aa4d5..707817673 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
@@ -122,7 +122,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
const btScalar posError = 0; //why assume it's zero?
const btVector3 dummy(0, 0, 0);
- btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+ btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
{
//expect either prismatic or revolute joint type for now
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 86c7d3a74..326a6ac48 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,16 +119,16 @@ 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);
+
+ fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index 2ccb9827d..3e28f80df 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -159,7 +159,7 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
- fillMultiBodyConstraint(constraintRow, data, 0, 0,
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
index d81647b68..53f88c4b7 100644
--- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
+++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
@@ -14,6 +14,7 @@ class vec3;
class vecx;
class mat33;
typedef btMatrixX matxx;
+typedef btVectorX vecxx;
class vec3 : public btVector3 {
public:
@@ -46,11 +47,11 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s)
inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
-class vecx : public btVectorX {
+class vecx : public vecxx {
public:
- vecx(int size) : btVectorX(size) {}
- const vecx& operator=(const btVectorX& rhs) {
- *static_cast*>(this) = rhs;
+ vecx(int size) : vecxx(size) {}
+ const vecx& operator=(const vecxx& rhs) {
+ *static_cast(this) = rhs;
return *this;
}
diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt
index d39f36df4..b7bc9a255 100644
--- a/test/SharedMemory/CMakeLists.txt
+++ b/test/SharedMemory/CMakeLists.txt
@@ -13,7 +13,7 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA
LINK_LIBRARIES(
- BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest
+ BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest
)
IF (NOT WIN32)
diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua
index 952b4c98a..466ad76a4 100644
--- a/test/SharedMemory/premake4.lua
+++ b/test/SharedMemory/premake4.lua
@@ -36,6 +36,8 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_LOOP_BACK"}
links {
+ "BulletInverseDynamicsUtils",
+ "BulletInverseDynamics",
"BulletFileLoader",
"BulletWorldImporter",
"Bullet3Common",
@@ -104,6 +106,8 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_SERVER_DIRECT"}
links {
+ "BulletInverseDynamicsUtils",
+ "BulletInverseDynamics",
"BulletFileLoader",
"BulletWorldImporter",
"Bullet3Common",
@@ -213,55 +217,55 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp",
- "../../examples/SharedMemory/PhysicsClient.h",
- "../../examples/SharedMemory/PhysicsServer.cpp",
- "../../examples/SharedMemory/PhysicsServer.h",
+ "../../examples/SharedMemory/PhysicsClient.h",
+ "../../examples/SharedMemory/PhysicsServer.cpp",
+ "../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
- "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
- "../../examples/SharedMemory/PhysicsServerSharedMemory.h",
- "../../examples/SharedMemory/PhysicsDirect.cpp",
- "../../examples/SharedMemory/PhysicsDirect.h",
- "../../examples/SharedMemory/PhysicsDirectC_API.cpp",
- "../../examples/SharedMemory/PhysicsDirectC_API.h",
- "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
- "../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
- "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
- "../../examples/SharedMemory/PhysicsClientSharedMemory.h",
- "../../examples/SharedMemory/PhysicsClientC_API.cpp",
- "../../examples/SharedMemory/PhysicsClientC_API.h",
- "../../examples/SharedMemory/Win32SharedMemory.cpp",
- "../../examples/SharedMemory/Win32SharedMemory.h",
- "../../examples/SharedMemory/PosixSharedMemory.cpp",
- "../../examples/SharedMemory/PosixSharedMemory.h",
- "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
- "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
- "../../examples/TinyRenderer/geometry.cpp",
- "../../examples/TinyRenderer/model.cpp",
- "../../examples/TinyRenderer/tgaimage.cpp",
- "../../examples/TinyRenderer/our_gl.cpp",
- "../../examples/TinyRenderer/TinyRenderer.cpp",
- "../../examples/Utils/b3ResourcePath.cpp",
- "../../examples/Utils/b3ResourcePath.h",
- "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
- "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
- "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
- "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
- "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
- "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
- "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
- "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
- "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
- "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
- "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
- "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
- "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
- "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsDirect.cpp",
+ "../../examples/SharedMemory/PhysicsDirect.h",
+ "../../examples/SharedMemory/PhysicsDirectC_API.cpp",
+ "../../examples/SharedMemory/PhysicsDirectC_API.h",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsClientC_API.cpp",
+ "../../examples/SharedMemory/PhysicsClientC_API.h",
+ "../../examples/SharedMemory/Win32SharedMemory.cpp",
+ "../../examples/SharedMemory/Win32SharedMemory.h",
+ "../../examples/SharedMemory/PosixSharedMemory.cpp",
+ "../../examples/SharedMemory/PosixSharedMemory.h",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
+ "../../examples/TinyRenderer/geometry.cpp",
+ "../../examples/TinyRenderer/model.cpp",
+ "../../examples/TinyRenderer/tgaimage.cpp",
+ "../../examples/TinyRenderer/our_gl.cpp",
+ "../../examples/TinyRenderer/TinyRenderer.cpp",
+ "../../examples/Utils/b3ResourcePath.cpp",
+ "../../examples/Utils/b3ResourcePath.h",
+ "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
- "../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
+ "../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
- "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
}
if os.is("Linux") then
initX11()