<commit_before>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 <NAME> (Kaelthas)
 */
public class ConstraintDefinitionIK extends ConstraintDefinition {
    private static final float MIN_DISTANCE  = 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<Long>();
        }
    }

    /**
     * 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);

            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 <NAME> (Kaelthas)
     */
    private static class BonesChain extends ArrayList<BoneContext> {
        private static final long serialVersionUID = -1850524345643600718L;

        private List<Matrix>      bonesMatrices    = new ArrayList<Matrix>();

        public BonesChain(Bone bone, boolean useTail, int bonesAffected, Collection<Long> alteredOmas, BlenderContext blenderContext) {
            if (bone != null) {
                ConstraintHelper constraintHelper = blenderContext.getHelper(ConstraintHelper.class);
                if (!useTail) {
                    bone = bone.getParent();<commit_msg>Improvement: Inverse Kinematics now breaks the iteration if the computed angle change drops below some minimal level.<commit_after>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 <NAME> (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<Long>();
        }
    }

    /**
     * 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 <NAME> (Kaelthas)
     */
    private static class BonesChain extends ArrayList<BoneContext> {
        private static final long serialVersionUID = -1850524345643600718L;

        private List<Matrix> bonesMatrices = new ArrayList<Matrix>();

        public BonesChain(Bone bone, boolean useTail, int bonesAffected, Collection<Long> alteredOmas, BlenderContext blenderContext) {
            if (bone != null) {
                ConstraintHelper constraintHelper = blenderContext.getHelper(ConstraintHelper.class);
                if (!useTail) {
                    bone = bone.getParent();
{'fraction_non_alphanumeric': 0.09963054187192118, 'fraction_numerical': 0.010098522167487686, 'mean_word_length': 2.4098257400797816, 'pattern_counts': {'":': 0, '<': 28, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3878887', 'n_tokens_mistral': 4662, 'n_tokens_neox': 4385, 'n_words': 1103}