Update theta dot for velocity IK.

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -194,14 +194,22 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
             break;
     }
     
-    m_data->m_ikJacobian->UpdateThetas();
+    // Use for velocity IK, update theta dot
+    m_data->m_ikJacobian->UpdateThetaDot();
+    
+    // Use for position IK, incrementally update theta
+    //m_data->m_ikJacobian->UpdateThetas();
     
     // Apply the change in the theta values
     //m_data->m_ikJacobian->UpdatedSClampValue(&targets);
     
     for (int i=0;i<numQ;i++)
     {
+        // Use for velocity IK
         q_new[i] = m_data->m_ikNodes[i]->GetTheta()*dt + q_current[i];
+        
+        // Use for position IK
+        //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
     }
     return true;
 }

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -170,6 +170,24 @@ void Jacobian::UpdateThetas()
 	tree->Compute();
 }
 
+void Jacobian::UpdateThetaDot()
+{
+    // 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->UpdateTheta( dTheta[i] );
+            
+        }
+        n = tree->GetSuccessor( n );
+    }
+    
+    // Update the positions and rotation axes of all joints/effectors
+    tree->Compute();
+}
+
 void Jacobian::CalcDeltaThetas() 
 {
 	switch (CurrentUpdateMode) {

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -72,6 +72,7 @@ public:
     void CalcThetasDotDLS(float dt);
 
 	void UpdateThetas();
+    void UpdateThetaDot();
 	double UpdateErrorArray(VectorR3* targets);		// Returns sum of errors
 	const VectorRn& GetErrorArray() const { return errorArray; }
 	void UpdatedSClampValue(VectorR3* targets);

examples/ThirdPartyLibs/BussIK/Node.h

@@ -56,6 +56,10 @@ public:
 		delta = actualDelta;
 #endif
 		return theta; }
+    
+    double UpdateTheta( double& delta ) {
+        theta = delta;
+        return theta; }
 
 	const VectorR3& GetS() const { return s; }
 	const VectorR3& GetW() const { return w; }