Bugfix: improvements to Inverse Kinematics constraint.

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/SimulationNode.java

@@ -295,7 +295,7 @@ public class SimulationNode {
                             
                             // track contains differences between the frame position and bind positions of bones/spatials
                             Vector3f bonePositionDifference = bone.getLocalPosition().subtract(startTransform.getTranslation());
-                            Quaternion boneRotationDifference = bone.getLocalRotation().mult(startTransform.getRotation().inverse()).normalizeLocal();
+                            Quaternion boneRotationDifference = startTransform.getRotation().inverse().mult(bone.getLocalRotation()).normalizeLocal();
                             Vector3f boneScaleDifference = bone.getLocalScale().divide(startTransform.getScale());
                             
                             trackEntry.getValue().setTransform(frame, new Transform(bonePositionDifference, boneRotationDifference, boneScaleDifference));

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java

@@ -1,33 +1,42 @@
 package com.jme3.scene.plugins.blender.constraints.definitions;
 
 import java.util.ArrayList;
+import java.util.Collections;
 import java.util.HashSet;
 import java.util.List;
 
 import com.jme3.animation.Bone;
-import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Transform;
 import com.jme3.math.Vector3f;
 import com.jme3.scene.plugins.blender.BlenderContext;
 import com.jme3.scene.plugins.blender.animations.BoneContext;
-import com.jme3.scene.plugins.blender.constraints.ConstraintHelper;
 import com.jme3.scene.plugins.blender.constraints.ConstraintHelper.Space;
 import com.jme3.scene.plugins.blender.file.Structure;
 
+/**
+ * The Inverse Kinematics constraint.
+ * 
+ * @author Wesley Shillingford (wezrule)
+ * @author Marcin Roguski (Kaelthas)
+ */
 public class ConstraintDefinitionIK extends ConstraintDefinition {
-
-    private static final int FLAG_POSITION = 0x20;
+    private static final float MIN_DISTANCE  = 0.0001f;
+    private static final int   FLAG_POSITION = 0x20;
 
     /** The number of affected bones. Zero means that all parent bones of the current bone should take part in baking. */
-    private int              bonesAffected;
-    private float            chainLength;
-    private BoneContext[]    bones;
-    private boolean          needToCompute = true;
+    private int                bonesAffected;
+    /** The total length of the bone chain. Useful for optimisation of computations speed in some cases. */
+    private float              chainLength;
+    /** Tells if there is anything to compute at all. */
+    private boolean            needToCompute = true;
+    /** The amount of iterations of the algorithm. */
+    private int                iterations;
 
     public ConstraintDefinitionIK(Structure constraintData, Long ownerOMA, BlenderContext blenderContext) {
         super(constraintData, ownerOMA, blenderContext);
         bonesAffected = ((Number) constraintData.getFieldValue("rootbone")).intValue();
+        iterations = ((Number) constraintData.getFieldValue("iterations")).intValue();
 
         if ((flag & FLAG_POSITION) == 0) {
             needToCompute = false;
@@ -37,55 +46,76 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
             alteredOmas = new HashSet<Long>();
         }
     }
-    
+
     @Override
     public void bake(Space ownerSpace, Space targetSpace, Transform targetTransform, float influence) {
-        if (needToCompute && influence != 0) {
-            ConstraintHelper constraintHelper = blenderContext.getHelper(ConstraintHelper.class);
-            BoneContext[] boneContexts = this.getBones();
-            float b = chainLength;
-            Quaternion boneWorldRotation = new Quaternion();
-
-            for (int i = 0; i < boneContexts.length; ++i) {
-                Bone bone = boneContexts[i].getBone();
-
-                bone.updateModelTransforms();
-                Transform boneWorldTransform = constraintHelper.getTransform(boneContexts[i].getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD);
-
-                Vector3f head = boneWorldTransform.getTranslation();
-                Vector3f tail = head.add(bone.getModelSpaceRotation().mult(Vector3f.UNIT_Y.mult(boneContexts[i].getLength())));
-
-                Vector3f vectorA = tail.subtract(head);
-                float a = vectorA.length();
-                vectorA.normalizeLocal();
-
-                Vector3f vectorC = targetTransform.getTranslation().subtract(head);
-                float c = vectorC.length();
-                vectorC.normalizeLocal();
-
-                b -= a;
-                float theta = 0;
-
-                if (c >= a + b) {
-                    theta = vectorA.angleBetween(vectorC);
-                } else if (c <= FastMath.abs(a - b) && i < boneContexts.length - 1) {
-                    theta = vectorA.angleBetween(vectorC) - FastMath.HALF_PI;
-                } else {
-                    theta = vectorA.angleBetween(vectorC) - FastMath.acos(-(b * b - a * a - c * c) / (2 * a * c));
-                }
-                
-                theta *= influence;
-
-                if (theta != 0) {
-                    Vector3f vectorR = vectorA.cross(vectorC);
-                    boneWorldRotation.fromAngleAxis(theta, vectorR);
-                    boneWorldTransform.getRotation().multLocal(boneWorldRotation);
-                    constraintHelper.applyTransform(boneContexts[i].getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneWorldTransform);
+        if(influence == 0 || !needToCompute) {
+            return ;//no need to do anything
+        }
+        Quaternion q = new Quaternion();
+        Vector3f t = targetTransform.getTranslation();
+        List<BoneContext> bones = this.loadBones();
+        float distanceFromTarget = Float.MAX_VALUE;
+
+        int iterations = this.iterations;
+        if (bones.size() == 1) {
+            iterations = 1;// if only one bone is in the chain then only one iteration that will properly rotate it will be needed
+        } else {
+            // if the target cannot be rached by the bones' chain then the chain will become straight and point towards the target
+            // in this case only one iteration will be needed, computed from the root to top bone
+            BoneContext rootBone = bones.get(bones.size() - 1);
+            Transform rootBoneTransform = constraintHelper.getTransform(rootBone.getArmatureObjectOMA(), rootBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
+            if (t.distance(rootBoneTransform.getTranslation()) >= chainLength) {
+                Collections.reverse(bones);
+
+                for (BoneContext boneContext : bones) {
+                    Bone bone = boneContext.getBone();
+                    Transform boneTransform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD);
+
+                    Vector3f e = boneTransform.getTranslation().add(boneTransform.getRotation().mult(Vector3f.UNIT_Y).multLocal(boneContext.getLength()));// effector
+                    Vector3f j = boneTransform.getTranslation(); // current join position
+
+                    Vector3f currentDir = e.subtractLocal(j).normalizeLocal();
+                    Vector3f target = t.subtract(j).normalizeLocal();
+                    float angle = currentDir.angleBetween(target);
+                    if (angle != 0) {
+                        Vector3f cross = currentDir.crossLocal(target).normalizeLocal();
+                        q.fromAngleAxis(angle, cross);
+
+                        boneTransform.getRotation().set(q.multLocal(boneTransform.getRotation()));
+                        constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneTransform);
+                    }
                 }
 
-                bone.updateModelTransforms();
-                alteredOmas.add(boneContexts[i].getBoneOma());
+                iterations = 0;
+            }
+        }
+
+        BoneContext topBone = bones.get(0);
+        for (int i = 0; i < iterations && distanceFromTarget > MIN_DISTANCE; ++i) {
+            for (BoneContext boneContext : bones) {
+                Bone bone = boneContext.getBone();
+                Transform topBoneTransform = constraintHelper.getTransform(topBone.getArmatureObjectOMA(), topBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
+                Transform boneWorldTransform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD);
+
+                Vector3f e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3f.UNIT_Y).multLocal(topBone.getLength()));// effector
+                Vector3f j = boneWorldTransform.getTranslation(); // current join position
+
+                Vector3f currentDir = e.subtractLocal(j).normalizeLocal();
+                Vector3f target = t.subtract(j).normalizeLocal();
+                float angle = currentDir.angleBetween(target);
+                if (angle != 0) {
+                    Vector3f cross = currentDir.crossLocal(target).normalizeLocal();
+                    q.fromAngleAxis(angle, cross);
+
+                    boneWorldTransform.getRotation().set(q.multLocal(boneWorldTransform.getRotation()));
+                    constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneWorldTransform);
+                }
             }
+
+            Transform topBoneTransform = constraintHelper.getTransform(topBone.getArmatureObjectOMA(), topBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
+            Vector3f e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3f.UNIT_Y).multLocal(topBone.getLength()));// effector
+            distanceFromTarget = e.distance(t);
         }
     }
 
@@ -97,20 +127,18 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
     /**
      * @return the bone contexts of all bones that will be used in this constraint computations
      */
-    private BoneContext[] getBones() {
-        if (bones == null) {
-            List<BoneContext> bones = new ArrayList<BoneContext>();
-            Bone bone = (Bone) this.getOwner();
-            while (bone != null) {
-                BoneContext boneContext = blenderContext.getBoneContext(bone);
-                bones.add(0, boneContext);
-                chainLength += boneContext.getLength();
-                if (bonesAffected != 0 && bones.size() >= bonesAffected) {
-                    break;
-                }
-                bone = bone.getParent();
+    private List<BoneContext> loadBones() {
+        List<BoneContext> bones = new ArrayList<BoneContext>();
+        Bone bone = (Bone) this.getOwner();
+        while (bone != null) {
+            BoneContext boneContext = blenderContext.getBoneContext(bone);
+            chainLength += boneContext.getLength();
+            bones.add(boneContext);
+            alteredOmas.add(boneContext.getBoneOma());
+            if (bonesAffected != 0 && bones.size() >= bonesAffected) {
+                break;
             }
-            this.bones = bones.toArray(new BoneContext[bones.size()]);
+            bone = bone.getParent();
         }
         return bones;
     }