@ -88,6 +88,8 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
if ( angle ! = 0 ) {
if ( angle ! = 0 ) {
Vector3d cross = currentDir . crossLocal ( target ) . normalizeLocal ( ) ;
Vector3d cross = currentDir . crossLocal ( target ) . normalizeLocal ( ) ;
q . fromAngleAxis ( angle , cross ) ;
q . fromAngleAxis ( angle , cross ) ;
if ( bone . equals ( this . getOwner ( ) ) ) {
if ( boneContext . isLockX ( ) ) {
if ( boneContext . isLockX ( ) ) {
q . set ( 0 , q . getY ( ) , q . getZ ( ) , q . getW ( ) ) ;
q . set ( 0 , q . getY ( ) , q . getZ ( ) , q . getW ( ) ) ;
}
}
@ -97,6 +99,7 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
if ( boneContext . isLockZ ( ) ) {
if ( boneContext . isLockZ ( ) ) {
q . set ( q . getX ( ) , q . getY ( ) , 0 , q . getW ( ) ) ;
q . set ( q . getX ( ) , q . getY ( ) , 0 , q . getW ( ) ) ;
}
}
}
boneTransform . getRotation ( ) . set ( q . multLocal ( boneTransform . getRotation ( ) ) ) ;
boneTransform . getRotation ( ) . set ( q . multLocal ( boneTransform . getRotation ( ) ) ) ;
constraintHelper . applyTransform ( boneContext . getArmatureObjectOMA ( ) , bone . getName ( ) , Space . CONSTRAINT_SPACE_WORLD , boneTransform . toTransform ( ) ) ;
constraintHelper . applyTransform ( boneContext . getArmatureObjectOMA ( ) , bone . getName ( ) , Space . CONSTRAINT_SPACE_WORLD , boneTransform . toTransform ( ) ) ;
@ -107,6 +110,8 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
}
}
}
}
List < Transform > bestSolution = new ArrayList < Transform > ( bones . size ( ) ) ;
double bestSolutionDistance = Double . MAX_VALUE ;
BoneContext topBone = bones . get ( 0 ) ;
BoneContext topBone = bones . get ( 0 ) ;
for ( int i = 0 ; i < iterations & & distanceFromTarget > MIN_DISTANCE ; + + i ) {
for ( int i = 0 ; i < iterations & & distanceFromTarget > MIN_DISTANCE ; + + i ) {
for ( BoneContext boneContext : bones ) {
for ( BoneContext boneContext : bones ) {
@ -124,6 +129,7 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
Vector3d cross = currentDir . crossLocal ( target ) . normalizeLocal ( ) ;
Vector3d cross = currentDir . crossLocal ( target ) . normalizeLocal ( ) ;
q . fromAngleAxis ( angle , cross ) ;
q . fromAngleAxis ( angle , cross ) ;
if ( bone . equals ( this . getOwner ( ) ) ) {
if ( boneContext . isLockX ( ) ) {
if ( boneContext . isLockX ( ) ) {
q . set ( 0 , q . getY ( ) , q . getZ ( ) , q . getW ( ) ) ;
q . set ( 0 , q . getY ( ) , q . getZ ( ) , q . getW ( ) ) ;
}
}
@ -133,6 +139,7 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
if ( boneContext . isLockZ ( ) ) {
if ( boneContext . isLockZ ( ) ) {
q . set ( q . getX ( ) , q . getY ( ) , 0 , q . getW ( ) ) ;
q . set ( q . getX ( ) , q . getY ( ) , 0 , q . getW ( ) ) ;
}
}
}
boneWorldTransform . getRotation ( ) . set ( q . multLocal ( boneWorldTransform . getRotation ( ) ) ) ;
boneWorldTransform . getRotation ( ) . set ( q . multLocal ( boneWorldTransform . getRotation ( ) ) ) ;
constraintHelper . applyTransform ( boneContext . getArmatureObjectOMA ( ) , bone . getName ( ) , Space . CONSTRAINT_SPACE_WORLD , boneWorldTransform . toTransform ( ) ) ;
constraintHelper . applyTransform ( boneContext . getArmatureObjectOMA ( ) , bone . getName ( ) , Space . CONSTRAINT_SPACE_WORLD , boneWorldTransform . toTransform ( ) ) ;
@ -145,6 +152,20 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
DTransform topBoneTransform = new DTransform ( constraintHelper . getTransform ( topBone . getArmatureObjectOMA ( ) , topBone . getBone ( ) . getName ( ) , Space . CONSTRAINT_SPACE_WORLD ) ) ;
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
Vector3d e = topBoneTransform . getTranslation ( ) . addLocal ( topBoneTransform . getRotation ( ) . mult ( Vector3d . UNIT_Y ) . multLocal ( topBone . getLength ( ) ) ) ; // effector
distanceFromTarget = e . distance ( t ) ;
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 ) {
BoneContext boneContext = bones . get ( i ) ;
constraintHelper . applyTransform ( boneContext . getArmatureObjectOMA ( ) , boneContext . getBone ( ) . getName ( ) , Space . CONSTRAINT_SPACE_WORLD , bestSolution . get ( i ) ) ;
}
}
}
}