diff --git a/jme3-blender/build.gradle b/jme3-blender/build.gradle index 1b42c7109..a556d7a65 100644 --- a/jme3-blender/build.gradle +++ b/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') +} \ No newline at end of file diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/Constraint.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/Constraint.java index 91cfd2f3f..f2268199c 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/Constraint.java +++ b/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; diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinition.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinition.java index 346554052..a1f3a7316 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinition.java +++ b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinition.java @@ -30,6 +30,8 @@ public abstract class ConstraintDefinition { protected Set 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 diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionFactory.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionFactory.java index cbf727290..c1d69fe06 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionFactory.java +++ b/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 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); } diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java index 35a4e5618..820a283b3 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/constraints/definitions/ConstraintDefinitionIK.java +++ b/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 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 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 bestSolution = new ArrayList(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= 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 loadBones() { - List bones = new ArrayList(); - 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 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 { + private static final long serialVersionUID = -1850524345643600718L; + + private Set alteredOmas = new HashSet(); + private List originalBonesMatrices = new ArrayList(); + private List bonesMatrices = new ArrayList(); + + 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)); + } + } + } } diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DQuaternion.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DQuaternion.java index 359abf057..9739ccd4b 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DQuaternion.java +++ b/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; } + /** + * norm 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; + } + + /** + * toRotationMatrix 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; + } + /** * fromAngleAxis sets this quaternion to the values specified * by an angle and an axis of rotation. This method creates an object, so diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DTransform.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DTransform.java index ed31a4c98..28fcda0c7 100644 --- a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/DTransform.java +++ b/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
*
@@ -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 diff --git a/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java b/jme3-blender/src/main/java/com/jme3/scene/plugins/blender/math/Matrix.java new file mode 100644 index 000000000..5ea580b84 --- /dev/null +++ b/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 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;isetRotationQuaternion builds a rotation from a + * Quaternion. + * + * @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); + } +}