Merge branch 'master' of https://github.com/jMonkeyEngine/jmonkeyengine.git

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

@@ -7,6 +7,7 @@ import java.util.List;
 import java.util.Map;
 import java.util.Map.Entry;
 import java.util.Set;
+import java.util.Stack;
 import java.util.logging.Logger;
 
 import com.jme3.animation.AnimChannel;
@@ -38,9 +39,9 @@ import com.jme3.util.TempVars;
  * @author Marcin Roguski (Kaelthas)
  */
 public class SimulationNode {
-    private static final Logger  LOGGER   = Logger.getLogger(SimulationNode.class.getName());
+    private static final Logger LOGGER = Logger.getLogger(SimulationNode.class.getName());
 
-    private Long featureOMA;
+    private Long                 featureOMA;
     /** The blender context. */
     private BlenderContext       blenderContext;
     /** The name of the node (for debugging purposes). */
@@ -51,11 +52,11 @@ public class SimulationNode {
     private List<Animation>      animations;
 
     /** The nodes spatial (if null then the boneContext should be set). */
-    private Spatial              spatial;
+    private Spatial     spatial;
     /** The skeleton of the bone (not null if the node simulated the bone). */
-    private Skeleton             skeleton;
+    private Skeleton    skeleton;
     /** Animation controller for the node's feature. */
-    private AnimControl          animControl;
+    private AnimControl animControl;
 
     /**
      * The star transform of a spatial. Needed to properly reset the spatial to
@@ -64,7 +65,7 @@ public class SimulationNode {
     private Transform            spatialStartTransform;
     /** Star transformations for bones. Needed to properly reset the bones. */
     private Map<Bone, Transform> boneStartTransforms;
-    
+
     /**
      * Builds the nodes tree for the given feature. The feature (bone or
      * spatial) is found by its OMA. The feature must be a root bone or a root
@@ -208,8 +209,7 @@ public class SimulationNode {
         if (animations != null) {
             TempVars vars = TempVars.get();
             AnimChannel animChannel = animControl.createChannel();
-            
-            // List<Bone> bonesWithConstraints = this.collectBonesWithConstraints(skeleton);
+
             for (Animation animation : animations) {
                 float[] animationTimeBoundaries = this.computeAnimationTimeBoundaries(animation);
                 int maxFrame = (int) animationTimeBoundaries[0];
@@ -233,7 +233,7 @@ public class SimulationNode {
                     for (Bone rootBone : skeleton.getRoots()) {
                         // ignore the 0-indexed bone
                         if (skeleton.getBoneIndex(rootBone) > 0) {
-                            this.applyConstraints(rootBone, alteredOmas, applied, frame);
+                            this.applyConstraints(rootBone, alteredOmas, applied, frame, new Stack<Bone>());
                         }
                     }
 
@@ -294,34 +294,39 @@ public class SimulationNode {
      *            the set of OMAS of the altered bones (is populated if necessary)
      * @param frame
      *            the current frame of the animation
+     * @param bonesStack
+     *            the stack of bones used to avoid infinite loops while applying constraints
      */
-    private void applyConstraints(Bone bone, Set<Long> alteredOmas, Set<Long> applied, int frame) {
-        BoneContext boneContext = blenderContext.getBoneContext(bone);
-        if(!applied.contains(boneContext.getBoneOma())) {
-            List<Constraint> constraints = this.findConstraints(boneContext.getBoneOma(), blenderContext);
-            if (constraints != null && constraints.size() > 0) {
-                // TODO: BEWARE OF INFINITE LOOPS !!!!!!!!!!!!!!!!!!!!!!!!!!
-                for (Constraint constraint : constraints) {
-                    if (constraint.getTargetOMA() != null && constraint.getTargetOMA() > 0L) {
-                        // first apply constraints of the target bone
-                        BoneContext targetBone = blenderContext.getBoneContext(constraint.getTargetOMA());
-                        this.applyConstraints(targetBone.getBone(), alteredOmas, applied, frame);
-                    }
-                    constraint.apply(frame);
-                    if (constraint.getAlteredOmas() != null) {
-                        alteredOmas.addAll(constraint.getAlteredOmas());
+    private void applyConstraints(Bone bone, Set<Long> alteredOmas, Set<Long> applied, int frame, Stack<Bone> bonesStack) {
+        if (!bonesStack.contains(bone)) {
+            bonesStack.push(bone);
+            BoneContext boneContext = blenderContext.getBoneContext(bone);
+            if (!applied.contains(boneContext.getBoneOma())) {
+                List<Constraint> constraints = this.findConstraints(boneContext.getBoneOma(), blenderContext);
+                if (constraints != null && constraints.size() > 0) {
+                    for (Constraint constraint : constraints) {
+                        if (constraint.getTargetOMA() != null && constraint.getTargetOMA() > 0L) {
+                            // first apply constraints of the target bone
+                            BoneContext targetBone = blenderContext.getBoneContext(constraint.getTargetOMA());
+                            this.applyConstraints(targetBone.getBone(), alteredOmas, applied, frame, bonesStack);
+                        }
+                        constraint.apply(frame);
+                        if (constraint.getAlteredOmas() != null) {
+                            alteredOmas.addAll(constraint.getAlteredOmas());
+                        }
+                        alteredOmas.add(boneContext.getBoneOma());
                     }
-                    alteredOmas.add(boneContext.getBoneOma());
                 }
+                applied.add(boneContext.getBoneOma());
             }
-            applied.add(boneContext.getBoneOma());
-        }
-        
-        List<Bone> children = bone.getChildren();
-        if (children != null && children.size() > 0) {
-            for (Bone child : bone.getChildren()) {
-                this.applyConstraints(child, alteredOmas, applied, frame);
+
+            List<Bone> children = bone.getChildren();
+            if (children != null && children.size() > 0) {
+                for (Bone child : bone.getChildren()) {
+                    this.applyConstraints(child, alteredOmas, applied, frame, bonesStack);
+                }
             }
+            bonesStack.pop();
         }
     }
 

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

@@ -1,9 +1,9 @@
 package com.jme3.scene.plugins.blender.constraints.definitions;
 
 import java.util.ArrayList;
+import java.util.Collection;
 import java.util.HashSet;
 import java.util.List;
-import java.util.Set;
 
 import org.ejml.simple.SimpleMatrix;
 
@@ -19,18 +19,26 @@ import com.jme3.scene.plugins.blender.math.DTransform;
 import com.jme3.scene.plugins.blender.math.Matrix;
 import com.jme3.scene.plugins.blender.math.Vector3d;
 
+/**
+ * A definiotion of a Inverse Kinematics constraint. This implementation uses Jacobian pseudoinverse algorithm.
+ * 
+ * @author Marcin Roguski (Kaelthas)
+ */
 public class ConstraintDefinitionIK extends ConstraintDefinition {
-    private static final float MIN_DISTANCE  = 0.001f;
-    private static final int   FLAG_USE_TAIL = 0x01;
-    private static final int   FLAG_POSITION = 0x20;
+    private static final float MIN_DISTANCE     = 0.001f;
+    private static final float MIN_ANGLE_CHANGE = 0.001f;
+    private static final int   FLAG_USE_TAIL    = 0x01;
+    private static final int   FLAG_POSITION    = 0x20;
 
-    private BonesChain         bones;
+    private BonesChain bones;
     /** The number of affected bones. Zero means that all parent bones of the current bone should take part in baking. */
-    private int                bonesAffected;
+    private int        bonesAffected;
     /** Indicates if the tail of the bone should be used or not. */
-    private boolean            useTail;
+    private boolean    useTail;
     /** The amount of iterations of the algorithm. */
-    private int                iterations;
+    private int        iterations;
+    /** The count of bones' chain. */
+    private int        bonesCount = -1;
 
     public ConstraintDefinitionIK(Structure constraintData, Long ownerOMA, BlenderContext blenderContext) {
         super(constraintData, ownerOMA, blenderContext);
@@ -47,85 +55,104 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
         }
     }
 
-    @Override
-    public Set<Long> getAlteredOmas() {
-        return bones.alteredOmas;
-    }
+    /**
+     * Below are the variables that only need to be allocated once for IK constraint instance.
+     */
+    /** Temporal quaternion. */
+    private DQuaternion tempDQuaternion = new DQuaternion();
+    /** Temporal matrix column. */
+    private Vector3d    col             = new Vector3d();
+    /** Effector's position change. */
+    private Matrix      deltaP          = new Matrix(3, 1);
+    /** The current target position. */
+    private Vector3d    target          = new Vector3d();
+    /** Rotation vectors for each joint (allocated when we know the size of a bones' chain. */
+    private Vector3d[]  rotationVectors;
+    /** The Jacobian matrix. Allocated when the bones' chain size is known. */
+    private Matrix      J;
 
     @Override
     public void bake(Space ownerSpace, Space targetSpace, Transform targetTransform, float influence) {
-        if (influence == 0 || !trackToBeChanged || targetTransform == null) {
+        if (influence == 0 || !trackToBeChanged || targetTransform == null || bonesCount == 0) {
             return;// no need to do anything
         }
 
-        DQuaternion q = new DQuaternion();
-        Vector3d t = new Vector3d(targetTransform.getTranslation());
-        bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, blenderContext);
+        if (bones == null) {
+            bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, alteredOmas, blenderContext);
+        }
         if (bones.size() == 0) {
+            bonesCount = 0;
             return;// no need to do anything
         }
         double distanceFromTarget = Double.MAX_VALUE;
+        target.set(targetTransform.getTranslation().x, targetTransform.getTranslation().y, targetTransform.getTranslation().z);
+
+        if (bonesCount < 0) {
+            bonesCount = bones.size();
+            rotationVectors = new Vector3d[bonesCount];
+            for (int i = 0; i < bonesCount; ++i) {
+                rotationVectors[i] = new Vector3d();
+            }
+            J = new Matrix(3, bonesCount);
+        }
 
-        Vector3d target = new Vector3d(targetTransform.getTranslation());
-        Vector3d[] rotationVectors = new Vector3d[bones.size()];
         BoneContext topBone = bones.get(0);
-        for (int i = 1; i <= iterations; ++i) {
+        for (int i = 0; i < iterations; ++i) {
             DTransform topBoneTransform = bones.getWorldTransform(topBone);
             Vector3d e = topBoneTransform.getTranslation().add(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
-            distanceFromTarget = e.distance(t);
+            distanceFromTarget = e.distance(target);
             if (distanceFromTarget <= MIN_DISTANCE) {
                 break;
             }
 
-            Matrix deltaP = new Matrix(3, 1);
-            deltaP.setColumn(target.subtract(e), 0);
-
-            Matrix J = new Matrix(3, bones.size());
+            deltaP.setColumn(0, 0, target.x - e.x, target.y - e.y, target.z - e.z);
             int column = 0;
             for (BoneContext boneContext : bones) {
                 DTransform boneWorldTransform = bones.getWorldTransform(boneContext);
                 Vector3d j = boneWorldTransform.getTranslation(); // current join position
                 Vector3d vectorFromJointToEffector = e.subtract(j);
-                rotationVectors[column] = vectorFromJointToEffector.cross(target.subtract(j)).normalize();
-                Vector3d col = rotationVectors[column].cross(vectorFromJointToEffector);
+                vectorFromJointToEffector.cross(target.subtract(j), rotationVectors[column]).normalizeLocal();
+                rotationVectors[column].cross(vectorFromJointToEffector, col);
                 J.setColumn(col, column++);
             }
             Matrix J_1 = J.pseudoinverse();
 
             SimpleMatrix deltaThetas = J_1.mult(deltaP);
-
+            if (deltaThetas.elementMaxAbs() < MIN_ANGLE_CHANGE) {
+                break;
+            }
             for (int j = 0; j < deltaThetas.numRows(); ++j) {
                 double angle = deltaThetas.get(j, 0);
                 Vector3d rotationVector = rotationVectors[j];
 
-                q.fromAngleAxis(angle, rotationVector);
+                tempDQuaternion.fromAngleAxis(angle, rotationVector);
                 BoneContext boneContext = bones.get(j);
                 Bone bone = boneContext.getBone();
                 if (bone.equals(this.getOwner())) {
                     if (boneContext.isLockX()) {
-                        q.set(0, q.getY(), q.getZ(), q.getW());
+                        tempDQuaternion.set(0, tempDQuaternion.getY(), tempDQuaternion.getZ(), tempDQuaternion.getW());
                     }
                     if (boneContext.isLockY()) {
-                        q.set(q.getX(), 0, q.getZ(), q.getW());
+                        tempDQuaternion.set(tempDQuaternion.getX(), 0, tempDQuaternion.getZ(), tempDQuaternion.getW());
                     }
                     if (boneContext.isLockZ()) {
-                        q.set(q.getX(), q.getY(), 0, q.getW());
+                        tempDQuaternion.set(tempDQuaternion.getX(), tempDQuaternion.getY(), 0, tempDQuaternion.getW());
                     }
                 }
 
                 DTransform boneTransform = bones.getWorldTransform(boneContext);
-                boneTransform.getRotation().set(q.mult(boneTransform.getRotation()));
+                boneTransform.getRotation().set(tempDQuaternion.mult(boneTransform.getRotation()));
                 bones.setWorldTransform(boneContext, boneTransform);
             }
         }
 
         // applying the results
-        for (int i = bones.size() - 1; i >= 0; --i) {
+        for (int i = bonesCount - 1; i >= 0; --i) {
             BoneContext boneContext = bones.get(i);
             DTransform transform = bones.getWorldTransform(boneContext);
             constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, transform.toTransform());
         }
-        bones.reset();
+        bones = null;// need to reload them again
     }
 
     @Override
@@ -133,47 +160,39 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
         return "Inverse kinematics";
     }
 
-    @Override
-    public boolean isTrackToBeChanged() {
-        if (trackToBeChanged) {
-            // need to check the bone structure too (when constructor was called not all of the bones might have been loaded yet)
-            // that is why it is also checked here
-            bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, blenderContext);
-            trackToBeChanged = bones.size() > 0;
-        }
-        return trackToBeChanged;
-    }
-
     @Override
     public boolean isTargetRequired() {
         return true;
     }
 
+    /**
+     * Loaded bones' chain. This class allows to operate on transform matrices that use double precision in computations.
+     * Only the final result is being transformed to single precision numbers.
+     * 
+     * @author Marcin Roguski (Kaelthas)
+     */
     private static class BonesChain extends ArrayList<BoneContext> {
-        private static final long serialVersionUID      = -1850524345643600718L;
+        private static final long serialVersionUID = -1850524345643600718L;
 
-        private Set<Long>         alteredOmas           = new HashSet<Long>();
-        private List<Matrix>    originalBonesMatrices = new ArrayList<Matrix>();
-        private List<Matrix>    bonesMatrices         = new ArrayList<Matrix>();
+        private List<Matrix> bonesMatrices = new ArrayList<Matrix>();
 
-        public BonesChain(Bone bone, boolean useTail, int bonesAffected, BlenderContext blenderContext) {
+        public BonesChain(Bone bone, boolean useTail, int bonesAffected, Collection<Long> alteredOmas, BlenderContext blenderContext) {
             if (bone != null) {
                 ConstraintHelper constraintHelper = blenderContext.getHelper(ConstraintHelper.class);
                 if (!useTail) {
                     bone = bone.getParent();
                 }
-                while (bone != null && this.size() < bonesAffected) {
+                while (bone != null && (bonesAffected <= 0 || this.size() < bonesAffected)) {
                     BoneContext boneContext = blenderContext.getBoneContext(bone);
                     this.add(boneContext);
                     alteredOmas.add(boneContext.getBoneOma());
 
                     Space space = this.size() < bonesAffected ? Space.CONSTRAINT_SPACE_LOCAL : Space.CONSTRAINT_SPACE_WORLD;
                     Transform transform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), space);
-                    originalBonesMatrices.add(new DTransform(transform).toMatrix());
+                    bonesMatrices.add(new DTransform(transform).toMatrix());
 
                     bone = bone.getParent();
                 }
-                this.reset();
             }
         }
 
@@ -204,12 +223,5 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
             result = result.mult(bonesMatrices.get(index));
             return new Matrix(result);
         }
-
-        public void reset() {
-            bonesMatrices.clear();
-            for (Matrix m : originalBonesMatrices) {
-                bonesMatrices.add(new Matrix(m));
-            }
-        }
     }
 }

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java

@@ -51,7 +51,7 @@ public class Matrix extends SimpleMatrix {
         
         int N = Math.min(this.numRows(),this.numCols());
         double maxSingular = 0;
-        for( int i = 0; i < N; i++ ) {
+        for( int i = 0; i < N; ++i ) {
             if( S.get(i, i) > maxSingular ) {
                 maxSingular = S.get(i, i);
             }