|
|
@ -26,6 +26,7 @@ import com.jme3.scene.plugins.blender.math.Vector3d; |
|
|
|
*/ |
|
|
|
*/ |
|
|
|
public class ConstraintDefinitionIK extends ConstraintDefinition { |
|
|
|
public class ConstraintDefinitionIK extends ConstraintDefinition { |
|
|
|
private static final float MIN_DISTANCE = 0.001f; |
|
|
|
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_USE_TAIL = 0x01; |
|
|
|
private static final int FLAG_POSITION = 0x20; |
|
|
|
private static final int FLAG_POSITION = 0x20; |
|
|
|
|
|
|
|
|
|
|
@ -117,7 +118,9 @@ public class ConstraintDefinitionIK extends ConstraintDefinition { |
|
|
|
Matrix J_1 = J.pseudoinverse(); |
|
|
|
Matrix J_1 = J.pseudoinverse(); |
|
|
|
|
|
|
|
|
|
|
|
SimpleMatrix deltaThetas = J_1.mult(deltaP); |
|
|
|
SimpleMatrix deltaThetas = J_1.mult(deltaP); |
|
|
|
|
|
|
|
if (deltaThetas.elementMaxAbs() < MIN_ANGLE_CHANGE) { |
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
for (int j = 0; j < deltaThetas.numRows(); ++j) { |
|
|
|
for (int j = 0; j < deltaThetas.numRows(); ++j) { |
|
|
|
double angle = deltaThetas.get(j, 0); |
|
|
|
double angle = deltaThetas.get(j, 0); |
|
|
|
Vector3d rotationVector = rotationVectors[j]; |
|
|
|
Vector3d rotationVector = rotationVectors[j]; |
|
|
|