Update theta dot for velocity IK.

2016-09-20 10:24:48 -07:00
parent bf16c87987
commit cbda64c5e7
4 changed files with 32 additions and 1 deletions
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+++ b/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) {