Add inverse kinematics example with implementations by Sam Buss.

Uses Kuka IIWA model description and 4 methods:
Selectively Damped Least Squares,Damped Least Squares,
Jacobi Transpose, Jacobi Pseudo Inverse
Tweak some PD values in Inverse Dynamics example and Robot example.

examples/ExampleBrowser/CMakeLists.txt

@@ -155,6 +155,8 @@ SET(BulletExampleBrowser_SRCS
 	../BasicDemo/BasicExample.h
 	../InverseDynamics/InverseDynamicsExample.cpp
 	../InverseDynamics/InverseDynamicsExample.h
+	../InverseKinematics/InverseKinematicsExample.cpp
+	../InverseKinematics/InverseKinematicsExample.h
 	../ForkLift/ForkLiftDemo.cpp
 	../ForkLift/ForkLiftDemo.h
 	../Tutorial/Tutorial.cpp
@@ -298,6 +300,17 @@ SET(BulletExampleBrowser_SRCS
 
 	../ThirdPartyLibs/stb_image/stb_image.cpp
 	../ThirdPartyLibs/stb_image/stb_image.h
+	
+	../ThirdPartyLibs/BussIK/Jacobian.cpp
+	../ThirdPartyLibs/BussIK/Tree.cpp
+	../ThirdPartyLibs/BussIK/Node.cpp
+	../ThirdPartyLibs/BussIK/LinearR2.cpp
+	../ThirdPartyLibs/BussIK/LinearR3.cpp
+	../ThirdPartyLibs/BussIK/LinearR4.cpp
+	../ThirdPartyLibs/BussIK/MatrixRmn.cpp
+	../ThirdPartyLibs/BussIK/VectorRn.cpp
+	../ThirdPartyLibs/BussIK/Misc.cpp
+	
 
 	../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
 	../ThirdPartyLibs/tinyxml/tinystr.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -46,6 +46,7 @@
 #include "../MultiThreading/MultiThreadingExample.h"
 #include "../InverseDynamics/InverseDynamicsExample.h"
 #include "../RoboticsLearning/R2D2GraspExample.h"
+#include "../InverseKinematics/InverseKinematicsExample.h"
 
 #ifdef ENABLE_LUA
 #include "../LuaDemo/LuaPhysicsSetup.h"
@@ -95,7 +96,6 @@ struct ExampleEntry
 static ExampleEntry gDefaultExamples[]=
 {
 
-
 	ExampleEntry(0,"API"),
 
 	ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
@@ -124,14 +124,21 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
 	ExampleEntry(1,"TestPendulum","Simulate a pendulum using btMultiBody with a constant joint torque applied. The same code is also used as a unit test comparing Bullet with the numerical solution of second-order non-linear differential equation stored in pendulum_gold.h", TestPendulumCreateFunc),
 
- ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
+	ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 	ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
-	  ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0),
+	ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0),
 
 
-	 ExampleEntry(0,"Inverse Dynamics"),
-      ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
-      ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
+	ExampleEntry(0,"Inverse Dynamics"),
+    ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
+    ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
+
+	ExampleEntry(0, "Inverse Kinematics"),
+	ExampleEntry(1, "SDLS", "Selectively Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_SDLS),
+	ExampleEntry(1, "DLS", "Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS),
+	ExampleEntry(1, "Jacobi Transpose", "Jacobi Transpose by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_JACOB_TRANS),
+	ExampleEntry(1, "Jacobi Pseudo Inv", "Jacobi Pseudo Inverse Method by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_PURE_PSEUDO),
+
 
 	ExampleEntry(0,"Tutorial"),
 	ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY),

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -800,6 +800,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
         s_app = new SimpleOpenGL2App(title,width,height);
         s_app->m_renderer = new SimpleOpenGL2Renderer(width,height);
     } 
+
 #ifndef NO_OPENGL3
 	else
     {

examples/ExampleBrowser/premake4.lua

@@ -45,9 +45,10 @@ project "App_BulletExampleBrowser"
 	defines {"INCLUDE_CLOTH_DEMOS"}
 
         files {
+        	
         "main.cpp",
         "ExampleEntries.cpp",
-        
+        "../InverseKinematics/*",
 		"../TinyRenderer/geometry.cpp",
 		"../TinyRenderer/model.cpp",
 		"../TinyRenderer/tgaimage.cpp",
@@ -116,6 +117,7 @@ project "App_BulletExampleBrowser"
 		"../ThirdPartyLibs/stb_image/*",
 		"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
 		"../ThirdPartyLibs/tinyxml/*",
+		"../ThirdPartyLibs/BussIK/*",
 		"../GyroscopicDemo/GyroscopicSetup.cpp",
 		"../GyroscopicDemo/GyroscopicSetup.h",
         "../ThirdPartyLibs/tinyxml/tinystr.cpp",

examples/InverseDynamics/InverseDynamicsExample.cpp

@@ -157,7 +157,7 @@ void InverseDynamicsExample::initPhysics()
 
 			
             BulletURDFImporter u2b(m_guiHelper,0);
-            bool loadOk =  u2b.loadURDF("kuka_lwr/kuka.urdf");
+			bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
             if (loadOk)
             {
                     int rootLinkIndex = u2b.getRootLinkIndex();
@@ -225,13 +225,13 @@ void InverseDynamicsExample::initPhysics()
 			{
 				qd[dof] = 0;
 				char tmp[25];
-				sprintf(tmp,"q_desired[%zu]",dof);
+				sprintf(tmp,"q_desired[%u]",dof);
 				qd_name[dof] = tmp;
 				SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
 				slider.m_minVal=-3.14;
 				slider.m_maxVal=3.14;
 				
-				sprintf(tmp,"q[%zu]",dof); 
+				sprintf(tmp,"q[%u]",dof); 
 				q_name[dof] = tmp;   
 				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 				btVector4 color = sJointCurveColors[dof&7];
@@ -340,6 +340,21 @@ void InverseDynamicsExample::stepSimulation(float deltaTime)
         // todo(thomas) check that this is correct:
         // want to advance by 10ms, with 1ms timesteps
         m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
+		btAlignedObjectArray<btQuaternion> scratch_q;
+		btAlignedObjectArray<btVector3> scratch_m;
+		m_multiBody->forwardKinematics(scratch_q, scratch_m);
+		for (int i = 0; i < m_multiBody->getNumLinks(); i++)
+		{
+			//btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
+			btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
+			btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
+			btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
+			//printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
+
+			
+
+
+		}
     }
 }
 

examples/InverseKinematics/InverseKinematicsExample.cpp

@@ -0,0 +1,382 @@
+#include "InverseKinematicsExample.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../OpenGLWindow/OpenGLInclude.h"
+
+#include "BussIK/Node.h"
+#include "BussIK/Tree.h"
+#include "BussIK/Jacobian.h"
+#include "BussIK/VectorRn.h"
+
+#define RADIAN(X)	((X)*RadiansToDegrees)
+
+#define MAX_NUM_NODE	1000
+#define MAX_NUM_THETA	1000
+#define MAX_NUM_EFFECT	100
+
+double T = 0;
+VectorR3 target1[MAX_NUM_EFFECT];
+
+
+
+// Make slowdown factor larger to make the simulation take larger, less frequent steps
+// Make the constant factor in Tstep larger to make time pass more quickly
+//const int SlowdownFactor = 40;
+const int SlowdownFactor = 0;		// Make higher to take larger steps less frequently
+const int SleepsPerStep=SlowdownFactor;
+int SleepCounter=0;
+//const double Tstep = 0.0005*(double)SlowdownFactor;		// Time step
+
+
+int	AxesList;		/* list to hold the axes		*/
+int	AxesOn;			/* ON or OFF				*/
+
+float	Scale, Scale2;		/* scaling factors			*/
+
+
+
+int JointLimitsOn;
+int RestPositionOn;
+int UseJacobianTargets1;
+
+
+int numIteration = 1;
+double error = 0.0;
+double errorDLS = 0.0;
+double errorSDLS = 0.0;
+double sumError = 0.0;
+double sumErrorDLS = 0.0;
+double sumErrorSDLS = 0.0;
+
+#ifdef _DYNAMIC
+bool initMaxDist = true;
+extern double Excess[];
+extern double dsnorm[];
+#endif
+
+
+
+
+void Reset(Tree &tree, Jacobian* m_ikJacobian)
+{
+	AxesOn = false;
+	
+	Scale  = 1.0;
+	Scale2 = 0.0;		/* because add 1. to it in Display()	*/
+	
+	JointLimitsOn = true;
+	RestPositionOn = false;
+	UseJacobianTargets1 = false;
+	
+	                  
+	tree.Init();
+	tree.Compute();
+	m_ikJacobian->Reset();
+
+}
+
+// Update target positions
+
+void UpdateTargets( double T2, Tree & treeY) {
+	double T = T2 / 20.;
+	target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
+}
+
+
+// Does a single update (on one kind of tree)
+void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
+	
+	if ( SleepCounter==0 ) {
+		T += Tstep;
+		UpdateTargets( T , treeY);
+	} 
+
+	if ( UseJacobianTargets1 ) {
+		jacob->SetJtargetActive();
+	}
+	else {
+		jacob->SetJendActive();
+	}
+	jacob->ComputeJacobian();						// Set up Jacobian and deltaS vectors
+
+	// Calculate the change in theta values 
+	switch (ikMethod) {
+		case IK_JACOB_TRANS:
+			jacob->CalcDeltaThetasTranspose();		// Jacobian transpose method
+			break;
+		case IK_DLS:
+			jacob->CalcDeltaThetasDLS();			// Damped least squares method
+			break;
+		case IK_PURE_PSEUDO:
+			jacob->CalcDeltaThetasPseudoinverse();	// Pure pseudoinverse method
+			break;
+		case IK_SDLS:
+			jacob->CalcDeltaThetasSDLS();			// Selectively damped least squares method
+			break;
+		default:
+			jacob->ZeroDeltaThetas();
+			break;
+	}
+
+	if ( SleepCounter==0 ) {
+		jacob->UpdateThetas();							// Apply the change in the theta values
+		jacob->UpdatedSClampValue();
+		SleepCounter = SleepsPerStep;
+	}
+	else { 
+		SleepCounter--;
+	}
+
+
+}
+
+
+
+
+
+
+
+
+///quick demo showing the right-handed coordinate system and positive rotations around each axis
+class InverseKinematicsExample : public CommonExampleInterface
+{
+    CommonGraphicsApp* m_app;
+	int m_ikMethod;
+	Tree m_ikTree;
+	b3AlignedObjectArray<Node*> m_ikNodes;
+	Jacobian* m_ikJacobian;
+
+    float m_x;
+    float m_y;
+    float m_z;
+	b3AlignedObjectArray<int> m_movingInstances;
+	int m_targetInstance;
+	enum
+	{
+		numCubesX = 20,
+		numCubesY = 20
+	};
+public:
+    
+    InverseKinematicsExample(CommonGraphicsApp* app, int option)
+    :m_app(app),
+    m_x(0),
+    m_y(0),
+	m_z(0),
+	m_targetInstance(-1),
+	m_ikMethod(option)
+    {
+		m_app->setUpAxis(2);
+        
+		 {
+             b3Vector3 extents=b3MakeVector3(100,100,100);
+             extents[m_app->getUpAxis()]=1;
+             
+			 int xres = 20;
+			 int yres = 20;
+			 
+			 b3Vector4 color0=b3MakeVector4(0.4, 0.4, 0.4,1);
+			 b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
+            m_app->registerGrid(xres, yres, color0, color1);
+        }
+		 
+		 ///create some graphics proxy for the tracking target 
+		 ///the endeffector tries to track it using Inverse Kinematics
+		 {
+			int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
+			b3Vector3 pos = b3MakeVector3(1,1,1);
+			pos[app->getUpAxis()] = 1;
+			b3Quaternion orn(0, 0, 0, 1);
+			b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
+			b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
+			m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling);
+		 }
+		 m_app->m_renderer->writeTransforms();
+    }
+    virtual ~InverseKinematicsExample()
+    {
+        m_app->m_renderer->enableBlend(false);
+    }
+
+    
+    virtual void physicsDebugDraw(int debugDrawMode)
+    {
+        
+    }
+    virtual void    initPhysics()
+    {
+		BuildKukaIIWAShape();
+		m_ikJacobian = new Jacobian(&m_ikTree);
+		Reset(m_ikTree,m_ikJacobian);
+    }
+    virtual void    exitPhysics()
+    {
+		delete m_ikJacobian;
+		m_ikJacobian = 0;
+    }
+
+	void BuildKukaIIWAShape();
+
+	void getLocalTransform(const Node* node, b3Transform& act)
+	{
+		b3Vector3 axis = b3MakeVector3(node->v.x, node->v.y, node->v.z);
+		b3Quaternion rot(0, 0, 0, 1);
+		if (axis.length())
+		{
+			rot = b3Quaternion (axis, node->theta);
+		}
+		act.setIdentity();
+		act.setRotation(rot);
+		act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z));
+	}
+	void MyDrawTree(Node* node, const b3Transform& tr)
+	{
+		b3Vector3 lineColor = b3MakeVector3(0, 0, 0);
+		int lineWidth = 2;
+		if (node != 0) {
+		//	glPushMatrix();
+			b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
+			b3Vector3 color = b3MakeVector3(0, 1, 0);
+			int pointSize = 10;
+			m_app->m_renderer->drawPoint(pos, color, pointSize);
+
+			m_app->m_renderer->drawLine(pos, pos+ 0.05*tr.getBasis().getColumn(0), b3MakeVector3(1,0,0), lineWidth);
+			m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth);
+			m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth);
+			
+			b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z);
+			b3Vector3 axisWorld = tr.getBasis()*axisLocal;
+
+			m_app->m_renderer->drawLine(pos, pos + 0.1*axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
+
+			//node->DrawNode(node == root);	// Recursively draw node and update ModelView matrix
+			if (node->left) {
+				b3Transform act;
+				getLocalTransform(node->left, act);
+				
+				b3Transform trl = tr*act;
+				m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
+				MyDrawTree(node->left, trl);		// Draw tree of children recursively
+			}
+		//	glPopMatrix();
+			if (node->right) {
+				b3Transform act;
+				getLocalTransform(node->right, act);
+				b3Transform trr = tr*act;
+				m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth);
+				MyDrawTree(node->right,trr);		// Draw right siblings recursively
+			}
+		}
+
+	}
+    virtual void	stepSimulation(float deltaTime)
+    {
+		DoUpdateStep(deltaTime, m_ikTree, m_ikJacobian, m_ikMethod);
+    }
+    virtual void	renderScene()
+    {
+		
+		
+		b3Transform act;
+		getLocalTransform(m_ikTree.GetRoot(), act);
+		MyDrawTree(m_ikTree.GetRoot(), act);
+		
+		b3Vector3 pos = b3MakeVector3(target1[0].x, target1[0].y, target1[0].z);
+		b3Quaternion orn(0, 0, 0, 1);
+
+		m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);
+		m_app->m_renderer->writeTransforms();
+		m_app->m_renderer->renderScene();
+    }
+
+	
+    virtual void	physicsDebugDraw()
+    {
+      		
+    }
+    virtual bool	mouseMoveCallback(float x,float y)
+    {
+		return false;   
+    }
+    virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+    {
+        return false;   
+    }
+    virtual bool	keyboardCallback(int key, int state)
+    {
+        return false;   
+    }
+    
+	virtual void resetCamera()
+	{
+		float dist = 1.3;
+		float pitch = 120;
+		float yaw = 13;
+		float targetPos[3]={-0.35,0.14,0.25};
+		if (m_app->m_renderer  && m_app->m_renderer->getActiveCamera())
+		{
+			
+			m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+			m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+			m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+			m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+		}
+	}
+
+};
+
+void InverseKinematicsExample::BuildKukaIIWAShape()
+{
+	const VectorR3& unitx = VectorR3::UnitX;
+	const VectorR3& unity = VectorR3::UnitY;
+	const VectorR3& unitz = VectorR3::UnitZ;
+	const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
+	const VectorR3& zero = VectorR3::Zero;
+
+	float minTheta = -4 * PI;
+	float maxTheta = 4 * PI;
+
+	m_ikNodes.resize(8);//7DOF+additional endeffector
+
+	m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
+	m_ikTree.InsertRoot(m_ikNodes[0]);
+
+	m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
+	m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]);
+
+	m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]);
+
+	m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]);
+
+	m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]);
+
+	m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]);
+
+	m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+	m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]);
+
+	m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
+	m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]);
+
+}
+
+
+class	CommonExampleInterface*    InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)
+{
+	return new InverseKinematicsExample(options.m_guiHelper->getAppInterface(), options.m_option);
+}
+
+
+
+
+

examples/InverseKinematics/InverseKinematicsExample.h

@@ -0,0 +1,8 @@
+#ifndef INVERSE_KINEMATICSEXAMPLE_H
+#define INVERSE_KINEMATICSEXAMPLE_H
+
+enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS };
+
+class CommonExampleInterface*    InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif //INVERSE_KINEMATICSEXAMPLE_H

examples/SharedMemory/PhysicsClientExample.cpp

@@ -171,10 +171,10 @@ protected:
             
             b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
             b3JointControlSetDesiredVelocity(commandHandle,uIndex,0);
-            b3JointControlSetKp(commandHandle, qIndex, 0.01);
-            b3JointControlSetKd(commandHandle, uIndex, 0.1);
+            b3JointControlSetKp(commandHandle, qIndex, 0.2);
+            b3JointControlSetKd(commandHandle, uIndex, 1.);
             
-            b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
+            b3JointControlSetMaximumForce(commandHandle,uIndex,5000);
         }
 	}
 	virtual void	physicsDebugDraw(int debugFlags)

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -0,0 +1,640 @@
+/*
+*
+* 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://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 <stdlib.h>
+#include <math.h>
+#include <assert.h>
+#include <iostream>
+using namespace std;
+
+#include "../OpenGLWindow/OpenGLInclude.h"
+
+#include "Jacobian.h"
+
+void Arrow(const VectorR3& tail, const VectorR3& head);
+
+//extern RestPositionOn;
+extern VectorR3 target1[];
+
+// Optimal damping values have to be determined in an ad hoc manner  (Yuck!)
+const double Jacobian::DefaultDampingLambda = 0.6;		// Optimal for the "Y" shape (any lower gives jitter)
+//const double Jacobian::DefaultDampingLambda = 1.1;			// Optimal for the DLS "double Y" shape (any lower gives jitter)
+// const double Jacobian::DefaultDampingLambda = 0.7;			// Optimal for the DLS "double Y" shape with distance clamping (lower gives jitter)
+
+const double Jacobian::PseudoInverseThresholdFactor = 0.01;
+const double Jacobian::MaxAngleJtranspose = 30.0*DegreesToRadians;
+const double Jacobian::MaxAnglePseudoinverse = 5.0*DegreesToRadians;
+const double Jacobian::MaxAngleDLS = 45.0*DegreesToRadians;
+const double Jacobian::MaxAngleSDLS = 45.0*DegreesToRadians;
+const double Jacobian::BaseMaxTargetDist = 0.4;
+
+Jacobian::Jacobian(Tree* tree)
+{
+	Jacobian::tree = tree;
+	nEffector = tree->GetNumEffector();
+	nJoint = tree->GetNumJoint();
+	nRow = 3 * nEffector;
+	nCol = nJoint;
+
+	Jend.SetSize(nRow, nCol);				// The Jocobian matrix
+	Jend.SetZero();
+	Jtarget.SetSize(nRow, nCol);			// The Jacobian matrix based on target positions
+	Jtarget.SetZero();
+	SetJendActive();
+
+	U.SetSize(nRow, nRow);				// The U matrix for SVD calculations
+	w .SetLength(Min(nRow, nCol));
+	V.SetSize(nCol, nCol);				// The V matrix for SVD calculations
+
+	dS.SetLength(nRow);			// (Target positions) - (End effector positions)
+	dTheta.SetLength(nCol);		// Changes in joint angles
+	dPreTheta.SetLength(nCol);
+
+	// Used by Jacobian transpose method & DLS & SDLS
+	dT1.SetLength(nRow);			// Linearized change in end effector positions based on dTheta
+
+	// Used by the Selectively Damped Least Squares Method
+	//dT.SetLength(nRow);
+	dSclamp.SetLength(nEffector);
+	errorArray.SetLength(nEffector);
+	Jnorms.SetSize(nEffector, nCol);		// Holds the norms of the active J matrix
+
+	Reset();
+}
+
+void Jacobian::Reset() 
+{
+	// Used by Damped Least Squares Method
+	DampingLambda = DefaultDampingLambda;
+	DampingLambdaSq = Square(DampingLambda);
+	// DampingLambdaSDLS = 1.5*DefaultDampingLambda;
+	
+	dSclamp.Fill(HUGE_VAL);
+}
+
+// Compute the deltaS vector, dS, (the error in end effector positions
+// Compute the J and K matrices (the Jacobians)
+void Jacobian::ComputeJacobian() 
+{
+	// Traverse tree to find all end effectors
+	VectorR3 temp;
+	Node* n = tree->GetRoot();
+	while ( n ) {	
+		if ( n->IsEffector() ) {
+			int i = n->GetEffectorNum();
+			const VectorR3& targetPos = target1[i];
+
+			// Compute the delta S value (differences from end effectors to target positions.
+			temp = targetPos;
+			temp -= n->GetS();
+			dS.SetTriple(i, temp);
+
+			// Find all ancestors (they will usually all be joints)
+			// Set the corresponding entries in the Jacobians J, K.
+			Node* m = tree->GetParent(n);
+			while ( m ) {
+				int j = m->GetJointNum();
+				assert ( 0 <=i && i<nEffector && 0<=j && j<nJoint );
+				if ( m->IsFrozen() ) {
+					Jend.SetTriple(i, j, VectorR3::Zero);
+					Jtarget.SetTriple(i, j, VectorR3::Zero);
+				}
+				else {
+					temp = m->GetS();			// joint pos.
+					temp -= n->GetS();			// -(end effector pos. - joint pos.)
+					temp *= m->GetW();			// cross product with joint rotation axis
+					Jend.SetTriple(i, j, temp);	
+					temp = m->GetS();			// joint pos.
+					temp -= targetPos;		// -(target pos. - joint pos.)
+					temp *= m->GetW();			// cross product with joint rotation axis
+					Jtarget.SetTriple(i, j, temp);	
+				}
+				m = tree->GetParent( m );
+			}
+		}
+		n = tree->GetSuccessor( n );
+	}
+}
+
+// The delta theta values have been computed in dTheta array
+// Apply the delta theta values to the joints
+// Nothing is done about joint limits for now.
+void Jacobian::UpdateThetas() 
+{
+	// Traverse the tree to find all joints
+	// Update the joint angles
+	Node* n = tree->GetRoot();
+	while ( n ) {	
+		if ( n->IsJoint() ) {
+			int i = n->GetJointNum();
+			n->AddToTheta( dTheta[i] );
+
+		}
+		n = tree->GetSuccessor( n );
+	}
+	
+	// Update the positions and rotation axes of all joints/effectors
+	tree->Compute();
+}
+
+void Jacobian::CalcDeltaThetas() 
+{
+	switch (CurrentUpdateMode) {
+		case JACOB_Undefined:
+			ZeroDeltaThetas();
+			break;
+		case JACOB_JacobianTranspose:
+			CalcDeltaThetasTranspose();
+			break;
+		case JACOB_PseudoInverse:
+			CalcDeltaThetasPseudoinverse();
+			break;
+		case JACOB_DLS:
+			CalcDeltaThetasDLS();
+			break;
+		case JACOB_SDLS:
+			CalcDeltaThetasSDLS();
+			break;
+	}
+}
+
+void Jacobian::ZeroDeltaThetas()
+{
+	dTheta.SetZero();
+}
+
+// Find the delta theta values using inverse Jacobian method
+// Uses a greedy method to decide scaling factor
+void Jacobian::CalcDeltaThetasTranspose()
+{
+	const MatrixRmn& J = ActiveJacobian();
+
+	J.MultiplyTranspose( dS, dTheta );
+
+	// Scale back the dTheta values greedily 
+	J.Multiply ( dTheta, dT1 );						// dT = J * dTheta
+	double alpha = Dot(dS,dT1) / dT1.NormSq();
+	assert ( alpha>0.0 );
+	// Also scale back to be have max angle change less than MaxAngleJtranspose
+	double maxChange = dTheta.MaxAbs();
+	double beta = MaxAngleJtranspose/maxChange;
+	dTheta *= Min(alpha, beta);
+
+}
+
+void Jacobian::CalcDeltaThetasPseudoinverse() 
+{	
+	MatrixRmn& J = const_cast<MatrixRmn&>(ActiveJacobian());
+
+	// Compute Singular Value Decomposition 
+	//	This an inefficient way to do Pseudoinverse, but it is convenient since we need SVD anyway
+
+	J.ComputeSVD( U, w, V );
+	
+	// Next line for debugging only
+    assert(J.DebugCheckSVD(U, w , V));
+
+	// Calculate response vector dTheta that is the DLS solution.
+	//	Delta target values are the dS values
+	//  We multiply by Moore-Penrose pseudo-inverse of the J matrix
+	double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs();
+
+	long diagLength = w.GetLength();
+	double* wPtr = w.GetPtr();
+	dTheta.SetZero();
+	for ( long i=0; i<diagLength; i++ ) {		
+		double dotProdCol = U.DotProductColumn( dS, i );		// Dot product with i-th column of U
+		double alpha = *(wPtr++);
+		if ( fabs(alpha)>pseudoInverseThreshold ) {
+			alpha = 1.0/alpha;
+			MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha );
+		}
+	}
+
+	// Scale back to not exceed maximum angle changes
+	double maxChange = dTheta.MaxAbs();
+	if ( maxChange>MaxAnglePseudoinverse ) {
+		dTheta *= MaxAnglePseudoinverse/maxChange;
+	}
+
+}
+
+void Jacobian::CalcDeltaThetasDLS() 
+{	
+	const MatrixRmn& J = ActiveJacobian();
+
+	MatrixRmn::MultiplyTranspose(J, J, U);		// U = J * (J^T)
+	U.AddToDiagonal( DampingLambdaSq );
+	
+	// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
+	// CalcdTClampedFromdS();
+	// VectorRn dTextra(3*nEffector);
+	// U.Solve( dT, &dTextra );
+	// J.MultiplyTranspose( dTextra, dTheta );
+	
+	// Use these two lines for the traditional DLS method
+	U.Solve( dS, &dT1 );
+	J.MultiplyTranspose( dT1, dTheta );
+
+	// Scale back to not exceed maximum angle changes
+	double maxChange = dTheta.MaxAbs();
+	if ( maxChange>MaxAngleDLS ) {
+		dTheta *= MaxAngleDLS/maxChange;
+	}
+}
+
+void Jacobian::CalcDeltaThetasDLSwithSVD() 
+{	
+	const MatrixRmn& J = ActiveJacobian();
+
+	// Compute Singular Value Decomposition 
+	//	This an inefficient way to do DLS, but it is convenient since we need SVD anyway
+
+	J.ComputeSVD( U, w, V );
+	
+	// Next line for debugging only
+    assert(J.DebugCheckSVD(U, w , V));
+
+	// Calculate response vector dTheta that is the DLS solution.
+	//	Delta target values are the dS values
+	//  We multiply by DLS inverse of the J matrix
+	long diagLength = w.GetLength();
+	double* wPtr = w.GetPtr();
+	dTheta.SetZero();
+	for ( long i=0; i<diagLength; i++ ) {		
+		double dotProdCol = U.DotProductColumn( dS, i );		// Dot product with i-th column of U
+		double alpha = *(wPtr++);
+		alpha = alpha/(Square(alpha)+DampingLambdaSq);
+		MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha );
+	}
+
+	// Scale back to not exceed maximum angle changes
+	double maxChange = dTheta.MaxAbs();
+	if ( maxChange>MaxAngleDLS ) {
+		dTheta *= MaxAngleDLS/maxChange;
+	}
+}
+
+
+void Jacobian::CalcDeltaThetasSDLS() 
+{	
+	const MatrixRmn& J = ActiveJacobian();
+
+	// Compute Singular Value Decomposition 
+
+	J.ComputeSVD( U, w, V );
+
+	// Next line for debugging only
+    assert(J.DebugCheckSVD(U, w , V));
+
+	// Calculate response vector dTheta that is the SDLS solution.
+	//	Delta target values are the dS values
+	int nRows = J.GetNumRows();
+	int numEndEffectors = tree->GetNumEffector();		// Equals the number of rows of J divided by three
+	int nCols = J.GetNumColumns();
+	dTheta.SetZero();
+
+	// Calculate the norms of the 3-vectors in the Jacobian
+	long i;
+	const double *jx = J.GetPtr();
+	double *jnx = Jnorms.GetPtr();
+	for ( i=nCols*numEndEffectors; i>0; i-- ) {
+		double accumSq = Square(*(jx++));
+		accumSq += Square(*(jx++));
+		accumSq += Square(*(jx++));
+		*(jnx++) = sqrt(accumSq);
+	}
+
+	// Clamp the dS values
+	CalcdTClampedFromdS();
+
+	// Loop over each singular vector
+	for ( i=0; i<nRows; i++ ) {
+
+		double wiInv = w[i];
+		if ( NearZero(wiInv,1.0e-10) ) {
+			continue;
+		}
+		wiInv = 1.0/wiInv;
+
+		double N = 0.0;						// N is the quasi-1-norm of the i-th column of U
+		double alpha = 0.0;					// alpha is the dot product of dT and the i-th column of U
+
+		const double *dTx = dT1.GetPtr();
+		const double *ux = U.GetColumnPtr(i);
+		long j;
+		for ( j=numEndEffectors; j>0; j-- ) {
+			double tmp;
+			alpha += (*ux)*(*(dTx++));
+			tmp = Square( *(ux++) );
+			alpha += (*ux)*(*(dTx++));
+			tmp += Square(*(ux++));
+			alpha += (*ux)*(*(dTx++));
+			tmp += Square(*(ux++));
+			N += sqrt(tmp);
+		}
+
+		// M is the quasi-1-norm of the response to angles changing according to the i-th column of V
+		//		Then is multiplied by the wiInv value.
+		double M = 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++);
+			}
+			M += fabs((*(vx++)))*accum;
+		}
+		M *= fabs(wiInv);
+	
+		double gamma = MaxAngleSDLS;
+		if ( N<M ) {
+			gamma *= N/M;				// Scale back maximum permissable joint angle
+		}
+
+		// Calculate the dTheta from pure pseudoinverse considerations
+		double scale = alpha*wiInv;			// This times i-th column of V is the psuedoinverse response
+		dPreTheta.LoadScaled( V.GetColumnPtr(i), scale );
+		// Now rescale the dTheta values.
+		double max = dPreTheta.MaxAbs();
+		double rescale = (gamma)/(gamma+max);
+		dTheta.AddScaled(dPreTheta,rescale);
+		/*if ( gamma<max) {
+			dTheta.AddScaled( dPreTheta, gamma/max );
+		}
+		else {
+			dTheta += dPreTheta;
+		}*/
+	}
+
+	// Scale back to not exceed maximum angle changes
+	double maxChange = dTheta.MaxAbs();
+	if ( maxChange>MaxAngleSDLS ) {
+		dTheta *= MaxAngleSDLS/(MaxAngleSDLS+maxChange);
+		//dTheta *= MaxAngleSDLS/maxChange;
+	}
+}
+
+void Jacobian::CalcdTClampedFromdS() 
+{
+	long len = dS.GetLength();
+	long j = 0;
+	for ( long i=0; i<len; i+=3, j++ ) {
+		double normSq = Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]);
+		if ( normSq>Square(dSclamp[j]) ) {
+			double factor = dSclamp[j]/sqrt(normSq);
+			dT1[i] = dS[i]*factor;
+			dT1[i+1] = dS[i+1]*factor;
+			dT1[i+2] = dS[i+2]*factor;
+		}
+		else {
+			dT1[i] = dS[i];
+			dT1[i+1] = dS[i+1];
+			dT1[i+2] = dS[i+2];
+		}
+	}
+}
+
+double Jacobian::UpdateErrorArray()
+{
+	double totalError = 0.0;
+
+	// Traverse tree to find all end effectors
+	VectorR3 temp;
+	Node* n = tree->GetRoot();
+	while ( n ) {	
+		if ( n->IsEffector() ) {
+			int i = n->GetEffectorNum();
+			const VectorR3& targetPos = target1[i];
+			temp = targetPos;
+			temp -= n->GetS();
+			double err = temp.Norm();
+			errorArray[i] = err;
+			totalError += err;
+		}
+		n = tree->GetSuccessor( n );
+	}
+	return totalError;
+}
+
+void Jacobian::UpdatedSClampValue()
+{
+	// Traverse tree to find all end effectors
+	VectorR3 temp;
+	Node* n = tree->GetRoot();
+	while ( n ) {	
+		if ( n->IsEffector() ) {
+			int i = n->GetEffectorNum();
+			const VectorR3& targetPos = target1[i];
+
+			// Compute the delta S value (differences from end effectors to target positions.
+			// While we are at it, also update the clamping values in dSclamp;
+			temp = targetPos;
+			temp -= n->GetS();
+			double normSi = sqrt(Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]));
+			double changedDist = temp.Norm()-normSi;
+			if ( changedDist>0.0 ) {
+				dSclamp[i] = BaseMaxTargetDist + changedDist;
+			}
+			else {
+				dSclamp[i] = BaseMaxTargetDist;
+			}
+		}
+		n = tree->GetSuccessor( n );
+	}
+}
+
+void Jacobian::DrawEigenVectors() const
+{
+	int i, j;
+	VectorR3 tail;
+	VectorR3 head;
+	Node *node;
+
+	for (i=0; i<w.GetLength(); i++) {
+		if ( NearZero( w[i], 1.0e-10 ) ) {
+			continue;
+		}
+		for (j=0; j<nEffector; j++) {
+			node = tree->GetEffector(j);
+			tail = node->GetS();
+			U.GetTriple( j, i, &head );
+			head += tail;
+			glDisable(GL_LIGHTING);
+			glColor3f(1.0f, 0.2f, 0.0f);
+			glLineWidth(2.0);
+			glBegin(GL_LINES);
+			glVertex3f(tail.x, tail.y, tail.z);
+			glVertex3f(head.x, head.y, tail.z);
+			glEnd();
+			Arrow(tail, head);
+			glLineWidth(1.0);
+			glEnable(GL_LIGHTING);
+		}
+	}
+}
+
+void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 )
+{
+	const VectorRn& e1 = j1.errorArray;
+	const VectorRn& e2 = j2.errorArray;
+	double ret1 = 0.0;
+	double ret2 = 0.0;
+	int len = e1.GetLength();
+	for ( long i=0; i<len; i++ ) {
+		double v1 = e1[i];
+		double v2 = e2[i];
+		if ( v1<v2 ) {
+			ret1 += v1/v2;
+			ret2 += 1.0;
+		}
+		else if ( v1 != 0.0 ) {
+			ret1 += 1.0;
+			ret2 += v2/v1;
+		}
+		else {
+			ret1 += 0.0;
+			ret2 += 0.0;
+		}
+	}
+	*weightedDist1 = ret1;
+	*weightedDist2 = ret2;
+}
+
+void Jacobian::CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies )
+{
+	const VectorRn& e1 = j1.errorArray;
+	const VectorRn& e2 = j2.errorArray;
+	int b1=0, b2=0, tie=0;
+	int len = e1.GetLength();
+	for ( long i=0; i<len; i++ ) {
+		double v1 = e1[i];
+		double v2 = e2[i];
+		if ( v1<v2 ) {
+			b1++;
+		}
+		else if ( v2<v1 ) {
+			b2++;
+		}
+		else {
+			tie++;
+		}
+	}
+	*numBetter1 = b1;
+	*numBetter2 = b2;
+	*numTies = tie;
+}
+
+/* THIS VERSION IS NOT AS GOOD.  DO NOT USE!
+void Jacobian::CalcDeltaThetasSDLSrev2() 
+{	
+	const MatrixRmn& J = ActiveJacobian();
+
+	// Compute Singular Value Decomposition 
+
+	J.ComputeSVD( U, w, V );
+	
+	// Next line for debugging only
+    assert(J.DebugCheckSVD(U, w , V));
+
+	// Calculate response vector dTheta that is the SDLS solution.
+	//	Delta target values are the dS values
+	int nRows = J.GetNumRows();
+	int numEndEffectors = tree->GetNumEffector();		// Equals the number of rows of J divided by three
+	int nCols = J.GetNumColumns();
+	dTheta.SetZero();
+
+	// Calculate the norms of the 3-vectors in the Jacobian
+	long i;
+	const double *jx = J.GetPtr();
+	double *jnx = Jnorms.GetPtr();
+	for ( i=nCols*numEndEffectors; i>0; i-- ) {
+		double accumSq = Square(*(jx++));
+		accumSq += Square(*(jx++));
+		accumSq += Square(*(jx++));
+		*(jnx++) = sqrt(accumSq);
+	}
+
+	// Loop over each singular vector
+	for ( i=0; i<nRows; i++ ) {
+
+		double wiInv = w[i];
+		if ( NearZero(wiInv,1.0e-10) ) {
+			continue;
+		}
+
+		double N = 0.0;						// N is the quasi-1-norm of the i-th column of U
+		double alpha = 0.0;					// alpha is the dot product of dS and the i-th column of U
+
+		const double *dSx = dS.GetPtr();
+		const double *ux = U.GetColumnPtr(i);
+		long j;
+		for ( j=numEndEffectors; j>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 ( N<P ) {
+			lambda -= N/P;				// Scale back maximum permissable joint angle
+		}
+		lambda *= lambda;
+		lambda *= DampingLambdaSDLS;
+
+		// Calculate the dTheta from pure pseudoinverse considerations
+		double scale = alpha*wiInv/(Square(wiInv)+Square(lambda));			// This times i-th column of V is the SDLS response
+		MatrixRmn::AddArrayScale(nCols, V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, scale );
+	}
+
+	// Scale back to not exceed maximum angle changes
+	double maxChange = dTheta.MaxAbs();
+	if ( maxChange>MaxAngleSDLS ) {
+		dTheta *= MaxAngleSDLS/maxChange;
+	}
+} */
+
+
+
+

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -0,0 +1,132 @@
+
+/*
+*
+* 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://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 "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 DrawEigenVectors() const;
+
+	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

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 <assert.h>
+
+// ******************************************************
+// * 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 << ">");
+}
+
+

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 <math.h>
+#include <assert.h>
+#include <iostream>
+#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

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);
+}
+

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 <assert.h>
+
+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<rotmat.Column1().Norm()
+			 && rotmat.Column1().Norm()<1.0001 && 0.9999<rotmat.Column1().Norm()
+			 && (rotmat.Column1()^rotmat.Column2()) < 0.001
+			 && (rotmat.Column1()^rotmat.Column2()) > -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 << ">");
+}
+
+

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 <math.h>
+
+//
+// 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<class T> inline T Min ( T x, T y ) 
+{
+	return (x<y ? x : y);
+}
+
+template<class T> inline T Max ( T x, T y ) 
+{
+	return (y<x ? x : y);
+}
+
+template<class T> inline T ClampRange ( T x, T min, T max) 
+{
+	if ( x<min ) {
+		return min;
+	}
+	if ( x>max ) {
+		return max;
+	}
+	return x;
+}
+
+template<class T> inline bool ClampRange ( T *x, T min, T max) 
+{
+	if ( (*x)<min ) {
+		(*x) = min;
+		return false;
+	}
+	else if ( (*x)>max ) {
+		(*x) = max;
+		return false;
+	}
+	else {
+		return true;
+	}
+}
+
+template<class T> inline bool ClampMin ( T *x, T min) 
+{
+	if ( (*x)<min ) {
+		(*x) = min;
+		return false;
+	}
+	return true;
+}
+
+
+template<class T> inline bool ClampMax ( T *x, T max) 
+{
+	if ( (*x)>max ) {
+		(*x) = max;
+		return false;
+	}
+	return true;
+}
+
+template<class T> inline T& UpdateMin ( const T& x, T& y ) 
+{
+	if ( x<y ) {
+		y = x;
+	}
+	return y;
+}
+
+template<class T> inline T& UpdateMax ( const T& x, T& y ) 
+{
+	if ( x>y ) {
+		y = x;
+	}
+	return y;
+}
+
+
+template<class T> inline bool SameSignNonzero( T x, T y )
+{
+	if ( x<0 ) {
+		return (y<0);
+	}
+	else if ( 0<x ) {
+		return (0<y);
+	}
+	else {
+		return false;
+	}
+}
+
+inline double Mag ( double x ) {
+	return fabs(x);
+}
+
+inline double Dist ( double x, double y ) {
+	return fabs(x-y);
+}
+
+template <class T>
+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<x && x<1.0e-15 );
+	}
+}
+
+inline bool GreaterOrEqualFuzzy ( double x, double y ) 
+{
+	return LessOrEqualFuzzy( y, x );
+}
+
+inline bool UpdateMaxAbs( double *maxabs, double updateval )
+{
+	if ( updateval > *maxabs ) {
+		*maxabs = updateval;
+		return true;
+	}
+	else if ( -updateval > *maxabs ) {
+		*maxabs = -updateval;
+		return true;
+	}
+	else {
+		return false;
+	}
+}
+
+// **********************************************************
+// Combinations and averages.								*
+// **********************************************************
+
+template <class T>
+void averageOf ( const T& a, const T &b, T&c ) {
+	c = a;
+	c += b;
+	c *= 0.5;
+}
+
+template <class T>
+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 <class T>
+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<class T> inline T Square ( T x ) 
+{
+	return (x*x);
+}
+
+// Cube(x) returns x*x*x,  of course!
+
+template<class T> 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<class T> inline int Sign( T x) 
+{
+	if ( x<0 ) {
+		return -1;
+	}
+	else if ( x==0 ) {
+		return 0;
+	}
+	else {
+		return 1;
+	}
+}
+
+
+#endif		// #ifndef MATH_MISC_H

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<NumRows && startCol>=0 && startCol<NumCols );
+	assert( startRow+(length-1)*deltaRow>=0 && startRow+(length-1)*deltaRow<NumRows );
+ 	assert( startCol+(length-1)*deltaCol>=0 && startCol+(length-1)*deltaCol<NumCols );
+	double *to = x + startRow + NumRows*startCol;
+	double *from = d.x;
+	long stride = deltaRow + NumRows*deltaCol;
+	for ( ; length>0; 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 && idx<NumCols );
+	double *colA = x + idx*NumRows;
+	double *colB = colA + NumRows;
+	for ( long i = NumRows; i>0; 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 && idx1<NumCols && 0<=idx2 && idx2<NumCols );
+	double *colA = x + idx1*NumRows;
+	double *colB = x + idx2*NumRows;
+	for ( long i = NumRows; i>0; 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<lastBidiagIdx-1) ? Square(superDiag[lastBidiagIdx-2]): 0.0;
+		double BSq = Square(w[lastBidiagIdx-1]);
+		A += BSq;										// The "A" entry of M^T * M
+		double C = Square(superDiag[lastBidiagIdx-1]);	
+		BSq *= C;										// The squared "B" entry
+		C += Square(w[lastBidiagIdx]);					// The "C" entry
+		double lambda;									// lambda will hold the estimated eigenvalue
+		lambda = sqrt( Square((A-C)*0.5) + BSq );		// Use the lambda value that is closest to C.
+		if ( A > 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<lastBidiagIdx-1; i++ ) {
+			// Push non-zero from M[i+1,i] to M[i,i+2] 
+			CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
+			ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr+1, sdPtr+1 );
+			U.PostApplyGivens( c, -s, i );
+			// Push non-zero from M[i,i+2] to M[1+2,i+1]
+			CalcGivensValues( *sdPtr, extraOffDiag, &c, &s );
+			ApplyGivensCBTD( c, s, sdPtr, wPtr+1, &extraOffDiag, extraOffDiag, sdPtr+1, wPtr+2 );
+			V.PostApplyGivens( c, -s, i+1 );
+			wPtr++;
+			sdPtr++;
+		}
+		// Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
+		CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
+		ApplyGivensCBTD( c, s, wPtr, &extraOffDiag, sdPtr, wPtr+1 );
+		U.PostApplyGivens( c, -s, i );
+
+		// DEBUG
+		// DebugCalcBidiagCheck( V, w, superDiag, U );
+	}
+}
+
+// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry on the same row.
+// We use Givens rotations to "chase" the non-zero entry across the row; when it reaches the last
+//	column, it is finally zeroed away.
+// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero superdiagonal entry on the same row.
+void MatrixRmn::ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx, MatrixRmn& U, double *wPtr, double *sdPtr, double eps )
+{
+	double curSd = *sdPtr;		// Value being chased across the row
+	*sdPtr = 0.0;
+	long i=firstBidiagIdx+1;
+	while (true) { 
+		// Rotate row i and row firstBidiagIdx (Givens rotation)
+		double c, s;
+		CalcGivensValues( *(++wPtr), curSd, &c, &s );
+		U.PostApplyGivens( c, -s, i, firstBidiagIdx );
+		*wPtr = c*(*wPtr) - s*curSd;
+		if ( i==lastBidiagIdx ) {
+			break;
+		}
+		curSd = s*(*(++sdPtr));		// New value pops up one column over to the right
+		*sdPtr = c*(*sdPtr);
+		i++;
+	}
+}
+
+// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry in the same column.
+// We use Givens rotations to "chase" the non-zero entry up the column; when it reaches the last
+//	column, it is finally zeroed away.
+// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero superdiagonal entry in the same column.
+void MatrixRmn::ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps )
+{
+	double curSd = *sdPtr;		// Value being chased up the column
+	*sdPtr = 0.0;
+	long i = endIdx-1;
+	while ( true ) {
+		double c, s;
+		CalcGivensValues( *(--wPtr), curSd, &c, &s );
+		V.PostApplyGivens( c, -s, i, endIdx );
+		*wPtr = c*(*wPtr) - s*curSd;
+		if ( i==0 ) {
+			break;
+		}
+		curSd = s*(*(--sdPtr));		// New value pops up one row above
+		if ( NearZero( curSd, eps ) ) {
+			break;
+		}
+		*sdPtr = c*(*sdPtr);
+		i--;
+	}
+}
+
+
+
+// Matrix A is  ( ( a c ) ( b d ) ), i.e., given in column order.
+// Mult's G[c,s]  times  A, replaces A.
+void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d )
+{
+	double temp = *a;
+	*a = cosine*(*a) - sine*(*b);
+	*b = sine*temp + cosine*(*b);
+	temp = *c;
+	*c = cosine*(*c) - sine*(*d);
+	*d = sine*temp + cosine*(*d);
+}
+
+// Now matrix A given in row order, A = ( ( a b c ) ( d e f ) ).
+// Return G[c,s] * A, replace A.  d becomes zero, no need to return.
+//  Also, it is certain the old *c value is taken to be zero!
+void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, 
+															 double  d, double *e, double *f )
+{
+	*a = cosine*(*a) - sine*d;
+	double temp = *b;
+	*b = cosine*(*b) - sine*(*e);
+	*e = sine*temp + cosine*(*e);
+	*c =  -sine*(*f);
+	*f =  cosine*(*f);
+}
+
+// Helper routine for SVD conversion from bidiagonal to diagonal
+bool MatrixRmn::UpdateBidiagIndices( long *firstBidiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps )
+{
+	long lastIdx = *lastBidiagIdx;
+	double* sdPtr = superDiag.GetPtr( lastIdx-1 );		// Entry above the last diagonal entry
+	while ( NearZero(*sdPtr, eps) ) {				
+		*(sdPtr--) = 0.0;
+		lastIdx--;
+		if ( lastIdx == 0 ) {
+			return false;
+		}
+	}
+	*lastBidiagIdx = lastIdx;
+	long firstIdx = lastIdx-1;
+	double* wPtr = w.GetPtr( firstIdx );
+	while ( firstIdx > 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;
+}
+

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 <math.h>
+#include <assert.h>
+#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 ( i<NumRows && j<NumCols );	
+	return *(x+j*NumRows+i); 
+}
+
+// Return a VectorR3 out of a column.  Starts at row 3*i, in column j.
+inline void MatrixRmn::GetTriple( long i, long j, VectorR3 *retValue ) const
+{
+	long ii = 3*i;
+	assert ( 0<=i && ii+2<NumRows && 0<=j && j<NumCols );
+	retValue->Load( 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 && i<NumRows && 0<=j &&j<NumCols );	
+	return (x+j*NumRows+i); 
+}
+
+// Get a pointer to the (i,j) entry.
+// The entries are in column order.  
+// This version gives pointer to writable data
+inline double* MatrixRmn::GetPtr( long i, long j )
+{ 
+	assert ( i<NumRows && j<NumCols );	
+	return (x+j*NumRows+i); 
+}
+
+// Get a pointer to the j-th column.
+// The entries are in column order.  
+// This version gives read-only pointer
+inline const double* MatrixRmn::GetColumnPtr( long j ) const
+{ 
+	assert ( 0<=j && j<NumCols );	
+	return (x+j*NumRows); 
+}
+
+// Get a pointer to the j-th column.
+// This version gives pointer to writable data
+inline double* MatrixRmn::GetColumnPtr( long j )
+{ 
+	assert ( 0<=j && j<NumCols );	
+	return (x+j*NumRows); 
+}
+
+/// Get a pointer to the i-th row
+// The entries are in column order.  
+// This version gives read-only pointer
+inline const double* MatrixRmn::GetRowPtr( long i ) const
+{ 
+	assert ( 0<=i && i<NumRows );	
+	return (x+i); 
+}
+
+// Get a pointer to the i-th row
+// This version gives pointer to writable data
+inline double* MatrixRmn::GetRowPtr( long i )
+{ 
+	assert ( 0<=i && i<NumRows );	
+	return (x+i); 
+}
+
+// Set the (i,j) entry of the matrix
+inline void MatrixRmn::Set( long i, long j, double val ) 
+{ 
+	assert( i<NumRows && j<NumCols );
+	*(x+j*NumRows+i) = val;
+}
+
+// Set the i-th triple in the j-th column to u's three values
+inline void MatrixRmn::SetTriple( long i, long j, const VectorR3& u ) 
+{
+	long ii = 3*i;
+	assert ( 0<=i && ii+2<NumRows && 0<=j && j<NumCols );
+	u.Dump( x+j*NumRows + ii );
+}
+
+// Set to be equal to the identity matrix
+inline void MatrixRmn::SetIdentity()
+{
+	assert ( NumRows==NumCols );
+	SetZero();
+	SetDiagonalEntries(1.0);
+}
+
+inline MatrixRmn& MatrixRmn::operator*=( double mult )
+{
+	double* aPtr = x;
+	for ( long i=NumRows*NumCols; i>0; 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

examples/ThirdPartyLibs/BussIK/Misc.cpp

@@ -0,0 +1,346 @@
+/*
+*
+* 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 <math.h>
+#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
+
+void Axes( float length )
+{
+	int i, j;			/* counters			*/
+	float fact;			/* character scale factor	*/
+	float base;			/* character start location	*/
+
+	glBegin( GL_LINE_STRIP );
+		glVertex3f( length, 0., 0. );
+		glVertex3f( 0., 0., 0. );
+		glVertex3f( 0., length, 0. );
+	glEnd();
+	glBegin( GL_LINE_STRIP );
+		glVertex3f( 0., 0., 0. );
+		glVertex3f( 0., 0., length );
+	glEnd();
+
+	fact = LENFRAC * length;
+	base = BASEFRAC * length;
+
+	glBegin( GL_LINE_STRIP );
+		for( i = 0; i < 4; i++ )
+		{
+			j = xorder[i];
+			if( j < 0 )
+			{
+				
+				glEnd();
+				glBegin( GL_LINE_STRIP );
+				j = -j;
+			}
+			j--;
+			glVertex3f( base + fact*xx[j], fact*xy[j], 0.0 );
+		}
+	glEnd();
+
+	glBegin( GL_LINE_STRIP );
+		for( i = 0; i < 5; i++ )
+		{
+			j = yorder[i];
+			if( j < 0 )
+			{
+				
+				glEnd();
+				glBegin( GL_LINE_STRIP );
+				j = -j;
+			}
+			j--;
+			glVertex3f( fact*yx[j], base + fact*yy[j], 0.0 );
+		}
+	glEnd();
+
+	glBegin( GL_LINE_STRIP );
+		for( i = 0; i < 6; i++ )
+		{
+			j = zorder[i];
+			if( j < 0 )
+			{
+				
+				glEnd();
+				glBegin( GL_LINE_STRIP );
+				j = -j;
+			}
+			j--;
+			glVertex3f( 0.0, fact*zy[j], base + fact*zx[j] );
+		}
+	glEnd();
+
+}
+
+
+/****************************************************************
+							 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	Arrow( float tail[3], float head[3] );
+void	Arrow( const VectorR3& tail, const VectorR3& head );
+void	cross( float [3], float [3], float [3] );
+float	dot( float [3], float [3] );
+float	unit( float [3], float [3] );
+
+
+void	Arrow( const VectorR3& tail, const VectorR3& head )
+{
+	float t[3];
+	float h[3];
+	tail.Dump( t );
+	head.Dump( h );
+	Arrow( t, h );
+} 
+
+
+void Arrow( float tail[3], float head[3] )
+{
+	float u[3], v[3], w[3];		/* arrow coordinate system	*/
+	float d;			/* wing distance		*/
+	float x, y, z;			/* point to plot		*/
+	float mag;			/* magnitude of major direction	*/
+	float f;			/* fabs of magnitude		*/
+	int axis;			/* which axis is the major	*/
+
+
+	/* set w direction in u-v-w coordinate system:			*/
+
+	w[0] = head[0] - tail[0];
+	w[1] = head[1] - tail[1];
+	w[2] = head[2] - tail[2];
+
+
+	/* determine major direction:					*/
+
+	axis = X;
+	mag = fabs( w[0] );
+	if( (f=fabs(w[1]))  > mag )
+	{
+		axis = Y;
+		mag = f;
+	}
+	if( (f=fabs(w[2]))  > mag )
+	{
+		axis = Z;
+		mag = f;
+	}
+
+
+	/* set size of wings and turn w into a unit vector:		*/
+
+	d = WINGS * unit( w, w );
+
+
+	/* draw the shaft of the arrow:					*/
+
+	glBegin( GL_LINE_STRIP );
+		glVertex3fv( tail );
+		glVertex3fv( head );
+	glEnd();
+
+	/* draw two sets of wings in the non-major directions:		*/
+
+	if( axis != X )
+	{
+		cross( w, axx, v );
+		(void) unit( v, v );
+		cross( v, w, u  );
+		x = head[0] + d * ( u[0] - w[0] );
+		y = head[1] + d * ( u[1] - w[1] );
+		z = head[2] + d * ( u[2] - w[2] );
+		glBegin( GL_LINE_STRIP );
+			glVertex3fv( head );
+			glVertex3f( x, y, z );
+		glEnd();
+		x = head[0] + d * ( -u[0] - w[0] );
+		y = head[1] + d * ( -u[1] - w[1] );
+		z = head[2] + d * ( -u[2] - w[2] );
+		glBegin( GL_LINE_STRIP );
+			glVertex3fv( head );
+			glVertex3f( x, y, z );
+		glEnd();
+	}
+
+
+	if( axis != Y )
+	{
+		cross( w, ayy, v );
+		(void) unit( v, v );
+		cross( v, w, u  );
+		x = head[0] + d * ( u[0] - w[0] );
+		y = head[1] + d * ( u[1] - w[1] );
+		z = head[2] + d * ( u[2] - w[2] );
+		glBegin( GL_LINE_STRIP );
+			glVertex3fv( head );
+			glVertex3f( x, y, z );
+		glEnd();
+		x = head[0] + d * ( -u[0] - w[0] );
+		y = head[1] + d * ( -u[1] - w[1] );
+		z = head[2] + d * ( -u[2] - w[2] );
+		glBegin( GL_LINE_STRIP );
+			glVertex3fv( head );
+			glVertex3f( x, y, z );
+		glEnd();
+	}
+
+	if( axis != Z )
+	{
+		cross( w, azz, v );
+		(void) unit( v, v );
+		cross( v, w, u  );
+		x = head[0] + d * ( u[0] - w[0] );
+		y = head[1] + d * ( u[1] - w[1] );
+		z = head[2] + d * ( u[2] - w[2] );
+		glBegin( GL_LINE_STRIP );
+			glVertex3fv( head );
+			glVertex3f( x, y, z );
+		glEnd();
+		x = head[0] + d * ( -u[0] - w[0] );
+		y = head[1] + d * ( -u[1] - w[1] );
+		z = head[2] + d * ( -u[2] - w[2] );
+		glBegin( GL_LINE_STRIP );
+			glVertex3fv( head );
+			glVertex3f( x, y, z );
+		glEnd();
+	}
+
+
+	/* done:							*/
+
+}
+
+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 );
+}

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 <math.h>
+
+
+#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;
+}

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

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

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://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 <iostream>
+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);
+}

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://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 "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

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;
+}

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 <math.h>
+#include <assert.h>
+#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 && i<length );	return *(x+i); }
+	double& operator[]( long i ) { assert ( 0<=i && i<length );	return *(x+i); }
+	double Get( long i ) const { assert ( 0<=i && i<length );	return *(x+i); }
+
+	// Use GetPtr to get pointer into the array (efficient)
+	// Is friendly in that anyone can change the array contents (be careful!)
+	const double* GetPtr( long i ) const { assert(0<=i && i<length);  return (x+i); }
+	double* GetPtr( long i ) { assert(0<=i && i<length);  return (x+i); }
+	const double* GetPtr() const { return x; }
+	double* GetPtr() { return x; }
+
+	void Set( long i, double val ) { assert(0<=i && i<length), *(x+i) = val; }
+	void SetTriple( long i, const VectorR3& u );
+
+	VectorRn& operator+=( const VectorRn& src );
+	VectorRn& operator-=( const VectorRn& src );
+	void AddScaled (const VectorRn& src, double scaleFactor );
+
+	VectorRn& operator*=( double f );
+	double NormSq() const;
+	double Norm() const { return sqrt(NormSq()); }
+
+	double MaxAbs() const;
+    
+private:
+	long length;				// Logical or actual length 
+	long AllocLength;			// Allocated length
+	double *x;					// Array of vector entries
+
+	static VectorRn WorkVector;								// Serves as a temporary vector
+	static VectorRn& GetWorkVector() { return WorkVector; }
+	static VectorRn& GetWorkVector( long len ) { WorkVector.SetLength(len); return WorkVector; }
+};
+
+inline VectorRn::VectorRn() 
+{
+	length = 0;
+	AllocLength = 0;
+	x = 0;
+}
+
+inline VectorRn::VectorRn( long initLength ) 
+{
+	length = 0;
+	AllocLength = 0;
+	x = 0;
+	SetLength( initLength );
+}
+
+inline VectorRn::~VectorRn() 
+{
+	delete x;
+}
+
+// Resize.  
+// If the array is shortened, the information about the allocated length is lost.
+inline void VectorRn::SetLength( long newLength )
+{
+	assert ( newLength>0 );
+	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

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) {

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -23,9 +23,9 @@ subject to the following restrictions:
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity),
-	m_desiredPosition(0),
-	m_kd(1.),
+	m_desiredVelocity(desiredVelocity),
+	m_desiredPosition(0),
+	m_kd(1.),
 	m_kp(0)
 {
 
@@ -54,10 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
 	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity),
-	m_desiredPosition(0),
-	m_kd(1.),
-	m_kp(0)
+	m_desiredVelocity(desiredVelocity),
+	m_desiredPosition(0),
+	m_kd(1.),
+	m_kp(0)
 {
 	btAssert(linkDoF < body->getLink(link).m_dofCount);
 
@@ -119,14 +119,14 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 	for (int row=0;row<getNumRows();row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-
+
         int dof = 0;
-        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;
-        
+        btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+        btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+        btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
+        btScalar velocityError = (m_desiredVelocity - currentVelocity);
+        btScalar rhs =   m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
+        
         
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);
 		constraintRow.m_orgConstraint = this;