|
@@ -110,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);
|
|
|
for (int i = 0; i < iterations && distanceFromTarget > MIN_DISTANCE; ++i) {
|
|
|
for (BoneContext boneContext : bones) {
|
|
@@ -150,6 +152,20 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
|
|
|
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) {
|
|
|
+ BoneContext boneContext = bones.get(i);
|
|
|
+ constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, bestSolution.get(i));
|
|
|
}
|
|
|
}
|
|
|
|