BoneContext[] boneContexts = this.getBones();
float b = chainLength;
Quaternion boneWorldRotation = new Quaternion();
for (int i = 0; i < boneContexts.length; ++i) {
Bone bone = boneContexts[i].getBone();
bone.updateWorldVectors();
Transform boneWorldTransform = constraintHelper.getTransform(boneContexts[i].getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD);
Vector3f head = boneWorldTransform.getTranslation();
Vector3f tail = head.add(bone.getModelSpaceRotation().mult(Vector3f.UNIT_Y.mult(boneContexts[i].getLength())));
Vector3f vectorA = tail.subtract(head);
float a = vectorA.length();
vectorA.normalizeLocal();
Vector3f vectorC = targetTransform.getTranslation().subtract(head);
float c = vectorC.length();
vectorC.normalizeLocal();
b -= a;
float theta = 0;
if (c >= a + b) {
theta = vectorA.angleBetween(vectorC);
} else if (c <= FastMath.abs(a - b) && i < boneContexts.length - 1) {
theta = vectorA.angleBetween(vectorC) - FastMath.HALF_PI;
} else {
theta = vectorA.angleBetween(vectorC) - FastMath.acos(-(b * b - a * a - c * c) / (2 * a * c));
}
theta *= influence;
if (theta != 0) {
Vector3f vectorR = vectorA.cross(vectorC);
boneWorldRotation.fromAngleAxis(theta, vectorR);
boneWorldTransform.getRotation().multLocal(boneWorldRotation);
constraintHelper.applyTransform(boneContexts[i].getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneWorldTransform);
}
bone.updateWorldVectors();
alteredOmas.add(boneContexts[i].getBoneOma());
}
}
}