public void updateSpringPosition() {
if (mPrev != null && mSimulate) {
mWorldPosition.set(mPrev.mWorldEndPosition);
mWorldRotation.set(mRotation.getQuaternion());
mWorldRotation.multiply(mPrev.mWorldRotation);
MathUtil.setVectorToSumOf(
mWorldEndPosition,
mPrev.mWorldEndPosition,
MathUtil.setVectorToProductOf(mTempVec1, mWorldRotation, mDirection));
mWorldSpringRotation.set(mOriginalRotation);
mWorldSpringRotation.multiply(mPrev.mWorldRotation);
/* Method using only existing ngin3d (requires allocations/garbage)
* mWorldSpringPosition.set(mPrev.mWorldEndPosition);
* mWorldSpringPosition = mWorldSpringPosition.add(mWorldSpringRotation.applyTo(mDirection));
* */
// Method using new functions.
MathUtil.setVectorToSumOf(
mWorldSpringPosition,
mPrev.mWorldEndPosition,
MathUtil.setVectorToProductOf(mTempVec1, mWorldSpringRotation, mDirection));
}
}
public void updateNodeRotation() {
if (mPrev != null && mSimulate) {
Quaternion qRot = mRotation.getQuaternion();
mTempQuat.set(
mPrev.mWorldRotation.getQ0(),
-mPrev.mWorldRotation.getQ1(),
-mPrev.mWorldRotation.getQ2(),
-mPrev.mWorldRotation.getQ3());
qRot.set(mWorldRotation);
qRot.multiply(mTempQuat);
qRot.nor();
/* After pointing, the rotation will have an arbitrary "roll" around
* the "to" z-axis. We must find the new position of the "up" vector
* after rotation.
*/
// Vec3 dir = qRot.applyTo(BONE_DIR);
// Vec3 rolledUp = qRot.applyTo(BONE_ORTH);
MathUtil.setVectorToProductOf(mTempVec1, qRot, BONE_DIR);
MathUtil.setVectorToProductOf(mTempVec2, qRot, BONE_ORTH);
/* We now project this vector and the original "up" vector onto a plane
* perpendicular to the "to" vector so that we can find the extra
* rotation needed to rotate the rolled "up" vector onto the given
* "up" vector. Note that these vectors won't have unit length.
*/
// Vec3 upProjected = mOrthogonalTarget.subtract(Vec3.dotProduct(mTempVec1,
// mOrthogonalTarget), mTempVec1);
float dot = Vec3.dotProduct(mTempVec1, mOrthogonalTarget);
mTempVec3.set(dot * mTempVec1.x, dot * mTempVec1.y, dot * mTempVec1.z);
MathUtil.setVectorToDifferenceOf(mTempVec4, mOrthogonalTarget, mTempVec3);
/* Calculate the rotation bring rolledUpProjected onto upProjected.
* Note that this rotation will be around the "to" vector (because both
* vectors are parallel to the "to" vector after projection).
*/
// Rotation rollRotation = Rotation.fromTo(mTempVec2, upProjected);
// qRot.multiply(rollRotation.getQuaternion());
MathUtil.setQuaternionFromTo(mTempQuat, mTempVec2, mTempVec4);
qRot.multiply(mTempQuat);
mTempRot.getQuaternion().set(qRot);
mTempRot.getQuaternion().multiply(mPhysics.mFold);
mActorNode.setRotation(mTempRot);
}
}
/*
* Index represents the index of the joint (zero is the bottom joint)
*/
public Joint(
ActorNode actorNode,
Joint prev,
float x,
float y,
float z,
float a,
float b,
float c,
float d,
JointPhysics physics,
GlobalPhysics globalPhysics) {
mActorNode = actorNode;
mPhysics = physics;
mGlobalPhysics = globalPhysics;
y = 0;
z = 0;
mPrev = prev;
mOriginalRotation.set(a, b, c, d);
mRotation.set(a, b, c, d, false);
mWorldRotation.set(a, b, c, d); // Only correct for root (others will be set on first update)
// mDirection.set(.075f, 0f, 0f);
mDirection.set(physics.mSpringLength, 0f, 0f);
// mDirection.set(.05f + (float)Math.random() * 0.1f, 0f, 0f);
mOrthogonalTarget = mOriginalRotation.applyTo(BONE_ORTH);
if (prev != null) {
prev.mSimulate = true;
mPrev.mWorldEndPosition.set(x, y, z);
Vec3 dir = new Vec3(x, y, z);
if (dir.getLength() > 0.001f) {
// mPrev.mDirectionVector.set(x, y, z);
mPrev.mOrthogonalTarget = mPrev.mOriginalRotation.applyTo(BONE_ORTH);
}
}
while (prev != null) {
prev.mRootiness += 1f;
prev = prev.mPrev;
}
}
public void setRotation(Quaternion r) {
mOriginalRotation.set(r);
mRotation.getQuaternion().set(r);
mWorldRotation.set(r);
mActorNode.setRotation(mRotation);
}