@ -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 ) ;
}
}
}
Dokthar
9 years ago