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 c1e64bfa1..5f13ad316 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 @@ -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)); 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 54ee9431c..d3ebc4d18 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,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(); } } - + @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 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 bones = new ArrayList(); - 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 loadBones() { + List bones = new ArrayList(); + 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; }