Examples with DTransform com.jme3.scene.plugins.blender.math.DTransform used on opensource projects

Example 1 with DTransform

use of com.jme3.scene.plugins.blender.math.DTransform in project jmonkeyengine by jMonkeyEngine.

the class ConstraintDefinitionIK method bake.

@Override
public void bake(Space ownerSpace, Space targetSpace, Transform targetTransform, float influence) {
    if (influence == 0 || !trackToBeChanged || targetTransform == null || bonesCount == 0) {
        // no need to do anything
        return;
    }
    if (bones == null) {
        bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, alteredOmas, blenderContext);
    }
    if (bones.size() == 0) {
        bonesCount = 0;
        // no need to do anything
        return;
    }
    double distanceFromTarget = Double.MAX_VALUE;
    target.set(targetTransform.getTranslation().x, targetTransform.getTranslation().y, targetTransform.getTranslation().z);
    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);
    }
    BoneContext topBone = bones.get(0);
    for (int i = 0; i < iterations; ++i) {
        DTransform topBoneTransform = bones.getWorldTransform(topBone);
        // effector
        Vector3d e = topBoneTransform.getTranslation().add(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));
        distanceFromTarget = e.distance(target);
        if (distanceFromTarget <= MIN_DISTANCE) {
            break;
        }
        deltaP.setColumn(0, 0, target.x - e.x, target.y - e.y, target.z - e.z);
        int column = 0;
        for (BoneContext boneContext : bones) {
            DTransform boneWorldTransform = bones.getWorldTransform(boneContext);
            // current join position
            Vector3d j = boneWorldTransform.getTranslation();
            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();
        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()) {
                    tempDQuaternion.set(0, tempDQuaternion.getY(), tempDQuaternion.getZ(), tempDQuaternion.getW());
                }
                if (boneContext.isLockY()) {
                    tempDQuaternion.set(tempDQuaternion.getX(), 0, tempDQuaternion.getZ(), tempDQuaternion.getW());
                }
                if (boneContext.isLockZ()) {
                    tempDQuaternion.set(tempDQuaternion.getX(), tempDQuaternion.getY(), 0, tempDQuaternion.getW());
                }
            }
            DTransform boneTransform = bones.getWorldTransform(boneContext);
            boneTransform.getRotation().set(tempDQuaternion.mult(boneTransform.getRotation()));
            bones.setWorldTransform(boneContext, boneTransform);
        }
    }
    // applying the results
    for (int i = bonesCount - 1; i >= 0; --i) {
        BoneContext boneContext = bones.get(i);
        DTransform transform = bones.getWorldTransform(boneContext);
        constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, transform.toTransform());
    }
    // need to reload them again
    bones = null;
}