Improvement of the Inverse Kinematics Constraint.

jme3-blender/build.gradle

@@ -6,4 +6,7 @@ dependencies {
     compile project(':jme3-core')
     compile project(':jme3-desktop')
     compile project(':jme3-effects')
-}
+    compile ('org.ejml:core:0.27')
+    compile ('org.ejml:dense64:0.27')
+    compile ('org.ejml:simple:0.27')
+}

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/Constraint.java

@@ -64,7 +64,7 @@ public abstract class Constraint {
         Pointer pData = (Pointer) constraintStructure.getFieldValue("data");
         if (pData.isNotNull()) {
             Structure data = pData.fetchData().get(0);
-            constraintDefinition = ConstraintDefinitionFactory.createConstraintDefinition(data, ownerOMA, blenderContext);
+            constraintDefinition = ConstraintDefinitionFactory.createConstraintDefinition(data, name, ownerOMA, blenderContext);
             Pointer pTar = (Pointer) data.getFieldValue("tar");
             if (pTar != null && pTar.isNotNull()) {
                 targetOMA = pTar.getOldMemoryAddress();
@@ -77,7 +77,7 @@ public abstract class Constraint {
             }
         } else {
             // Null constraint has no data, so create it here
-            constraintDefinition = ConstraintDefinitionFactory.createConstraintDefinition(null, null, blenderContext);
+            constraintDefinition = ConstraintDefinitionFactory.createConstraintDefinition(null, name, null, blenderContext);
         }
         ownerSpace = Space.valueOf(((Number) constraintStructure.getFieldValue("ownspace")).byteValue());
         ipo = influenceIpo;

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinition.java

@@ -30,6 +30,8 @@ public abstract class ConstraintDefinition {
     protected Set<Long>        alteredOmas;
     /** The variable that determines if the constraint will alter the track in any way. */
     protected boolean          trackToBeChanged = true;
+    /** The name of the constraint. */
+    protected String           constraintName;
 
     /**
      * Loads a constraint definition based on the constraint definition
@@ -53,6 +55,10 @@ public abstract class ConstraintDefinition {
         constraintHelper = (ConstraintHelper) (blenderContext == null ? null : blenderContext.getHelper(ConstraintHelper.class));
         this.ownerOMA = ownerOMA;
     }
+    
+    public void setConstraintName(String constraintName) {
+        this.constraintName = constraintName;
+    }
 
     /**
      * @return determines if the definition of the constraint will change the bone in any way; in most cases

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionFactory.java

@@ -92,7 +92,7 @@ public class ConstraintDefinitionFactory {
      *             this exception is thrown when the blender file is somehow
      *             corrupted
      */
-    public static ConstraintDefinition createConstraintDefinition(Structure constraintStructure, Long ownerOMA, BlenderContext blenderContext) throws BlenderFileException {
+    public static ConstraintDefinition createConstraintDefinition(Structure constraintStructure, String constraintName, Long ownerOMA, BlenderContext blenderContext) throws BlenderFileException {
         if (constraintStructure == null) {
             return new ConstraintDefinitionNull(null, ownerOMA, blenderContext);
         }
@@ -100,7 +100,9 @@ public class ConstraintDefinitionFactory {
         Class<? extends ConstraintDefinition> constraintDefinitionClass = CONSTRAINT_CLASSES.get(constraintClassName);
         if (constraintDefinitionClass != null) {
             try {
-                return (ConstraintDefinition) constraintDefinitionClass.getDeclaredConstructors()[0].newInstance(constraintStructure, ownerOMA, blenderContext);
+                ConstraintDefinition def = (ConstraintDefinition) constraintDefinitionClass.getDeclaredConstructors()[0].newInstance(constraintStructure, ownerOMA, blenderContext);
+                def.setConstraintName(constraintName);
+                return def;
             } catch (IllegalArgumentException e) {
                 throw new BlenderFileException(e.getLocalizedMessage(), e);
             } catch (SecurityException e) {
@@ -113,9 +115,9 @@ public class ConstraintDefinitionFactory {
                 throw new BlenderFileException(e.getLocalizedMessage(), e);
             }
         } else {
-            String constraintName = UNSUPPORTED_CONSTRAINTS.get(constraintClassName);
-            if (constraintName != null) {
-                return new UnsupportedConstraintDefinition(constraintName);
+            String unsupportedConstraintClassName = UNSUPPORTED_CONSTRAINTS.get(constraintClassName);
+            if (unsupportedConstraintClassName != null) {
+                return new UnsupportedConstraintDefinition(unsupportedConstraintClassName);
             } else {
                 throw new BlenderFileException("Unknown constraint type: " + constraintClassName);
             }

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java

@@ -1,36 +1,32 @@
 package com.jme3.scene.plugins.blender.constraints.definitions;
 
 import java.util.ArrayList;
-import java.util.Collections;
 import java.util.HashSet;
 import java.util.List;
+import java.util.Set;
+
+import org.ejml.simple.SimpleMatrix;
 
 import com.jme3.animation.Bone;
 import com.jme3.math.Transform;
-import com.jme3.math.Vector3f;
 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;
 
-/**
- * The Inverse Kinematics constraint.
- * 
- * @author Wesley Shillingford (wezrule)
- * @author Marcin Roguski (Kaelthas)
- */
 public class ConstraintDefinitionIK extends ConstraintDefinition {
-    private static final float MIN_DISTANCE  = 0.0001f;
+    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;
-    /** The total length of the bone chain. Useful for optimisation of computations speed in some cases. */
-    private double              chainLength;
     /** Indicates if the tail of the bone should be used or not. */
     private boolean            useTail;
     /** The amount of iterations of the algorithm. */
@@ -51,122 +47,85 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
         }
     }
 
+    @Override
+    public Set<Long> getAlteredOmas() {
+        return bones.alteredOmas;
+    }
+
     @Override
     public void bake(Space ownerSpace, Space targetSpace, Transform targetTransform, float influence) {
         if (influence == 0 || !trackToBeChanged || targetTransform == null) {
             return;// no need to do anything
         }
+
         DQuaternion q = new DQuaternion();
         Vector3d t = new Vector3d(targetTransform.getTranslation());
-        List<BoneContext> bones = this.loadBones();
+        bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, blenderContext);
         if (bones.size() == 0) {
             return;// no need to do anything
         }
         double distanceFromTarget = Double.MAX_VALUE;
 
-        int iterations = this.iterations;
-        if (bones.size() == 1) {
-            iterations = 1;// if only one bone is in the chain then only one iteration that will properly rotate it will be needed
-        } else {
-            // if the target cannot be rached by the bones' chain then the chain will become straight and point towards the target
-            // in this case only one iteration will be needed, computed from the root to top bone
-            BoneContext rootBone = bones.get(bones.size() - 1);
-            Transform rootBoneTransform = constraintHelper.getTransform(rootBone.getArmatureObjectOMA(), rootBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
-            if (t.distance(new Vector3d(rootBoneTransform.getTranslation())) >= chainLength) {
-                Collections.reverse(bones);
-
-                for (BoneContext boneContext : bones) {
-                    Bone bone = boneContext.getBone();
-                    DTransform boneTransform = new DTransform(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD));
-
-                    Vector3d e = boneTransform.getTranslation().add(boneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(boneContext.getLength()));// effector
-                    Vector3d j = boneTransform.getTranslation(); // current join position
-
-                    Vector3d currentDir = e.subtractLocal(j).normalizeLocal();
-                    Vector3d target = t.subtract(j).normalizeLocal();
-                    double angle = currentDir.angleBetween(target);
-                    if (angle != 0) {
-                        Vector3d cross = currentDir.crossLocal(target).normalizeLocal();
-                        q.fromAngleAxis(angle, cross);
-                        
-                        if(bone.equals(this.getOwner())) {
-                            if (boneContext.isLockX()) {
-                                q.set(0, q.getY(), q.getZ(), q.getW());
-                            }
-                            if (boneContext.isLockY()) {
-                                q.set(q.getX(), 0, q.getZ(), q.getW());
-                            }
-                            if (boneContext.isLockZ()) {
-                                q.set(q.getX(), q.getY(), 0, q.getW());
-                            }
-                        }
-
-                        boneTransform.getRotation().set(q.multLocal(boneTransform.getRotation()));
-                        constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneTransform.toTransform());
-                    }
-                }
-
-                iterations = 0;
+        Vector3d target = new Vector3d(targetTransform.getTranslation());
+        Vector3d[] rotationVectors = new Vector3d[bones.size()];
+        BoneContext topBone = bones.get(0);
+        for (int i = 1; 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(t);
+            if (distanceFromTarget <= MIN_DISTANCE) {
+                break;
             }
-        }
 
-        List<Transform> bestSolution = new ArrayList<Transform>(bones.size());
-        double bestSolutionDistance = Double.MAX_VALUE;
-        BoneContext topBone = bones.get(0);
-        for (int i = 0; i < iterations && distanceFromTarget > MIN_DISTANCE; ++i) {
-            for (BoneContext boneContext : bones) {
-                Bone bone = boneContext.getBone();
-                DTransform topBoneTransform = new DTransform(constraintHelper.getTransform(topBone.getArmatureObjectOMA(), topBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD));
-                DTransform boneWorldTransform = new DTransform(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD));
+            Matrix deltaP = new Matrix(3, 1);
+            deltaP.setColumn(target.subtract(e), 0);
 
-                Vector3d e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
+            Matrix J = new Matrix(3, bones.size());
+            int column = 0;
+            for (BoneContext boneContext : bones) {
+                DTransform boneWorldTransform = bones.getWorldTransform(boneContext);
                 Vector3d j = boneWorldTransform.getTranslation(); // current join position
+                Vector3d vectorFromJointToEffector = e.subtract(j);
+                rotationVectors[column] = vectorFromJointToEffector.cross(target.subtract(j)).normalize();
+                Vector3d col = rotationVectors[column].cross(vectorFromJointToEffector);
+                J.setColumn(col, column++);
+            }
+            Matrix J_1 = J.pseudoinverse();
 
-                Vector3d currentDir = e.subtractLocal(j).normalizeLocal();
-                Vector3d target = t.subtract(j).normalizeLocal();
-                double angle = currentDir.angleBetween(target);
-                if (angle != 0) {
-                    Vector3d cross = currentDir.crossLocal(target).normalizeLocal();
-                    q.fromAngleAxis(angle, cross);
-
-                    if(bone.equals(this.getOwner())) {
-                        if (boneContext.isLockX()) {
-                            q.set(0, q.getY(), q.getZ(), q.getW());
-                        }
-                        if (boneContext.isLockY()) {
-                            q.set(q.getX(), 0, q.getZ(), q.getW());
-                        }
-                        if (boneContext.isLockZ()) {
-                            q.set(q.getX(), q.getY(), 0, q.getW());
-                        }
-                    }
+            SimpleMatrix deltaThetas = J_1.mult(deltaP);
 
-                    boneWorldTransform.getRotation().set(q.multLocal(boneWorldTransform.getRotation()));
-                    constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneWorldTransform.toTransform());
-                } else {
-                    iterations = 0;
-                    break;
-                }
-            }
+            for (int j = 0; j < deltaThetas.numRows(); ++j) {
+                double angle = deltaThetas.get(j, 0);
+                Vector3d rotationVector = rotationVectors[j];
 
-            DTransform topBoneTransform = new DTransform(constraintHelper.getTransform(topBone.getArmatureObjectOMA(), topBone.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD));
-            Vector3d e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
-            distanceFromTarget = e.distance(t);
-            
-            if(distanceFromTarget < bestSolutionDistance) {
-                bestSolutionDistance = distanceFromTarget;
-                bestSolution.clear();
-                for(BoneContext boneContext : bones) {
-                    bestSolution.add(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD));
+                q.fromAngleAxis(angle, rotationVector);
+                BoneContext boneContext = bones.get(j);
+                Bone bone = boneContext.getBone();
+                if (bone.equals(this.getOwner())) {
+                    if (boneContext.isLockX()) {
+                        q.set(0, q.getY(), q.getZ(), q.getW());
+                    }
+                    if (boneContext.isLockY()) {
+                        q.set(q.getX(), 0, q.getZ(), q.getW());
+                    }
+                    if (boneContext.isLockZ()) {
+                        q.set(q.getX(), q.getY(), 0, q.getW());
+                    }
                 }
+
+                DTransform boneTransform = bones.getWorldTransform(boneContext);
+                boneTransform.getRotation().set(q.mult(boneTransform.getRotation()));
+                bones.setWorldTransform(boneContext, boneTransform);
             }
         }
-        
-        // applying best solution
-        for(int i=0;i<bestSolution.size();++i) {
+
+        // applying the results
+        for (int i = bones.size() - 1; i >= 0; --i) {
             BoneContext boneContext = bones.get(i);
-            constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, bestSolution.get(i));
+            DTransform transform = bones.getWorldTransform(boneContext);
+            constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD, transform.toTransform());
         }
+        bones.reset();
     }
 
     @Override
@@ -174,49 +133,12 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
         return "Inverse kinematics";
     }
 
-    /**
-     * @return the bone contexts of all bones that will be used in this constraint computations
-     */
-    private List<BoneContext> loadBones() {
-        List<BoneContext> bones = new ArrayList<BoneContext>();
-        Bone bone = (Bone) this.getOwner();
-        if (bone == null) {
-            return bones;
-        }
-        if (!useTail) {
-            bone = bone.getParent();
-        }
-        chainLength = 0;
-        while (bone != null) {
-            BoneContext boneContext = blenderContext.getBoneContext(bone);
-            chainLength += boneContext.getLength();
-            bones.add(boneContext);
-            alteredOmas.add(boneContext.getBoneOma());
-            if (bonesAffected != 0 && bones.size() >= bonesAffected) {
-                break;
-            }
-            // need to add spaces between bones to the chain length
-            Transform boneWorldTransform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
-            Vector3f boneWorldTranslation = boneWorldTransform.getTranslation();
-
-            bone = bone.getParent();
-
-            if (bone != null) {
-                boneContext = blenderContext.getBoneContext(bone);
-                Transform parentWorldTransform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD);
-                Vector3f parentWorldTranslation = parentWorldTransform.getTranslation();
-                chainLength += boneWorldTranslation.distance(parentWorldTranslation);
-            }
-        }
-        return bones;
-    }
-
     @Override
     public boolean isTrackToBeChanged() {
         if (trackToBeChanged) {
             // need to check the bone structure too (when constructor was called not all of the bones might have been loaded yet)
             // that is why it is also checked here
-            List<BoneContext> bones = this.loadBones();
+            bones = new BonesChain((Bone) this.getOwner(), useTail, bonesAffected, blenderContext);
             trackToBeChanged = bones.size() > 0;
         }
         return trackToBeChanged;
@@ -226,4 +148,68 @@ public class ConstraintDefinitionIK extends ConstraintDefinition {
     public boolean isTargetRequired() {
         return true;
     }
+
+    private static class BonesChain extends ArrayList<BoneContext> {
+        private static final long serialVersionUID      = -1850524345643600718L;
+
+        private Set<Long>         alteredOmas           = new HashSet<Long>();
+        private List<Matrix>    originalBonesMatrices = new ArrayList<Matrix>();
+        private List<Matrix>    bonesMatrices         = new ArrayList<Matrix>();
+
+        public BonesChain(Bone bone, boolean useTail, int bonesAffected, BlenderContext blenderContext) {
+            if (bone != null) {
+                ConstraintHelper constraintHelper = blenderContext.getHelper(ConstraintHelper.class);
+                if (!useTail) {
+                    bone = bone.getParent();
+                }
+                while (bone != null && this.size() < bonesAffected) {
+                    BoneContext boneContext = blenderContext.getBoneContext(bone);
+                    this.add(boneContext);
+                    alteredOmas.add(boneContext.getBoneOma());
+
+                    Space space = this.size() < bonesAffected ? Space.CONSTRAINT_SPACE_LOCAL : Space.CONSTRAINT_SPACE_WORLD;
+                    Transform transform = constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), space);
+                    originalBonesMatrices.add(new DTransform(transform).toMatrix());
+
+                    bone = bone.getParent();
+                }
+                this.reset();
+            }
+        }
+
+        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);
+            }
+            bonesMatrices.set(index, boneMatrix);
+        }
+
+        public Matrix getWorldMatrix(int index) {
+            if (index == this.size() - 1) {
+                return new Matrix(bonesMatrices.get(this.size() - 1));
+            }
+
+            SimpleMatrix result = this.getWorldMatrix(index + 1);
+            result = result.mult(bonesMatrices.get(index));
+            return new Matrix(result);
+        }
+
+        public void reset() {
+            bonesMatrices.clear();
+            for (Matrix m : originalBonesMatrices) {
+                bonesMatrices.add(new Matrix(m));
+            }
+        }
+    }
 }

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DQuaternion.java

@@ -164,6 +164,134 @@ public final class DQuaternion implements Savable, Cloneable, java.io.Serializab
         w = 1;
     }
 
+    /**
+     * <code>norm</code> returns the norm of this quaternion. This is the dot
+     * product of this quaternion with itself.
+     *
+     * @return the norm of the quaternion.
+     */
+    public double norm() {
+        return w * w + x * x + y * y + z * z;
+    }
+    
+    public DQuaternion fromRotationMatrix(double m00, double m01, double m02,
+            double m10, double m11, double m12, double m20, double m21, double m22) {
+        // first normalize the forward (F), up (U) and side (S) vectors of the rotation matrix
+        // so that the scale does not affect the rotation
+        double lengthSquared = m00 * m00 + m10 * m10 + m20 * m20;
+        if (lengthSquared != 1f && lengthSquared != 0f) {
+            lengthSquared = 1.0 / Math.sqrt(lengthSquared);
+            m00 *= lengthSquared;
+            m10 *= lengthSquared;
+            m20 *= lengthSquared;
+        }
+        lengthSquared = m01 * m01 + m11 * m11 + m21 * m21;
+        if (lengthSquared != 1 && lengthSquared != 0f) {
+            lengthSquared = 1.0 / Math.sqrt(lengthSquared);
+            m01 *= lengthSquared;
+            m11 *= lengthSquared;
+            m21 *= lengthSquared;
+        }
+        lengthSquared = m02 * m02 + m12 * m12 + m22 * m22;
+        if (lengthSquared != 1f && lengthSquared != 0f) {
+            lengthSquared = 1.0 / Math.sqrt(lengthSquared);
+            m02 *= lengthSquared;
+            m12 *= lengthSquared;
+            m22 *= lengthSquared;
+        }
+
+        // Use the Graphics Gems code, from 
+        // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
+        // *NOT* the "Matrix and Quaternions FAQ", which has errors!
+
+        // the trace is the sum of the diagonal elements; see
+        // http://mathworld.wolfram.com/MatrixTrace.html
+        double t = m00 + m11 + m22;
+
+        // we protect the division by s by ensuring that s>=1
+        if (t >= 0) { // |w| >= .5
+            double s = Math.sqrt(t + 1); // |s|>=1 ...
+            w = 0.5f * s;
+            s = 0.5f / s;                 // so this division isn't bad
+            x = (m21 - m12) * s;
+            y = (m02 - m20) * s;
+            z = (m10 - m01) * s;
+        } else if (m00 > m11 && m00 > m22) {
+            double s = Math.sqrt(1.0 + m00 - m11 - m22); // |s|>=1
+            x = s * 0.5f; // |x| >= .5
+            s = 0.5f / s;
+            y = (m10 + m01) * s;
+            z = (m02 + m20) * s;
+            w = (m21 - m12) * s;
+        } else if (m11 > m22) {
+            double s = Math.sqrt(1.0 + m11 - m00 - m22); // |s|>=1
+            y = s * 0.5f; // |y| >= .5
+            s = 0.5f / s;
+            x = (m10 + m01) * s;
+            z = (m21 + m12) * s;
+            w = (m02 - m20) * s;
+        } else {
+            double s = Math.sqrt(1.0 + m22 - m00 - m11); // |s|>=1
+            z = s * 0.5f; // |z| >= .5
+            s = 0.5f / s;
+            x = (m02 + m20) * s;
+            y = (m21 + m12) * s;
+            w = (m10 - m01) * s;
+        }
+
+        return this;
+    }
+    
+    /**
+     * <code>toRotationMatrix</code> converts this quaternion to a rotational
+     * matrix. The result is stored in result. 4th row and 4th column values are
+     * untouched. Note: the result is created from a normalized version of this quat.
+     * 
+     * @param result
+     *            The Matrix4f to store the result in.
+     * @return the rotation matrix representation of this quaternion.
+     */
+    public Matrix toRotationMatrix(Matrix result) {
+        Vector3d originalScale = new Vector3d();
+        
+        result.toScaleVector(originalScale);
+        result.setScale(1, 1, 1);
+        double norm = this.norm();
+        // we explicitly test norm against one here, saving a division
+        // at the cost of a test and branch.  Is it worth it?
+        double s = norm == 1f ? 2f : norm > 0f ? 2f / norm : 0;
+
+        // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
+        // will be used 2-4 times each.
+        double xs = x * s;
+        double ys = y * s;
+        double zs = z * s;
+        double xx = x * xs;
+        double xy = x * ys;
+        double xz = x * zs;
+        double xw = w * xs;
+        double yy = y * ys;
+        double yz = y * zs;
+        double yw = w * ys;
+        double zz = z * zs;
+        double zw = w * zs;
+
+        // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
+        result.set(0, 0, 1 - (yy + zz));
+        result.set(0, 1, xy - zw);
+        result.set(0, 2, xz + yw);
+        result.set(1, 0, xy + zw);
+        result.set(1, 1, 1 - (xx + zz));
+        result.set(1, 2, yz - xw);
+        result.set(2, 0, xz - yw);
+        result.set(2, 1, yz + xw);
+        result.set(2, 2, 1 - (xx + yy));
+        
+        result.setScale(originalScale);
+        
+        return result;
+    }
+    
     /**
      * <code>fromAngleAxis</code> sets this quaternion to the values specified
      * by an angle and an axis of rotation. This method creates an object, so

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DTransform.java

@@ -31,11 +31,15 @@
  */
 package com.jme3.scene.plugins.blender.math;
 
-import com.jme3.export.*;
-import com.jme3.math.Transform;
-
 import java.io.IOException;
 
+import com.jme3.export.InputCapsule;
+import com.jme3.export.JmeExporter;
+import com.jme3.export.JmeImporter;
+import com.jme3.export.OutputCapsule;
+import com.jme3.export.Savable;
+import com.jme3.math.Transform;
+
 /**
  * Started Date: Jul 16, 2004<br>
  * <br>
@@ -57,6 +61,12 @@ public final class DTransform implements Savable, Cloneable, java.io.Serializabl
     private Vector3d          translation;
     private Vector3d          scale;
 
+    public DTransform() {
+        translation = new Vector3d();
+        rotation = new DQuaternion();
+        scale = new Vector3d();
+    }
+    
     public DTransform(Transform transform) {
         translation = new Vector3d(transform.getTranslation());
         rotation = new DQuaternion(transform.getRotation());
@@ -66,7 +76,15 @@ public final class DTransform implements Savable, Cloneable, java.io.Serializabl
     public Transform toTransform() {
         return new Transform(translation.toVector3f(), rotation.toQuaternion(), scale.toVector3f());
     }
-
+    
+    public Matrix toMatrix() {
+        Matrix m = Matrix.identity(4);
+        m.setTranslation(translation);
+        m.setRotationQuaternion(rotation);
+        m.setScale(scale);
+        return m;
+    }
+    
     /**
      * Sets this translation to the given value.
      * @param trans

jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java

@@ -0,0 +1,214 @@
+package com.jme3.scene.plugins.blender.math;
+
+import java.text.DecimalFormat;
+
+import org.ejml.ops.CommonOps;
+import org.ejml.simple.SimpleMatrix;
+import org.ejml.simple.SimpleSVD;
+
+import com.jme3.math.FastMath;
+
+/**
+ * Encapsulates a 4x4 matrix
+ *
+ *
+ */
+public class Matrix extends SimpleMatrix {
+    private static final long serialVersionUID = 2396600537315902559L;
+
+    public Matrix(int rows, int cols) {
+        super(rows, cols);
+    }
+
+    /**
+     * Copy constructor
+     */
+    public Matrix(SimpleMatrix m) {
+        super(m);
+    }
+    
+    public Matrix(double[][] data) {
+        super(data);
+    }
+    
+    public static Matrix identity(int size) {
+        Matrix result = new Matrix(size, size);
+        CommonOps.setIdentity(result.mat);
+        return result;
+    }
+    
+    public Matrix pseudoinverse() {
+        return this.pseudoinverse(1);
+    }
+    
+    @SuppressWarnings("unchecked")
+    public Matrix pseudoinverse(double lambda) {
+        SimpleSVD<SimpleMatrix> simpleSVD = this.svd();
+        
+        SimpleMatrix U = simpleSVD.getU();
+        SimpleMatrix S = simpleSVD.getW();
+        SimpleMatrix V = simpleSVD.getV();
+        
+        int N = Math.min(this.numRows(),this.numCols());
+        double maxSingular = 0;
+        for( int i = 0; i < N; i++ ) {
+            if( S.get(i, i) > maxSingular ) {
+                maxSingular = S.get(i, i);
+            }
+        }
+        
+        double tolerance = FastMath.DBL_EPSILON * Math.max(this.numRows(),this.numCols()) * maxSingular;
+        for(int i=0;i<Math.min(S.numRows(), S.numCols());++i) {
+            double a = S.get(i, i);
+            if(a <= tolerance) {
+                a = 0;
+            } else {
+                a = a/(a * a + lambda * lambda);
+            }
+            S.set(i, i, a);
+        }
+        return new Matrix(V.mult(S.transpose()).mult(U.transpose()));
+    }
+    
+    public void setColumn(Vector3d col, int column) {
+        this.setColumn(column, 0, col.x, col.y, col.z);
+    }
+    
+    /**
+     * Just for some debug informations in order to compare the results with the scilab computation program.
+     * @param name the name of the matrix
+     * @param m the matrix to print out
+     * @return the String format of the matrix to easily input it to Scilab
+     */
+    public String toScilabString(String name, SimpleMatrix m) {
+        String result = name + " = [";
+        
+        for(int i=0;i<m.numRows();++i) {
+            for(int j=0;j<m.numCols();++j) {
+                result += m.get(i, j) + " ";
+            }
+            result += ";";
+        }
+        
+        return result;
+    }
+    
+    /**
+     * @return a String representation of the matrix
+     */
+    @Override
+    public String toString() {
+        DecimalFormat df = new DecimalFormat("#.0000");
+        StringBuilder buf = new StringBuilder();
+        for (int r = 0; r < this.numRows(); ++r) {
+            buf.append("\n| ");
+            for (int c = 0; c < this.numCols(); ++c) {
+                buf.append(df.format(this.get(r, c))).append(' ');
+            }
+            buf.append('|');
+        }
+        return buf.toString();
+    }
+    
+    public void setTranslation(Vector3d translation) {
+        this.setColumn(translation, 3);
+    }
+    
+    /**
+     * Sets the scale.
+     * 
+     * @param scale
+     *            the scale vector to set
+     */
+    public void setScale(Vector3d scale) {
+        this.setScale(scale.x, scale.y, scale.z);
+    }
+    
+    /**
+     * Sets the scale.
+     * 
+     * @param x
+     *            the X scale
+     * @param y
+     *            the Y scale
+     * @param z
+     *            the Z scale
+     */
+    public void setScale(double x, double y, double z) {
+        Vector3d vect1 = new Vector3d(this.get(0, 0), this.get(1, 0), this.get(2, 0));
+        vect1.normalizeLocal().multLocal(x);
+        this.set(0, 0, vect1.x);
+        this.set(1, 0, vect1.y);
+        this.set(2, 0, vect1.z);
+
+        vect1.set(this.get(0, 1), this.get(1, 1), this.get(2, 1));
+        vect1.normalizeLocal().multLocal(y);
+        this.set(0, 1, vect1.x);
+        this.set(1, 1, vect1.y);
+        this.set(2, 1, vect1.z);
+
+        vect1.set(this.get(0, 2), this.get(1, 2), this.get(2, 2));
+        vect1.normalizeLocal().multLocal(z);
+        this.set(0, 2, vect1.x);
+        this.set(1, 2, vect1.y);
+        this.set(2, 2, vect1.z);
+    }
+    
+    /**
+     * <code>setRotationQuaternion</code> builds a rotation from a
+     * <code>Quaternion</code>.
+     * 
+     * @param quat
+     *            the quaternion to build the rotation from.
+     * @throws NullPointerException
+     *             if quat is null.
+     */
+    public void setRotationQuaternion(DQuaternion quat) {
+        quat.toRotationMatrix(this);
+    }
+    
+    public DTransform toTransform() {
+        DTransform result = new DTransform();
+        result.setTranslation(this.toTranslationVector());
+        result.setRotation(this.toRotationQuat());
+        result.setScale(this.toScaleVector());
+        return result;
+    }
+    
+    public Vector3d toTranslationVector() {
+        return new Vector3d(this.get(0, 3), this.get(1, 3), this.get(2, 3));
+    }
+    
+    public DQuaternion toRotationQuat() {
+        DQuaternion quat = new DQuaternion();
+        quat.fromRotationMatrix(this.get(0, 0), this.get(0, 1), this.get(0, 2), this.get(1, 0), this.get(1, 1), this.get(1, 2), this.get(2, 0), this.get(2, 1), this.get(2, 2));
+        return quat;
+    }
+    
+    /**
+     * Retreives the scale vector from the matrix and stores it into a given
+     * vector.
+     * 
+     * @param the
+     *            vector where the scale will be stored
+     */
+    public Vector3d toScaleVector() {
+        Vector3d result = new Vector3d();
+        this.toScaleVector(result);
+        return result;
+    }
+    
+    /**
+     * Retreives the scale vector from the matrix and stores it into a given
+     * vector.
+     * 
+     * @param the
+     *            vector where the scale will be stored
+     */
+    public void toScaleVector(Vector3d vector) {
+        double scaleX = Math.sqrt(this.get(0, 0) * this.get(0, 0) + this.get(1, 0) * this.get(1, 0) + this.get(2, 0) * this.get(2, 0));
+        double scaleY = Math.sqrt(this.get(0, 1) * this.get(0, 1) + this.get(1, 1) * this.get(1, 1) + this.get(2, 1) * this.get(2, 1));
+        double scaleZ = Math.sqrt(this.get(0, 2) * this.get(0, 2) + this.get(1, 2) * this.get(1, 2) + this.get(2, 2) * this.get(2, 2));
+        vector.set(scaleX, scaleY, scaleZ);
+    }
+}