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);
}
}
public void setRotation(Quaternion r) {
mOriginalRotation.set(r);
mRotation.getQuaternion().set(r);
mWorldRotation.set(r);
mActorNode.setRotation(mRotation);
}