/*
|
* 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 javax.vecmath.Quat4f;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* Utility functions for quaternions.
|
*
|
* @author jezek2
|
*/
|
public class QuaternionUtil {
|
|
public static float getAngle(Quat4f q) {
|
float s = 2f * (float) Math.acos(q.w);
|
return s;
|
}
|
|
public static void setRotation(Quat4f q, Vector3f axis, float angle) {
|
float d = axis.length();
|
assert (d != 0f);
|
float s = (float)Math.sin(angle * 0.5f) / d;
|
q.set(axis.x * s, axis.y * s, axis.z * s, (float) Math.cos(angle * 0.5f));
|
}
|
|
// Game Programming Gems 2.10. make sure v0,v1 are normalized
|
public static Quat4f shortestArcQuat(Vector3f v0, Vector3f v1, Quat4f out) {
|
Vector3f c = Stack.alloc(Vector3f.class);
|
c.cross(v0, v1);
|
float d = v0.dot(v1);
|
|
if (d < -1.0 + BulletGlobals.FLT_EPSILON) {
|
// just pick any vector
|
out.set(0.0f, 1.0f, 0.0f, 0.0f);
|
return out;
|
}
|
|
float s = (float) Math.sqrt((1.0f + d) * 2.0f);
|
float rs = 1.0f / s;
|
|
out.set(c.x * rs, c.y * rs, c.z * rs, s * 0.5f);
|
return out;
|
}
|
|
public static void mul(Quat4f q, Vector3f w) {
|
float rx = q.w * w.x + q.y * w.z - q.z * w.y;
|
float ry = q.w * w.y + q.z * w.x - q.x * w.z;
|
float rz = q.w * w.z + q.x * w.y - q.y * w.x;
|
float rw = -q.x * w.x - q.y * w.y - q.z * w.z;
|
q.set(rx, ry, rz, rw);
|
}
|
|
public static Vector3f quatRotate(Quat4f rotation, Vector3f v, Vector3f out) {
|
Quat4f q = null; // NORMAND Stack.alloc(rotation);
|
QuaternionUtil.mul(q, v);
|
|
Quat4f tmp = Stack.alloc(Quat4f.class);
|
inverse(tmp, rotation);
|
q.mul(tmp);
|
|
out.set(q.x, q.y, q.z);
|
return out;
|
}
|
|
public static void inverse(Quat4f q) {
|
q.x = -q.x;
|
q.y = -q.y;
|
q.z = -q.z;
|
}
|
|
public static void inverse(Quat4f q, Quat4f src) {
|
q.x = -src.x;
|
q.y = -src.y;
|
q.z = -src.z;
|
q.w = src.w;
|
}
|
|
public static void setEuler(Quat4f q, float yaw, float pitch, float roll) {
|
float halfYaw = yaw * 0.5f;
|
float halfPitch = pitch * 0.5f;
|
float halfRoll = roll * 0.5f;
|
float cosYaw = (float)Math.cos(halfYaw);
|
float sinYaw = (float)Math.sin(halfYaw);
|
float cosPitch = (float)Math.cos(halfPitch);
|
float sinPitch = (float)Math.sin(halfPitch);
|
float cosRoll = (float)Math.cos(halfRoll);
|
float sinRoll = (float)Math.sin(halfRoll);
|
q.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
|
q.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
|
q.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
|
q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
|
}
|
|
}
|
