Quaternion3d
//package pl.polsl.pum2.pj.math;
class Quaternion3d {
private float x;
private float y;
private float z;
private float w;
private static final float QUATERNION_TRACE_ZERO_TOLERANCE = 0.1f;
private Quaternion3d() {
}
public Quaternion3d(float x, float y, float z, float w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public Quaternion3d clone() {
return new Quaternion3d(x, y, z, w);
}
public static Quaternion3d identity() {
return new Quaternion3d(0, 0, 0, 1);
}
public static void normalize(Quaternion3d quaternion) {
float magnitude = (float) Math
.sqrt(((quaternion.x * quaternion.x)
+ (quaternion.y * quaternion.y)
+ (quaternion.z * quaternion.z) + (quaternion.w * quaternion.w)));
if (magnitude != 0) {
quaternion.x /= magnitude;
quaternion.y /= magnitude;
quaternion.z /= magnitude;
quaternion.w /= magnitude;
}
}
public static Quaternion3d fromMatrix(float[] matrix) {
Quaternion3d quat = new Quaternion3d();
// Trace of diagonal
float trace = matrix[0] + matrix[5] + matrix[10];
if (trace > 0.0f) {
float s = (float) Math.sqrt(trace + 1.0f);
quat.w = s * 0.5f;
s = 0.5f / s;
quat.x = (matrix[9] - matrix[6]) * s;
quat.y = (matrix[2] - matrix[8]) * s;
quat.z = (matrix[4] - matrix[1]) * s;
return quat;
}
if (matrix[0] > matrix[5]) {
if (matrix[10] > matrix[0]) {
if (!option2(matrix, quat)) {
if (!option1(matrix, quat)) {
option3(matrix, quat);
}
}
} else {
if (!option1(matrix, quat)) {
if (!option2(matrix, quat)) {
option3(matrix, quat);
}
}
}
} else {
if (!option3(matrix, quat)) {
if (!option2(matrix, quat)) {
option1(matrix, quat);
}
}
}
return quat;
}
private static boolean option1(float[] matrix, Quaternion3d quat) {
float s = (float) Math
.sqrt(matrix[0] - (matrix[5] + matrix[10]) + 1.0f);
if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
quat.x = s * 0.5f;
s = 0.5f / s;
quat.w = (matrix[9] - matrix[6]) * s;
quat.y = (matrix[1] + matrix[4]) * s;
quat.z = (matrix[2] + matrix[8]) * s;
return true;
}
return false;
}
private static boolean option2(float[] matrix, Quaternion3d quat) {
float s = (float) Math
.sqrt(matrix[10] - (matrix[0] + matrix[5]) + 1.0f);
if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
quat.z = s * 0.5f;
s = 0.5f / s;
quat.w = (matrix[4] - matrix[1]) * s;
quat.x = (matrix[8] + matrix[2]) * s;
quat.y = (matrix[9] + matrix[6]) * s;
return true;
}
return false;
}
private static boolean option3(float[] matrix, Quaternion3d quat) {
float s = (float) Math
.sqrt(matrix[5] - (matrix[10] + matrix[0]) + 1.0f);
if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
quat.y = s * 0.5f;
s = 0.5f / s;
quat.w = (matrix[2] - matrix[8]) * s;
quat.z = (matrix[6] + matrix[9]) * s;
quat.x = (matrix[4] + matrix[1]) * s;
return true;
}
return false;
}
public static void setMatrixFromQuaternion(float[] matrix, Quaternion3d quat) {
matrix[0] = 1.0f - (2.0f * ((quat.y * quat.y) + (quat.z * quat.z)));
matrix[1] = 2.0f * ((quat.x * quat.y) - (quat.z * quat.w));
matrix[2] = 2.0f * ((quat.x * quat.z) + (quat.y * quat.w));
matrix[3] = 0.0f;
matrix[4] = 2.0f * ((quat.x * quat.y) + (quat.z * quat.w));
matrix[5] = 1.0f - (2.0f * ((quat.x * quat.x) + (quat.z * quat.z)));
matrix[6] = 2.0f * ((quat.y * quat.z) - (quat.x * quat.w));
matrix[7] = 0.0f;
matrix[8] = 2.0f * ((quat.x * quat.z) - (quat.y * quat.w));
matrix[9] = 2.0f * ((quat.y * quat.z) + (quat.x * quat.w));
matrix[10] = 1.0f - (2.0f * ((quat.x * quat.x) + (quat.y * quat.y)));
matrix[11] = 0.0f;
matrix[12] = 0.0f;
matrix[13] = 0.0f;
matrix[14] = 0.0f;
matrix[15] = 1.0f;
}
public static Quaternion3d fromAxisAndAngle(Vector3d axis, float angle) {
Quaternion3d quat = new Quaternion3d();
angle *= 0.5f;
Vector3d.normalize(axis);
float sinAngle = (float) Math.sin(angle);
quat.x = (axis.x * sinAngle);
quat.y = (axis.y * sinAngle);
quat.z = (axis.z * sinAngle);
quat.w = (float) Math.cos(angle);
return quat;
}
public static float extractAxisAndAngle(Quaternion3d quat, Vector3d axis) {
normalize(quat);
float s = (float) Math.sqrt(1.0f - (quat.w * quat.w));
if (Math.abs(s) < 0.0005f) {
s = 1.0f;
}
if (axis != null) {
axis.x = (quat.x / s);
axis.y = (quat.y / s);
axis.z = (quat.z / s);
}
return (float) (Math.acos(quat.w) * 2.0f); // return angle as float
}
public static Quaternion3d multiply(Quaternion3d quat1, Quaternion3d quat2) {
Vector3d v1 = new Vector3d(quat1.x, quat1.y, quat1.z);
Vector3d v2 = new Vector3d(quat2.x, quat2.y, quat2.z);
float angle = (quat1.w * quat2.w) - Vector3d.dotProduct(v1, v2);
Vector3d cp = Vector3d.crossProduct(v1, v2);
v1.x *= quat2.w;
v1.y *= quat2.w;
v1.z *= quat2.w;
v2.x *= quat1.w;
v2.y *= quat1.w;
v2.z *= quat1.w;
return new Quaternion3d(v1.x + v2.x + cp.x, v1.y + v2.y + cp.y, v1.z
+ v2.z + cp.z, angle);
}
public static void invert(Quaternion3d quat) {
float length = 1.0f / ((quat.x * quat.x) + (quat.y * quat.y)
+ (quat.z * quat.z) + (quat.w * quat.w));
quat.x *= -length;
quat.y *= -length;
quat.z *= -length;
quat.w *= length;
}
public static Quaternion3d fromEulerAngles(float x, float y, float z) {
Vector3d vx = new Vector3d(1.f, 0.f, 0.f);
Vector3d vy = new Vector3d(0.f, 1.f, 0.f);
Vector3d vz = new Vector3d(0.f, 0.f, 1.f);
Quaternion3d qx = fromAxisAndAngle(vx, x);
Quaternion3d qy = fromAxisAndAngle(vy, y);
Quaternion3d qz = fromAxisAndAngle(vz, z);
Quaternion3d temp = multiply(qx, qy);
return multiply(temp, qz);
}
public static float dotProduct(Quaternion3d quat1, Quaternion3d quat2) {
return quat1.x * quat2.x + quat2.y * quat2.y + quat1.z * quat2.z
+ quat1.w * quat2.w;
}
public static Quaternion3d slerp(Quaternion3d start, Quaternion3d finish,
float progress) {
float startWeight, finishWeight;
float difference = (start.x * finish.x) + (start.y * finish.y)
+ (start.z * finish.z) + (start.w * finish.w);
if (1f - Math.abs(difference) > .01f) {
float theta = (float) Math.acos(Math.abs(difference));
float oneOverSinTheta = (float) (1.f / Math.sin(theta));
startWeight = (float) (Math.sin(theta * (1.f - progress)) * oneOverSinTheta);
finishWeight = (float) (Math.sin(theta * progress) * oneOverSinTheta);
if (difference < 0f) {
startWeight = -startWeight;
}
} else {
startWeight = (1.f - progress);
finishWeight = progress;
}
Quaternion3d ret = new Quaternion3d();
ret.x = (start.x * startWeight) + (finish.x * finishWeight);
ret.y = (start.y * startWeight) + (finish.y * finishWeight);
ret.z = (start.z * startWeight) + (finish.z * finishWeight);
ret.w = (start.w * startWeight) + (finish.w * finishWeight);
normalize(ret);
return ret;
}
}
class Vector3d {
public float x;
public float y;
public float z;
private Vector3d() {
}
public Vector3d(float x, float y, float z) {
this.x = x;
this.y = y;
this.z = z;
}
public float[] toFloatArray3() {
return new float[] { x, y, z };
}
public float[] toFloatArray4() {
return new float[] { x, y, z, 1f };
}
public void updateFromArray(float[] tab) {
x = tab[0];
y = tab[1];
z = tab[2];
}
public static Vector3d empty() {
return new Vector3d();
}
public static float magnitude(Vector3d vector) {
return (float) Math.sqrt((vector.x * vector.x) + (vector.y * vector.y)
+ (vector.z * vector.z));
}
public static void normalize(Vector3d vector) {
float vecMag = magnitude(vector);
if (vecMag == 0.0f) {
vector.x = 1.0f;
vector.y = 0.0f;
vector.z = 0.0f;
return;
}
vector.x /= vecMag;
vector.y /= vecMag;
vector.z /= vecMag;
}
public static float dotProduct(Vector3d vector1, Vector3d vector2) {
return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z
* vector2.z;
}
public static Vector3d crossProduct(Vector3d vector1, Vector3d vector2) {
Vector3d ret = new Vector3d();
ret.x = (vector1.y * vector2.z) - (vector1.z * vector2.y);
ret.y = (vector1.z * vector2.x) - (vector1.x * vector2.z);
ret.z = (vector1.x * vector2.y) - (vector1.y * vector2.x);
return ret;
}
public Vector3d clone() {
return new Vector3d(x, y, z);
}
public static float distance(Vector3d a, Vector3d b) {
float dx = a.x - b.x;
float dy = a.y - b.y;
float dz = a.z - b.z;
return (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
}
}
Related examples in the same category