com.jme3.scene.plugins.blender.constraints.definitions.ConstraintDefinitionIK Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of jme3-blender Show documentation
jMonkeyEngine is a 3D game engine for adventurous Java developers
The newest version!
package com.jme3.scene.plugins.blender.constraints.definitions;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.List;

import org.ejml.simple.SimpleMatrix;

import com.jme3.animation.Bone;
import com.jme3.math.Transform;
import com.jme3.scene.plugins.blender.BlenderContext;
import com.jme3.scene.plugins.blender.animations.BoneContext;
import com.jme3.scene.plugins.blender.constraints.ConstraintHelper;
import com.jme3.scene.plugins.blender.constraints.ConstraintHelper.Space;
import com.jme3.scene.plugins.blender.file.Structure;
import com.jme3.scene.plugins.blender.math.DQuaternion;
import com.jme3.scene.plugins.blender.math.DTransform;
import com.jme3.scene.plugins.blender.math.Matrix;
import com.jme3.scene.plugins.blender.math.Vector3d;

/**
 * A definiotion of a Inverse Kinematics constraint. This implementation uses Jacobian pseudoinverse algorithm.
 * 
 * @author Marcin Roguski (Kaelthas)
 */
public class ConstraintDefinitionIK extends ConstraintDefinition {
    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_POSITION    = 0x20;

    private BonesChain bones;
    /** The number of affected bones. Zero means that all parent bones of the current bone should take part in baking. */
    private int        bonesAffected;
    /** Indicates if the tail of the bone should be used or not. */
    private boolean    useTail;
    /** The amount of iterations of the algorithm. */
    private int        iterations;
    /** The count of bones' chain. */
    private int        bonesCount = -1;

    public ConstraintDefinitionIK(Structure constraintData, Long ownerOMA, BlenderContext blenderContext) {
        super(constraintData, ownerOMA, blenderContext);
        bonesAffected = ((Number) constraintData.getFieldValue("rootbone")).intValue();
        iterations = ((Number) constraintData.getFieldValue("iterations")).intValue();
        useTail = (flag & FLAG_USE_TAIL) != 0;

        if ((flag & FLAG_POSITION) == 0) {
            trackToBeChanged = false;
        }

        if (trackToBeChanged) {
            alteredOmas = new HashSet();
        }
    }

    /**
     * Below are the variables that only need to be allocated once for IK constraint instance.
     */
    /** Temporal quaternion. */
    private DQuaternion tempDQuaternion = new DQuaternion();
    /** Temporal matrix column. */
    private Vector3d    col             = new Vector3d();
    /** Effector's position change. */
    private Matrix      deltaP          = new Matrix(3, 1);
    /** The current target position. */
    private Vector3d    target          = new Vector3d();
    /** Rotation vectors for each joint (allocated when we know the size of a bones' chain. */
    private Vector3d[]  rotationVectors;
    /** The Jacobian matrix. Allocated when the bones' chain size is known. */
    private Matrix      J;

    @Override
    public void bake(Space ownerSpace, Space targetSpace, Transform targetTransform, float influence) {
        if (influence == 0 || !trackToBeChanged || targetTransform == null || bonesCount == 0) {
            return;// no need to do anything
        }

        if (bones == null) {
            bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, alteredOmas, blenderContext);
        }
        if (bones.size() == 0) {
            bonesCount = 0;
            return;// no need to do anything
        }
        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);
            Vector3d e = topBoneTransform.getTranslation().add(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
            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);
                Vector3d j = boneWorldTransform.getTranslation(); // current join position
                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());
        }
        bones = null;// need to reload them again
    }

    @Override
    public String getConstraintTypeName() {
        return "Inverse kinematics";
    }

    @Override
    public boolean isTargetRequired() {
        return true;
    }

    /**
     * Loaded bones' chain. This class allows to operate on transform matrices that use double precision in computations.
     * Only the final result is being transformed to single precision numbers.
     * 
     * @author Marcin Roguski (Kaelthas)
     */
    private static class BonesChain extends ArrayList {
        private static final long serialVersionUID = -1850524345643600718L;

        private List localBonesMatrices = new ArrayList();

        public BonesChain(Bone bone, boolean useTail, int bonesAffected, Collection alteredOmas, BlenderContext blenderContext) {
            if (bone != null) {
                ConstraintHelper constraintHelper = blenderContext.getHelper(ConstraintHelper.class);
                if (!useTail) {
                    bone = bone.getParent();
                }
                while (bone != null && (bonesAffected <= 0 || this.size() < bonesAffected)) {
                    BoneContext boneContext = blenderContext.getBoneContext(bone);
                    this.add(boneContext);
                    alteredOmas.add(boneContext.getBoneOma());

                    Transform transform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
                    localBonesMatrices.add(new DTransform(transform).toMatrix());

                    bone = bone.getParent();
                }
                
                if(localBonesMatrices.size() > 0) {
                	// making the matrices describe the local transformation
                    Matrix parentWorldMatrix = localBonesMatrices.get(localBonesMatrices.size() - 1);
                    for(int i=localBonesMatrices.size() - 2;i>=0;--i) {
                    	SimpleMatrix m = parentWorldMatrix.invert().mult(localBonesMatrices.get(i));
                    	parentWorldMatrix = localBonesMatrices.get(i);
                    	localBonesMatrices.set(i, new Matrix(m));
                    }
                }
            }
        }

        public DTransform getWorldTransform(BoneContext bone) {
            int index = this.indexOf(bone);
            return this.getWorldMatrix(index).toTransform();
        }

        public void setWorldTransform(BoneContext bone, DTransform transform) {
            int index = this.indexOf(bone);
            Matrix boneMatrix = transform.toMatrix();

            if (index < this.size() - 1) {
                // computing the current bone local transform
                Matrix parentWorldMatrix = this.getWorldMatrix(index + 1);
                SimpleMatrix m = parentWorldMatrix.invert().mult(boneMatrix);
                boneMatrix = new Matrix(m);
            }
            localBonesMatrices.set(index, boneMatrix);
        }

        public Matrix getWorldMatrix(int index) {
            if (index == this.size() - 1) {
                return new Matrix(localBonesMatrices.get(this.size() - 1));
            }

            SimpleMatrix result = this.getWorldMatrix(index + 1);
            result = result.mult(localBonesMatrices.get(index));
            return new Matrix(result);
        }
    }
}