package com.gebauz.bauzoid2.math;
/**
* Created by cchiu on 02.01.2015.
*/
public class Quaternion
{
public float x
;
public float y
;
public float z
;
public float w
;
public Quaternion
()
{
}
public Quaternion
(float _x,
float _y,
float _z,
float _w
)
{
set
(_x, _y, _z, _w
);
}
public Quaternion
(float[] array
)
{
x = array
[0];
y = array
[1];
z = array
[2];
w = array
[3];
}
public void set
(float _x,
float _y,
float _z,
float _w
)
{
x = _x
;
y = _y
;
z = _z
;
w = _w
;
}
public void setFrom
(Vector4 other
)
{
x = other.
x;
y = other.
y;
z = other.
z;
w = other.
w;
}
public void identity
()
{
x =
0; y =
0; z =
0; w =
1;
}
public Quaternion createIdentity
()
{
Quaternion result =
new Quaternion
();
result.
identity();
return result
;
}
public Quaternion getConjugate
()
{
return new Quaternion
(-x, -y, -z, w
);
}
public void normalize
()
{
// Don't normalize if we don't have to
float mag2 = w
* w + x
* x + y
* y + z
* z
;
if (Math.
abs(mag2
) > MathUtil.
EPSILON && Math.
abs(mag2 - 1.0f
) > MathUtil.
EPSILON)
{
float mag =
(float)Math.
sqrt(mag2
);
w /= mag
;
x /= mag
;
y /= mag
;
z /= mag
;
}
}
static public void multiply
(Quaternion result, Quaternion a, Quaternion b
)
{
result.
x = a.
w * b.
x + a.
x * b.
w + a.
y * b.
z - a.
z * b.
y;
result.
y = a.
w * b.
y + a.
y * b.
w + a.
z * b.
x - a.
x * b.
z;
result.
z = a.
w * b.
z + a.
z * b.
w + a.
x * b.
y - a.
y * b.
x;
result.
w = a.
w * b.
w - a.
x * b.
x - a.
y * b.
y - a.
z * b.
z;
}
static public Quaternion multiply
(Quaternion a, Quaternion b
)
{
Quaternion result =
new Quaternion
();
multiply
(result, a, b
);
return result
;
}
public Vector3 rotate
(Vector3 v
)
{
Vector3 vn = v.
copy();
vn.
normalize();
Quaternion vecQuat =
new Quaternion
();
vecQuat.
x = vn.
x;
vecQuat.
y = vn.
y;
vecQuat.
z = vn.
z;
vecQuat.
w = 0.0f
;
Quaternion result = multiply
(this, multiply
(vecQuat, getConjugate
()));
return new Vector3
(result.
x, result.
y, result.
z);
}
public void setAxisAngle
(float axisX,
float axisY,
float axisZ,
float degrees
)
{
degrees
*= 0.5f
;
Vector3 vn =
new Vector3
(axisX, axisY, axisZ
);
vn.
normalize();
float sinAngle = MathUtil.
sin(degrees
);
x =
(vn.
x * sinAngle
);
y =
(vn.
y * sinAngle
);
z =
(vn.
z * sinAngle
);
w = MathUtil.
cos(degrees
);
}
public void setAxisAngle
(Vector3 axis,
float degrees
)
{
setAxisAngle
(axis.
x, axis.
y, axis.
z, degrees
);
}
public static Quaternion createAxisAngle
(float axisX,
float axisY,
float axisZ,
float degrees
)
{
Quaternion result =
new Quaternion
();
result.
setAxisAngle(axisX, axisY, axisZ, degrees
);
return result
;
}
public static Quaternion createAxisAngle
(Vector3 axis,
float degrees
)
{
Quaternion result =
new Quaternion
();
result.
setAxisAngle(axis, degrees
);
return result
;
}
/** pitch = x-axis, yaw = y-axis, roll = z-axis */
public void setEuler
(float pitch,
float yaw,
float roll
)
{
float p = MathUtil.
degToRad(pitch
) / 2.0f
;
float y = MathUtil.
degToRad(yaw
) / 2.0f
;
float r = MathUtil.
degToRad(roll
) / 2.0f
;
float sinp =
(float)Math.
sin(p
);
float siny =
(float)Math.
sin(y
);
float sinr =
(float)Math.
sin(r
);
float cosp =
(float)Math.
cos(p
);
float cosy =
(float)Math.
cos(y
);
float cosr =
(float)Math.
cos(r
);
x = sinr
* cosp
* cosy - cosr
* sinp
* siny
;
y = cosr
* sinp
* cosy + sinr
* cosp
* siny
;
z = cosr
* cosp
* siny - sinr
* sinp
* cosy
;
w = cosr
* cosp
* cosy + sinr
* sinp
* siny
;
normalize
();
}
public static Quaternion createEuler
(float pitch,
float yaw,
float roll
)
{
Quaternion result =
new Quaternion
();
result.
setEuler(pitch, yaw, roll
);
return result
;
}
public Matrix4 toMatrix
()
{
final float x2 = x
* x
;
final float y2 = y
* y
;
final float z2 = z
* z
;
final float xy = x
* y
;
final float xz = x
* z
;
final float yz = y
* z
;
final float wx = w
* x
;
final float wy = w
* y
;
final float wz = w
* z
;
// This calculation would be a lot more complicated for non-unit length quaternions
// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
// OpenGL
Matrix4 matrix =
new Matrix4
();
matrix.
values[Matrix4.
M11] = 1.0f - 2.0f
* (y2 + z2
);
matrix.
values[Matrix4.
M12] = 2.0f
* (xy - wz
);
matrix.
values[Matrix4.
M13] = 2.0f
* (xz + wy
);
matrix.
values[Matrix4.
M14] = 0.0f
;
matrix.
values[Matrix4.
M21] = 2.0f
* (xy + wz
);
matrix.
values[Matrix4.
M22] = 1.0f - 2.0f
* (x2 + z2
);
matrix.
values[Matrix4.
M23] = 2.0f
* (yz - wx
);
matrix.
values[Matrix4.
M24] = 0.0f
;
matrix.
values[Matrix4.
M31] = 2.0f
* (xz - wy
);
matrix.
values[Matrix4.
M32] = 2.0f
* (yz + wx
);
matrix.
values[Matrix4.
M33] = 1.0f - 2.0f
* (x2 + y2
);
matrix.
values[Matrix4.
M34] = 0.0f
;
matrix.
values[Matrix4.
M41] = 0.0f
;
matrix.
values[Matrix4.
M42] = 0.0f
;
matrix.
values[Matrix4.
M43] = 0.0f
;
matrix.
values[Matrix4.
M44] = 1.0f
;
return matrix
;
/*
// Set matrix from quaternion
matrix[Matrix4.M00] = 1 - 2 * (yy + zz);
matrix[Matrix4.M01] = 2 * (xy - zw);
matrix[Matrix4.M02] = 2 * (xz + yw);
matrix[Matrix4.M03] = 0;
matrix[Matrix4.M10] = 2 * (xy + zw);
matrix[Matrix4.M11] = 1 - 2 * (xx + zz);
matrix[Matrix4.M12] = 2 * (yz - xw);
matrix[Matrix4.M13] = 0;
matrix[Matrix4.M20] = 2 * (xz - yw);
matrix[Matrix4.M21] = 2 * (yz + xw);
matrix[Matrix4.M22] = 1 - 2 * (xx + yy);
matrix[Matrix4.M23] = 0;
matrix[Matrix4.M30] = 0;
matrix[Matrix4.M31] = 0;
matrix[Matrix4.M32] = 0;
matrix[Matrix4.M33] = 1;
*/
}
public Vector3 getAxis
()
{
float scale =
(float)Math.
sqrt(x
* x + y
* y + z
* z
);
return new Vector3
(x / scale, y / scale, z / scale
);
}
public float getAngleDeg
()
{
return MathUtil.
radToDeg((float)Math.
acos(w
) * 2.0f
);
}
}