|
|
|
|
@ -1,40 +1,44 @@ |
|
|
|
|
package com.jme3.scene.plugins.blender.constraints.definitions; |
|
|
|
|
|
|
|
|
|
import java.util.ArrayList; |
|
|
|
|
import java.util.Collections; |
|
|
|
|
import java.util.Collection; |
|
|
|
|
import java.util.HashSet; |
|
|
|
|
import java.util.List; |
|
|
|
|
|
|
|
|
|
import org.ejml.simple.SimpleMatrix; |
|
|
|
|
|
|
|
|
|
import com.jme3.animation.Bone; |
|
|
|
|
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; |
|
|
|
|
import com.jme3.scene.plugins.blender.math.DQuaternion; |
|
|
|
|
import com.jme3.scene.plugins.blender.math.DTransform; |
|
|
|
|
import com.jme3.scene.plugins.blender.math.Matrix; |
|
|
|
|
import com.jme3.scene.plugins.blender.math.Vector3d; |
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
* The Inverse Kinematics constraint. |
|
|
|
|
* A definiotion of a Inverse Kinematics constraint. This implementation uses Jacobian pseudoinverse algorithm. |
|
|
|
|
* |
|
|
|
|
* @author Wesley Shillingford (wezrule) |
|
|
|
|
* @author Marcin Roguski (Kaelthas) |
|
|
|
|
*/ |
|
|
|
|
public class ConstraintDefinitionIK extends ConstraintDefinition { |
|
|
|
|
private static final float MIN_DISTANCE = 0.0001f; |
|
|
|
|
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; |
|
|
|
|
/** The number of affected bones. Zero means that all parent bones of the current bone should take part in baking. */ |
|
|
|
|
private int bonesAffected; |
|
|
|
|
/** The total length of the bone chain. Useful for optimisation of computations speed in some cases. */ |
|
|
|
|
private double chainLength; |
|
|
|
|
/** Indicates if the tail of the bone should be used or not. */ |
|
|
|
|
private boolean useTail; |
|
|
|
|
/** The amount of iterations of the algorithm. */ |
|
|
|
|
private int iterations; |
|
|
|
|
/** The count of bones' chain. */ |
|
|
|
|
private int bonesCount = -1; |
|
|
|
|
|
|
|
|
|
public ConstraintDefinitionIK(Structure constraintData, Long ownerOMA, BlenderContext blenderContext) { |
|
|
|
|
super(constraintData, ownerOMA, blenderContext); |
|
|
|
|
@ -51,122 +55,104 @@ public class ConstraintDefinitionIK extends ConstraintDefinition { |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
* 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()); |
|
|
|
|
List<BoneContext> bones = this.loadBones(); |
|
|
|
|
|
|
|
|
|
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); |
|
|
|
|
|
|
|
|
|
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(new Vector3d(rootBoneTransform.getTranslation())) >= chainLength) { |
|
|
|
|
Collections.reverse(bones); |
|
|
|
|
|
|
|
|
|
for (BoneContext boneContext : bones) { |
|
|
|
|
Bone bone = boneContext.getBone(); |
|
|
|
|
DTransform boneTransform = new DTransform(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD)); |
|
|
|
|
|
|
|
|
|
Vector3d e = boneTransform.getTranslation().add(boneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(boneContext.getLength()));// effector
|
|
|
|
|
Vector3d j = boneTransform.getTranslation(); // current join position
|
|
|
|
|
|
|
|
|
|
Vector3d currentDir = e.subtractLocal(j).normalizeLocal(); |
|
|
|
|
Vector3d target = t.subtract(j).normalizeLocal(); |
|
|
|
|
double angle = currentDir.angleBetween(target); |
|
|
|
|
if (angle != 0) { |
|
|
|
|
Vector3d cross = currentDir.crossLocal(target).normalizeLocal(); |
|
|
|
|
q.fromAngleAxis(angle, cross); |
|
|
|
|
|
|
|
|
|
if(bone.equals(this.getOwner())) { |
|
|
|
|
if (boneContext.isLockX()) { |
|
|
|
|
q.set(0, q.getY(), q.getZ(), q.getW()); |
|
|
|
|
} |
|
|
|
|
if (boneContext.isLockY()) { |
|
|
|
|
q.set(q.getX(), 0, q.getZ(), q.getW()); |
|
|
|
|
} |
|
|
|
|
if (boneContext.isLockZ()) { |
|
|
|
|
q.set(q.getX(), q.getY(), 0, q.getW()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
boneTransform.getRotation().set(q.multLocal(boneTransform.getRotation())); |
|
|
|
|
constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneTransform.toTransform()); |
|
|
|
|
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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
iterations = 0; |
|
|
|
|
} |
|
|
|
|
BoneContext topBone = bones.get(0); |
|
|
|
|
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(target); |
|
|
|
|
if (distanceFromTarget <= MIN_DISTANCE) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
List<Transform> bestSolution = new ArrayList<Transform>(bones.size()); |
|
|
|
|
double bestSolutionDistance = Double.MAX_VALUE; |
|
|
|
|
BoneContext topBone = bones.get(0); |
|
|
|
|
for (int i = 0; i < iterations && distanceFromTarget > MIN_DISTANCE; ++i) { |
|
|
|
|
deltaP.setColumn(0, 0, target.x - e.x, target.y - e.y, target.z - e.z); |
|
|
|
|
int column = 0; |
|
|
|
|
for (BoneContext boneContext : bones) { |
|
|
|
|
Bone bone = boneContext.getBone(); |
|
|
|
|
DTransform topBoneTransform = new DTransform(constraintHelper.getTransform(topBone.getArmatureObjectOMA(), topBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD)); |
|
|
|
|
DTransform boneWorldTransform = new DTransform(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD)); |
|
|
|
|
|
|
|
|
|
Vector3d e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
|
|
|
|
|
DTransform boneWorldTransform = bones.getWorldTransform(boneContext); |
|
|
|
|
Vector3d j = boneWorldTransform.getTranslation(); // current join position
|
|
|
|
|
Vector3d vectorFromJointToEffector = e.subtract(j); |
|
|
|
|
vectorFromJointToEffector.cross(target.subtract(j), rotationVectors[column]).normalizeLocal(); |
|
|
|
|
rotationVectors[column].cross(vectorFromJointToEffector, col); |
|
|
|
|
J.setColumn(col, column++); |
|
|
|
|
} |
|
|
|
|
Matrix J_1 = J.pseudoinverse(); |
|
|
|
|
|
|
|
|
|
Vector3d currentDir = e.subtractLocal(j).normalizeLocal(); |
|
|
|
|
Vector3d target = t.subtract(j).normalizeLocal(); |
|
|
|
|
double angle = currentDir.angleBetween(target); |
|
|
|
|
if (angle != 0) { |
|
|
|
|
Vector3d cross = currentDir.crossLocal(target).normalizeLocal(); |
|
|
|
|
q.fromAngleAxis(angle, cross); |
|
|
|
|
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]; |
|
|
|
|
|
|
|
|
|
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()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
boneWorldTransform.getRotation().set(q.multLocal(boneWorldTransform.getRotation())); |
|
|
|
|
constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneWorldTransform.toTransform()); |
|
|
|
|
} else { |
|
|
|
|
iterations = 0; |
|
|
|
|
break; |
|
|
|
|
DTransform boneTransform = bones.getWorldTransform(boneContext); |
|
|
|
|
boneTransform.getRotation().set(tempDQuaternion.mult(boneTransform.getRotation())); |
|
|
|
|
bones.setWorldTransform(boneContext, boneTransform); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
DTransform topBoneTransform = new DTransform(constraintHelper.getTransform(topBone.getArmatureObjectOMA(), topBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD)); |
|
|
|
|
Vector3d e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
|
|
|
|
|
distanceFromTarget = e.distance(t); |
|
|
|
|
|
|
|
|
|
if(distanceFromTarget < bestSolutionDistance) { |
|
|
|
|
bestSolutionDistance = distanceFromTarget; |
|
|
|
|
bestSolution.clear(); |
|
|
|
|
for(BoneContext boneContext : bones) { |
|
|
|
|
bestSolution.add(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD)); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// applying best solution
|
|
|
|
|
for(int i=0;i<bestSolution.size();++i) { |
|
|
|
|
// applying the results
|
|
|
|
|
for (int i = bonesCount - 1; i >= 0; --i) { |
|
|
|
|
BoneContext boneContext = bones.get(i); |
|
|
|
|
constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, bestSolution.get(i)); |
|
|
|
|
DTransform transform = bones.getWorldTransform(boneContext); |
|
|
|
|
constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, transform.toTransform()); |
|
|
|
|
} |
|
|
|
|
bones = null;// need to reload them again
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@Override |
|
|
|
|
@ -174,56 +160,68 @@ public class ConstraintDefinitionIK extends ConstraintDefinition { |
|
|
|
|
return "Inverse kinematics"; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@Override |
|
|
|
|
public boolean isTargetRequired() { |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
* @return the bone contexts of all bones that will be used in this constraint computations |
|
|
|
|
* 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 List<BoneContext> loadBones() { |
|
|
|
|
List<BoneContext> bones = new ArrayList<BoneContext>(); |
|
|
|
|
Bone bone = (Bone) this.getOwner(); |
|
|
|
|
if (bone == null) { |
|
|
|
|
return bones; |
|
|
|
|
} |
|
|
|
|
private static class BonesChain extends ArrayList<BoneContext> { |
|
|
|
|
private static final long serialVersionUID = -1850524345643600718L; |
|
|
|
|
|
|
|
|
|
private List<Matrix> bonesMatrices = new ArrayList<Matrix>(); |
|
|
|
|
|
|
|
|
|
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(); |
|
|
|
|
} |
|
|
|
|
chainLength = 0; |
|
|
|
|
while (bone != null) { |
|
|
|
|
while (bone != null && (bonesAffected <= 0 || this.size() < bonesAffected)) { |
|
|
|
|
BoneContext boneContext = blenderContext.getBoneContext(bone); |
|
|
|
|
chainLength += boneContext.getLength(); |
|
|
|
|
bones.add(boneContext); |
|
|
|
|
this.add(boneContext); |
|
|
|
|
alteredOmas.add(boneContext.getBoneOma()); |
|
|
|
|
if (bonesAffected != 0 && bones.size() >= bonesAffected) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
// need to add spaces between bones to the chain length
|
|
|
|
|
Transform boneWorldTransform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD); |
|
|
|
|
Vector3f boneWorldTranslation = boneWorldTransform.getTranslation(); |
|
|
|
|
|
|
|
|
|
bone = bone.getParent(); |
|
|
|
|
Space space = this.size() < bonesAffected ? Space.CONSTRAINT_SPACE_LOCAL : Space.CONSTRAINT_SPACE_WORLD; |
|
|
|
|
Transform transform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), space); |
|
|
|
|
bonesMatrices.add(new DTransform(transform).toMatrix()); |
|
|
|
|
|
|
|
|
|
if (bone != null) { |
|
|
|
|
boneContext = blenderContext.getBoneContext(bone); |
|
|
|
|
Transform parentWorldTransform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD); |
|
|
|
|
Vector3f parentWorldTranslation = parentWorldTransform.getTranslation(); |
|
|
|
|
chainLength += boneWorldTranslation.distance(parentWorldTranslation); |
|
|
|
|
bone = bone.getParent(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
return bones; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@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
|
|
|
|
|
List<BoneContext> bones = this.loadBones(); |
|
|
|
|
trackToBeChanged = bones.size() > 0; |
|
|
|
|
public DTransform getWorldTransform(BoneContext bone) { |
|
|
|
|
int index = this.indexOf(bone); |
|
|
|
|
return this.getWorldMatrix(index).toTransform(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
public void setWorldTransform(BoneContext bone, DTransform transform) { |
|
|
|
|
int index = this.indexOf(bone); |
|
|
|
|
Matrix boneMatrix = transform.toMatrix(); |
|
|
|
|
|
|
|
|
|
if (index < this.size() - 1) { |
|
|
|
|
// computing the current bone local transform
|
|
|
|
|
Matrix parentWorldMatrix = this.getWorldMatrix(index + 1); |
|
|
|
|
SimpleMatrix m = parentWorldMatrix.invert().mult(boneMatrix); |
|
|
|
|
boneMatrix = new Matrix(m); |
|
|
|
|
} |
|
|
|
|
return trackToBeChanged; |
|
|
|
|
bonesMatrices.set(index, boneMatrix); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@Override |
|
|
|
|
public boolean isTargetRequired() { |
|
|
|
|
return true; |
|
|
|
|
public Matrix getWorldMatrix(int index) { |
|
|
|
|
if (index == this.size() - 1) { |
|
|
|
|
return new Matrix(bonesMatrices.get(this.size() - 1)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
SimpleMatrix result = this.getWorldMatrix(index + 1); |
|
|
|
|
result = result.mult(bonesMatrices.get(index)); |
|
|
|
|
return new Matrix(result); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
Nehon
10 years ago