Improvement of the Inverse Kinematics Constraint.
This commit is contained in:
jmekaelthas
parent
e2d8fe8293
commit
f7a4fe9f25
@ -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')
|
||||
}
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
@ -54,6 +56,10 @@ public abstract class ConstraintDefinition {
|
||||
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
|
||||
* it is possible to tell that even before the constraint baking simulation is started, so we can discard such bones from constraint
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
List<Transform> bestSolution = new ArrayList<Transform>(bones.size());
|
||||
double bestSolutionDistance = Double.MAX_VALUE;
|
||||
Vector3d target = new Vector3d(targetTransform.getTranslation());
|
||||
Vector3d[] rotationVectors = new Vector3d[bones.size()];
|
||||
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));
|
||||
|
||||
Vector3d e = topBoneTransform.getTranslation().addLocal(topBoneTransform.getRotation().mult(Vector3d.UNIT_Y).multLocal(topBone.getLength()));// effector
|
||||
Vector3d j = boneWorldTransform.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());
|
||||
}
|
||||
}
|
||||
|
||||
boneWorldTransform.getRotation().set(q.multLocal(boneWorldTransform.getRotation()));
|
||||
constraintHelper.applyTransform(boneContext.getArmatureObjectOMA(), bone.getName(), Space.CONSTRAINT_SPACE_WORLD, boneWorldTransform.toTransform());
|
||||
} else {
|
||||
iterations = 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;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
Matrix deltaP = new Matrix(3, 1);
|
||||
deltaP.setColumn(target.subtract(e), 0);
|
||||
|
||||
if(distanceFromTarget < bestSolutionDistance) {
|
||||
bestSolutionDistance = distanceFromTarget;
|
||||
bestSolution.clear();
|
||||
for(BoneContext boneContext : bones) {
|
||||
bestSolution.add(constraintHelper.getTransform(boneContext.getArmatureObjectOMA(), boneContext.getBone().getName(), Space.CONSTRAINT_SPACE_WORLD));
|
||||
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();
|
||||
|
||||
SimpleMatrix deltaThetas = J_1.mult(deltaP);
|
||||
|
||||
for (int j = 0; j < deltaThetas.numRows(); ++j) {
|
||||
double angle = deltaThetas.get(j, 0);
|
||||
Vector3d rotationVector = rotationVectors[j];
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
// applying best solution
|
||||
for(int i=0;i<bestSolution.size();++i) {
|
||||
DTransform boneTransform = bones.getWorldTransform(boneContext);
|
||||
boneTransform.getRotation().set(q.mult(boneTransform.getRotation()));
|
||||
bones.setWorldTransform(boneContext, boneTransform);
|
||||
}
|
||||
}
|
||||
|
||||
// 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));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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());
|
||||
@ -67,6 +77,14 @@ public final class DTransform implements Savable, Cloneable, java.io.Serializabl
|
||||
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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user