Add orientation constraint to IK.

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -55,7 +55,7 @@ Jacobian::Jacobian(Tree* tree)
 	Jacobian::tree = tree;
 	nEffector = tree->GetNumEffector();
 	nJoint = tree->GetNumJoint();
-	nRow = 3 * nEffector;
+	nRow = 2 * 3 * nEffector; // Include both linear and angular part
 	nCol = nJoint;
 
 	Jend.SetSize(nRow, nCol);				// The Jocobian matrix
@@ -276,6 +276,31 @@ void Jacobian::CalcDeltaThetasDLS()
 	}
 }
 
+void Jacobian::CalcThetasDotDLS(float dt)
+{
+    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 MaxVelDLS = MaxAngleDLS/dt;
+    double maxChange = dTheta.MaxAbs();
+    if ( maxChange>MaxVelDLS ) {
+        dTheta *= MaxVelDLS/maxChange;
+    }
+}
+
 void Jacobian::CalcDeltaThetasDLSwithSVD() 
 {	
 	const MatrixRmn& J = ActiveJacobian();

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -69,6 +69,7 @@ public:
 	void CalcDeltaThetasDLS();
 	void CalcDeltaThetasDLSwithSVD();
 	void CalcDeltaThetasSDLS();
+    void CalcThetasDotDLS(float dt);
 
 	void UpdateThetas();
 	double UpdateErrorArray(VectorR3* targets);		// Returns sum of errors