/*
|
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
|
*
|
* Bullet Continuous Collision Detection and Physics Library
|
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.com/
|
*
|
* This software is provided 'as-is', without any express or implied warranty.
|
* In no event will the authors be held liable for any damages arising from
|
* the use of this software.
|
*
|
* Permission is granted to anyone to use this software for any purpose,
|
* including commercial applications, and to alter it and redistribute it
|
* freely, subject to the following restrictions:
|
*
|
* 1. The origin of this software must not be misrepresented; you must not
|
* claim that you wrote the original software. If you use this software
|
* in a product, an acknowledgment in the product documentation would be
|
* appreciated but is not required.
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
* misrepresented as being the original software.
|
* 3. This notice may not be removed or altered from any source distribution.
|
*/
|
|
package com.bulletphysics.linearmath;
|
|
import com.bulletphysics.BulletGlobals;
|
import cz.advel.stack.Stack;
|
import cz.advel.stack.StaticAlloc;
|
import javax.vecmath.Matrix3f;
|
import javax.vecmath.Quat4f;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* Utility functions for transforms.
|
*
|
* @author jezek2
|
*/
|
public class TransformUtil {
|
|
public static final float SIMDSQRT12 = 0.7071067811865475244008443621048490f;
|
public static final float ANGULAR_MOTION_THRESHOLD = 0.5f*BulletGlobals.SIMD_HALF_PI;
|
|
public static float recipSqrt(float x) {
|
return 1f / (float)Math.sqrt(x); /* reciprocal square root */
|
}
|
|
public static void planeSpace1(Vector3f n, Vector3f p, Vector3f q) {
|
if (Math.abs(n.z) > SIMDSQRT12) {
|
// choose p in y-z plane
|
float a = n.y * n.y + n.z * n.z;
|
float k = recipSqrt(a);
|
p.set(0, -n.z * k, n.y * k);
|
// set q = n x p
|
q.set(a * k, -n.x * p.z, n.x * p.y);
|
}
|
else {
|
// choose p in x-y plane
|
float a = n.x * n.x + n.y * n.y;
|
float k = recipSqrt(a);
|
p.set(-n.y * k, n.x * k, 0);
|
// set q = n x p
|
q.set(-n.z * p.y, n.z * p.x, a * k);
|
}
|
}
|
|
@StaticAlloc
|
public static void integrateTransform(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Transform predictedTransform) {
|
predictedTransform.origin.scaleAdd(timeStep, linvel, curTrans.origin);
|
// //#define QUATERNION_DERIVATIVE
|
// #ifdef QUATERNION_DERIVATIVE
|
// btQuaternion predictedOrn = curTrans.getRotation();
|
// predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
|
// predictedOrn.normalize();
|
// #else
|
// Exponential map
|
// google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
|
|
Vector3f axis = Stack.alloc(Vector3f.class);
|
float fAngle = angvel.length();
|
|
// limit the angular motion
|
if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) {
|
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
|
}
|
|
if (fAngle < 0.001f) {
|
// use Taylor's expansions of sync function
|
axis.scale(0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle, angvel);
|
}
|
else {
|
// sync(fAngle) = sin(c*fAngle)/t
|
axis.scale((float) Math.sin(0.5f * fAngle * timeStep) / fAngle, angvel);
|
}
|
Quat4f dorn = Stack.alloc(Quat4f.class);
|
dorn.set(axis.x, axis.y, axis.z, (float) Math.cos(fAngle * timeStep * 0.5f));
|
Quat4f orn0 = curTrans.getRotation(Stack.alloc(Quat4f.class));
|
|
Quat4f predictedOrn = Stack.alloc(Quat4f.class);
|
predictedOrn.mul(dorn, orn0);
|
predictedOrn.normalize();
|
// #endif
|
predictedTransform.setRotation(predictedOrn);
|
}
|
|
public static void calculateVelocity(Transform transform0, Transform transform1, float timeStep, Vector3f linVel, Vector3f angVel) {
|
linVel.sub(transform1.origin, transform0.origin);
|
linVel.scale(1f / timeStep);
|
|
Vector3f axis = Stack.alloc(Vector3f.class);
|
float[] angle = new float[1];
|
calculateDiffAxisAngle(transform0, transform1, axis, angle);
|
angVel.scale(angle[0] / timeStep, axis);
|
}
|
|
public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, Vector3f axis, float[] angle) {
|
// #ifdef USE_QUATERNION_DIFF
|
// btQuaternion orn0 = transform0.getRotation();
|
// btQuaternion orn1a = transform1.getRotation();
|
// btQuaternion orn1 = orn0.farthest(orn1a);
|
// btQuaternion dorn = orn1 * orn0.inverse();
|
// #else
|
Matrix3f tmp = Stack.alloc(Matrix3f.class);
|
tmp.set(transform0.basis);
|
MatrixUtil.invert(tmp);
|
|
Matrix3f dmat = Stack.alloc(Matrix3f.class);
|
dmat.mul(transform1.basis, tmp);
|
|
Quat4f dorn = Stack.alloc(Quat4f.class);
|
MatrixUtil.getRotation(dmat, dorn);
|
// #endif
|
|
// floating point inaccuracy can lead to w component > 1..., which breaks
|
|
dorn.normalize();
|
|
angle[0] = QuaternionUtil.getAngle(dorn);
|
axis.set(dorn.x, dorn.y, dorn.z);
|
// TODO: probably not needed
|
//axis[3] = btScalar(0.);
|
|
// check for axis length
|
float len = axis.lengthSquared();
|
if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
|
axis.set(1f, 0f, 0f);
|
}
|
else {
|
axis.scale(1f / (float) Math.sqrt(len));
|
}
|
}
|
|
}
|
