package com.asha.vrlib.model;
/*
* Copyright (C) 2015 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
import com.badlogic.gdx.math.MathUtils;
/**
* Generic Quaternion
* Written for maximum portability between desktop and Android
* Not in performance critical sections
*
* copy from package com.example.android.rs.vr.engine.Quaternion;
*/
public class MDQuaternion {
private final float[] q = new float[4]; // w,x,y,z,
private void set(float w, float x, float y, float z) {
this.q[0] = w;
this.q[1] = x;
this.q[2] = y;
this.q[3] = z;
}
private void set(float[] v1, float[] v2) {
float[] vec1 = normal(v1);
float[] vec2 = normal(v2);
float[] axis = normal(cross(vec1, vec2));
float angle = (float) Math.acos(dot(vec1, vec2));
set(angle, axis);
}
private void set(float angle, float[] axis) {
q[0] = (float) Math.cos(angle / 2);
float sin = (float) Math.sin(angle / 2);
q[1] = axis[0] * sin;
q[2] = axis[1] * sin;
q[3] = axis[2] * sin;
}
public void clone(MDQuaternion src) {
System.arraycopy(src.q, 0, q, 0, q.length);
}
public void idt(){
set(1, 0, 0, 0);
}
private static float[] cross(float[] a, float[] b) {
float out0 = a[1] * b[2] - b[1] * a[2];
float out1 = a[2] * b[0] - b[2] * a[0];
float out2 = a[0] * b[1] - b[0] * a[1];
return new float[]{out0, out1, out2};
}
private static float dot(float[] a, float[] b) {
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
private static float[] normal(float[] a) {
float norm = (float) Math.sqrt(dot(a, a));
return new float[]{a[0] / norm, a[1] / norm, a[2] / norm};
}
public static float calcAngle(float[] v1, float[] v2) {
float[] vec1 = normal(v1);
float[] vec2 = normal(v2);
return (float) Math.acos(Math.min(dot(vec1, vec2), 1));
}
public static float[] calcAxis(float[] v1, float[] v2) {
float[] vec1 = normal(v1);
float[] vec2 = normal(v2);
return normal(cross(vec1, vec2));
}
public MDQuaternion(float x0, float x1, float x2, float x3) {
q[0] = x0;
q[1] = x1;
q[2] = x2;
q[3] = x3;
}
public MDQuaternion() {
idt();
}
public MDQuaternion conjugate() {
return new MDQuaternion(q[0], -q[1], -q[2], -q[3]);
}
public MDQuaternion plus(MDQuaternion b) {
MDQuaternion a = this;
return new MDQuaternion(a.q[0] + b.q[0], a.q[1] + b.q[1], a.q[2] + b.q[2], a.q[3] + b.q[3]);
}
public MDQuaternion times(MDQuaternion b) {
MDQuaternion a = this;
float y0 = a.q[0] * b.q[0] - a.q[1] * b.q[1] - a.q[2] * b.q[2] - a.q[3] * b.q[3];
float y1 = a.q[0] * b.q[1] + a.q[1] * b.q[0] + a.q[2] * b.q[3] - a.q[3] * b.q[2];
float y2 = a.q[0] * b.q[2] - a.q[1] * b.q[3] + a.q[2] * b.q[0] + a.q[3] * b.q[1];
float y3 = a.q[0] * b.q[3] + a.q[1] * b.q[2] - a.q[2] * b.q[1] + a.q[3] * b.q[0];
return new MDQuaternion(y0, y1, y2, y3);
}
public MDQuaternion inverse() {
float d = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
return new MDQuaternion(q[0] / d, -q[1] / d, -q[2] / d, -q[3] / d);
}
public MDQuaternion divides(MDQuaternion b) {
MDQuaternion a = this;
return a.inverse().times(b);
}
public float[] rotateVec(float[] v) {
float v0 = v[0];
float v1 = v[1];
float v2 = v[2];
float s = q[1] * v0 + q[2] * v1 + q[3] * v2;
float n0 = 2 * (q[0] * (v0 * q[0] - (q[2] * v2 - q[3] * v1)) + s * q[1]) - v0;
float n1 = 2 * (q[0] * (v1 * q[0] - (q[3] * v0 - q[1] * v2)) + s * q[2]) - v1;
float n2 = 2 * (q[0] * (v2 * q[0] - (q[1] * v1 - q[2] * v0)) + s * q[3]) - v2;
return new float[]{n0, n1, n2};
}
public void toMatrix(float[] m) {
float xx = q[1] * q[1];
float xy = q[1] * q[2];
float xz = q[1] * q[3];
float xw = q[1] * q[0];
float yy = q[2] * q[2];
float yz = q[2] * q[3];
float yw = q[2] * q[0];
float zz = q[3] * q[3];
float zw = q[3] * q[0];
m[0] = 1 - 2 * (yy + zz);
m[1] = 2 * (xy - zw);
m[2] = 2 * (xz + yw);
m[4] = 2 * (xy + zw);
m[5] = 1 - 2 * (xx + zz);
m[6] = 2 * (yz - xw);
m[8] = 2 * (xz - yw);
m[9] = 2 * (yz + xw);
m[10] = 1 - 2 * (xx + yy);
m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0;
m[15] = 1;
}
public void fromMatrix(float[] matrix) {
setFromAxes(false, matrix[0], matrix[1], matrix[2],
matrix[4], matrix[5], matrix[6],
matrix[8], matrix[9], matrix[10]);
}
/** Sets the quaternion components from the given axis and angle around that axis.
* @param x X direction of the axis
* @param y Y direction of the axis
* @param z Z direction of the axis
* @param degrees The angle in degrees
* @return This quaternion for chaining. */
public void setFromAxis (final float x, final float y, final float z, final float degrees) {
setFromAxisRad(x, y, z, degrees * MathUtils.degreesToRadians);
}
/** Sets the quaternion components from the given axis and angle around that axis.
* @param x X direction of the axis
* @param y Y direction of the axis
* @param z Z direction of the axis
* @param radians The angle in radians
* @return This quaternion for chaining. */
public void setFromAxisRad (final float x, final float y, final float z, final float radians) {
float d = MDVector3D.len(x, y, z);
if (d == 0f){
idt();
return;
}
d = 1f / d;
float l_ang = radians < 0 ? MathUtils.PI2 - (-radians % MathUtils.PI2) : radians % MathUtils.PI2;
float l_sin = (float)Math.sin(l_ang / 2);
float l_cos = (float)Math.cos(l_ang / 2);
this.set(l_cos, d * x * l_sin, d * y * l_sin, d * z * l_sin);
this.nor();
}
/** <p>
* Sets the Quaternion from the given x-, y- and z-axis.
* </p>
*
* <p>
* Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ which in turn took it from Graphics Gem code at
* ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z.
* </p>
*
* @param normalizeAxes whether to normalize the axes (necessary when they contain scaling)
* @param xx x-axis x-coordinate
* @param xy x-axis y-coordinate
* @param xz x-axis z-coordinate
* @param yx y-axis x-coordinate
* @param yy y-axis y-coordinate
* @param yz y-axis z-coordinate
* @param zx z-axis x-coordinate
* @param zy z-axis y-coordinate
* @param zz z-axis z-coordinate */
private void setFromAxes (boolean normalizeAxes, float xx, float xy, float xz, float yx, float yy, float yz, float zx,
float zy, float zz) {
float w,x,y,z;
if (normalizeAxes) {
final float lx = 1f / MDVector3D.len(xx, xy, xz);
final float ly = 1f / MDVector3D.len(yx, yy, yz);
final float lz = 1f / MDVector3D.len(zx, zy, zz);
xx *= lx;
xy *= lx;
xz *= lx;
yx *= ly;
yy *= ly;
yz *= ly;
zx *= lz;
zy *= lz;
zz *= lz;
}
// the trace is the sum of the diagonal elements; see
// http://mathworld.wolfram.com/MatrixTrace.html
final float t = xx + yy + zz;
// we protect the division by s by ensuring that s>=1
if (t >= 0) { // |w| >= .5
float s = (float)Math.sqrt(t + 1); // |s|>=1 ...
w = 0.5f * s;
s = 0.5f / s; // so this division isn't bad
x = (zy - yz) * s;
y = (xz - zx) * s;
z = (yx - xy) * s;
} else if ((xx > yy) && (xx > zz)) {
float s = (float)Math.sqrt(1.0 + xx - yy - zz); // |s|>=1
x = s * 0.5f; // |x| >= .5
s = 0.5f / s;
y = (yx + xy) * s;
z = (xz + zx) * s;
w = (zy - yz) * s;
} else if (yy > zz) {
float s = (float)Math.sqrt(1.0 + yy - xx - zz); // |s|>=1
y = s * 0.5f; // |y| >= .5
s = 0.5f / s;
x = (yx + xy) * s;
z = (zy + yz) * s;
w = (xz - zx) * s;
} else {
float s = (float)Math.sqrt(1.0 + zz - xx - yy); // |s|>=1
z = s * 0.5f; // |z| >= .5
s = 0.5f / s;
x = (xz + zx) * s;
y = (zy + yz) * s;
w = (yx - xy) * s;
}
set(w, x, y, z);
}
public void setEulerAngles (float pitch, float yaw, float roll) {
setEulerAnglesRad(pitch * MathUtils.degreesToRadians, yaw * MathUtils.degreesToRadians, roll * MathUtils.degreesToRadians);
}
/** Sets the quaternion to the given euler angles in radians.
* @param pitch the rotation around the x axis in radians
* @param yaw the rotation around the y axis in radians
* @param roll the rotation around the z axis in radians
* @return this quaternion */
public void setEulerAnglesRad (float pitch, float yaw, float roll) {
final float hr = roll * 0.5f;
final float shr = (float)Math.sin(hr);
final float chr = (float)Math.cos(hr);
final float hp = pitch * 0.5f;
final float shp = (float)Math.sin(hp);
final float chp = (float)Math.cos(hp);
final float hy = yaw * 0.5f;
final float shy = (float)Math.sin(hy);
final float chy = (float)Math.cos(hy);
final float chy_shp = chy * shp;
final float shy_chp = shy * chp;
final float chy_chp = chy * chp;
final float shy_shp = shy * shp;
q[1] = (chy_shp * chr) + (shy_chp * shr); // cos(yaw/2) * sin(pitch/2) * cos(roll/2) + sin(yaw/2) * cos(pitch/2) * sin(roll/2)
q[2] = (shy_chp * chr) - (chy_shp * shr); // sin(yaw/2) * cos(pitch/2) * cos(roll/2) - cos(yaw/2) * sin(pitch/2) * sin(roll/2)
q[3] = (chy_chp * shr) - (shy_shp * chr); // cos(yaw/2) * cos(pitch/2) * sin(roll/2) - sin(yaw/2) * sin(pitch/2) * cos(roll/2)
// w
q[0] = (chy_chp * chr) + (shy_shp * shr); // cos(yaw/2) * cos(pitch/2) * cos(roll/2) + sin(yaw/2) * sin(pitch/2) * sin(roll/2)
}
/** Get the pole of the gimbal lock, if any.
* @return positive (+1) for north pole, negative (-1) for south pole, zero (0) when no gimbal lock */
public int getGimbalPole () {
float w = q[0];
float x = q[1];
float y = q[2];
float z = q[3];
final float t = y * x + z * w;
return t > 0.499f ? 1 : (t < -0.499f ? -1 : 0);
}
/** Get the roll euler angle in radians, which is the rotation around the z axis. Requires that this quaternion is normalized.
* @return the rotation around the z axis in radians (between -PI and +PI) */
public float getRollRad () {
float w = q[0];
float x = q[1];
float y = q[2];
float z = q[3];
final int pole = getGimbalPole();
return pole == 0 ? MathUtils.atan2(2f * (w * z + y * x), 1f - 2f * (x * x + z * z)) : (float)pole * 2f
* MathUtils.atan2(y, w);
}
/** Get the roll euler angle in degrees, which is the rotation around the z axis. Requires that this quaternion is normalized.
* @return the rotation around the z axis in degrees (between -180 and +180) */
public float getRoll () {
return getRollRad() * MathUtils.radiansToDegrees;
}
/** Get the pitch euler angle in radians, which is the rotation around the x axis. Requires that this quaternion is normalized.
* @return the rotation around the x axis in radians (between -(PI/2) and +(PI/2)) */
public float getPitchRad () {
float w = q[0];
float x = q[1];
float y = q[2];
float z = q[3];
final int pole = getGimbalPole();
return pole == 0 ? (float)Math.asin(MathUtils.clamp(2f * (w * x - z * y), -1f, 1f)) : (float)pole * MathUtils.PI * 0.5f;
}
/** Get the pitch euler angle in degrees, which is the rotation around the x axis. Requires that this quaternion is normalized.
* @return the rotation around the x axis in degrees (between -90 and +90) */
public float getPitch () {
return getPitchRad() * MathUtils.radiansToDegrees;
}
/** Get the yaw euler angle in radians, which is the rotation around the y axis. Requires that this quaternion is normalized.
* @return the rotation around the y axis in radians (between -PI and +PI) */
public float getYawRad () {
float w = q[0];
float x = q[1];
float y = q[2];
float z = q[3];
return getGimbalPole() == 0 ? MathUtils.atan2(2f * (y * w + x * z), 1f - 2f * (y * y + x * x)) : 0f;
}
/** Get the yaw euler angle in degrees, which is the rotation around the y axis. Requires that this quaternion is normalized.
* @return the rotation around the y axis in degrees (between -180 and +180) */
public float getYaw () {
return getYawRad() * MathUtils.radiansToDegrees;
}
public void nor () {
float w = q[0];
float x = q[1];
float y = q[2];
float z = q[3];
float len = x * x + y * y + z * z + w * w;
if (len != 0.f && !MathUtils.isEqual(len, 1f)) {
len = (float)Math.sqrt(len);
w /= len;
x /= len;
y /= len;
z /= len;
}
set(w, x, y, z);
}
@Override
public String toString() {
return String.format("MDQuaternion w=%f x=%f, y=%f, z=%f", q[0], q[1], q[2], q[3]);
}
}