diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/SimulationNode.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/SimulationNode.java index 031676b94..711e0a338 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/SimulationNode.java +++ b/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 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 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 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()); } } @@ -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 alteredOmas, Set applied, int frame) { - BoneContext boneContext = blenderContext.getBoneContext(bone); - if(!applied.contains(boneContext.getBoneOma())) { - List 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 alteredOmas, Set applied, int frame, Stack bonesStack) { + if (!bonesStack.contains(bone)) { + bonesStack.push(bone); + BoneContext boneContext = blenderContext.getBoneContext(bone); + if (!applied.contains(boneContext.getBoneOma())) { + List 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 children = bone.getChildren(); - if (children != null && children.size() > 0) { - for (Bone child : bone.getChildren()) { - this.applyConstraints(child, alteredOmas, applied, frame); + + List children = bone.getChildren(); + if (children != null && children.size() > 0) { + for (Bone child : bone.getChildren()) { + this.applyConstraints(child, alteredOmas, applied, frame, bonesStack); + } } + bonesStack.pop(); } } diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java index 820a283b3..69e087a28 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java +++ b/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 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 { - private static final long serialVersionUID = -1850524345643600718L; + private static final long serialVersionUID = -1850524345643600718L; - private Set alteredOmas = new HashSet(); - private List originalBonesMatrices = new ArrayList(); - private List bonesMatrices = new ArrayList(); + private List bonesMatrices = new ArrayList(); - public BonesChain(Bone bone, boolean useTail, int bonesAffected, BlenderContext blenderContext) { + public BonesChain(Bone bone, boolean useTail, int bonesAffected, Collection 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)); - } - } } } diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java index 5ea580b84..29550f23b 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java +++ b/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); }