28package com.jogamp.math;
46 private float x, y, z, w;
64 public Quaternion(
final float x,
final float y,
final float z,
final float w) {
76 return w*w + x*x + y*y + z*z;
104 public final float w() {
108 public final void setW(
final float w) {
112 public final float x() {
116 public final void setX(
final float x) {
120 public final float y() {
124 public final void setY(
final float y) {
128 public final float z() {
132 public final void setZ(
final float z) {
139 public final float dot(
final float x,
final float y,
final float z,
final float w) {
140 return this.x * x + this.y * y + this.z * z + this.w * w;
147 return dot(quat.x(), quat.y(), quat.z(), quat.w());
169 x = y = z = 0f; w = 1f;
188 final float invNorm = 1f/norm;
225 final float invmsq = 1f/magnitudeSQ;
250 public final Quaternion set(
final float x,
final float y,
final float z,
final float w) {
297 return set( w * q.x + x * q.w + y * q.z - z * q.y,
298 w * q.y - x * q.z + y * q.w + z * q.x,
299 w * q.z + x * q.y - y * q.x + z * q.w,
300 w * q.w - x * q.x - y * q.y - z * q.z );
338 final float halfAngle = 0.5f * angle;
341 final float qx = sin * axisX;
342 final float qy = sin * axisY;
343 final float qz = sin * axisZ;
344 return set( x * qw + y * qz - z * qy + w * qx,
345 -x * qz + y * qw + z * qx + w * qy,
346 x * qy - y * qx + z * qw + w * qz,
347 -x * qx - y * qy - z * qz + w * qw);
374 final float halfAngle = 0.5f * angle;
377 return set( x * cos + w * sin,
390 final float halfAngle = 0.5f * angle;
393 return set( x * cos - z * sin,
406 final float halfAngle = 0.5f * angle;
409 return set( x * cos + y * sin,
431 return rotateByEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z());
482 final float vecX = vecIn.x();
483 final float vecY = vecIn.y();
484 final float vecZ = vecIn.z();
485 final float x_x = x*x;
486 final float y_y = y*y;
487 final float z_z = z*z;
488 final float w_w = w*w;
490 vecOut.
setX( w_w * vecX
494 + 2f * ( y*w*vecZ - z*w*vecY + y*x*vecY + z*x*vecZ ) );
497 vecOut.
setY( y_y * vecY
501 + 2f * ( x*y*vecX + z*y*vecZ + w*z*vecX - x*w*vecZ ) );;
503 vecOut.
setZ( z_z * vecZ
507 + 2f * ( x*z*vecX + y*z*vecY - w*y*vecX + w*x*vecY ) );
527 if (changeAmnt == 0.0f) {
529 }
else if (changeAmnt == 1.0f) {
538 float cosHalfTheta = a.x * bx + a.y * by + a.z * bz + a.w * bw;
540 final float scale0, scale1;
542 if( cosHalfTheta >= 0.95f ) {
544 scale0 = 1.0f - changeAmnt;
547 }
else if ( cosHalfTheta <= -0.99f ) {
565 final float sinHalfTheta =
FloatUtil.
sqrt(1.0f - cosHalfTheta*cosHalfTheta);
568 if ( Math.abs(sinHalfTheta) < 0.001f ){
575 scale0 =
FloatUtil.
sin((1f - changeAmnt) * halfTheta) / sinHalfTheta;
576 scale1 =
FloatUtil.
sin(changeAmnt * halfTheta) / sinHalfTheta;
580 x = a.x * scale0 + bx * scale1;
581 y = a.y * scale0 + by * scale1;
582 z = a.z * scale0 + bz * scale1;
583 w = a.w * scale0 + bw * scale1;
665 final float dot = v1.
dot(v2) / factor;
668 tmpPivotVec.
cross(v1, v2);
676 if (Math.abs(v1.x()) > Math.abs(v1.y())) {
677 if (Math.abs(v1.x()) > Math.abs(v1.z())) {
683 if (Math.abs(v1.y()) > Math.abs(v1.z())) {
689 tmpPivotVec.
set( dominantIndex, -v1.
get( (dominantIndex + 1) % 3 ) );
690 tmpPivotVec.
set( (dominantIndex + 1) % 3, v1.
get( dominantIndex ) );
691 tmpPivotVec.
set( (dominantIndex + 2) % 3, 0f );
720 final float dot = v1.
dot(v2) / factor;
723 tmpPivotVec.
cross(v1, v2);
731 if (Math.abs(v1.x()) > Math.abs(v1.y())) {
732 if (Math.abs(v1.x()) > Math.abs(v1.z())) {
738 if (Math.abs(v1.y()) > Math.abs(v1.z())) {
744 tmpPivotVec.
set( dominantIndex, -v1.
get( (dominantIndex + 1) % 3 ) );
745 tmpPivotVec.
set( (dominantIndex + 1) % 3, v1.
get( dominantIndex ) );
746 tmpPivotVec.
set( (dominantIndex + 2) % 3, 0f );
792 final float halfangle = angle * 0.5f;
794 x = vector.x() * sin;
795 y = vector.y() * sin;
796 z = vector.z() * sin;
810 final float sqrLength = x*x + y*y + z*z;
814 axis.
set( 1.0f, 0.0f, 0.0f );
818 axis.
set( x * invLength,
841 return setFromEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z());
875 float angle = headingY * 0.5f;
878 angle = attitudeZ * 0.5f;
881 angle = bankX * 0.5f;
886 final float cosHeadingXcosAttitude = cosHeadingY * cosAttitudeZ;
887 final float sinHeadingXsinAttitude = sinHeadingY * sinAttitudeZ;
888 final float cosHeadingXsinAttitude = cosHeadingY * sinAttitudeZ;
889 final float sinHeadingXcosAttitude = sinHeadingY * cosAttitudeZ;
891 w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX;
892 x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX;
893 y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX;
894 z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX;
916 final float sqw = w*w;
917 final float sqx = x*x;
918 final float sqy = y*y;
919 final float sqz = z*z;
920 final float unit = sqx + sqy + sqz + sqw;
921 final float test = x*y + z*w;
923 if (test > 0.499f * unit) {
927 }
else if (test < -0.499f * unit) {
953 final float m10,
final float m11,
final float m12,
954 final float m20,
final float m21,
final float m22) {
960 final float T = m00 + m11 + m22 + 1f;
966 x = ( m21 - m12 ) * S;
967 y = ( m02 - m20 ) * S;
968 z = ( m10 - m01 ) * S;
969 }
else if ( m00 > m11 && m00 > m22) {
971 final float S = 0.5f /
FloatUtil.
sqrt(1.0f + m00 - m11 - m22);
972 w = ( m21 - m12 ) * S;
974 y = ( m10 + m01 ) * S;
975 z = ( m02 + m20 ) * S;
976 }
else if ( m11 > m22 ) {
978 final float S = 0.5f /
FloatUtil.
sqrt(1.0f + m11 - m00 - m22);
979 w = ( m02 - m20 ) * S;
980 x = ( m20 + m01 ) * S;
982 z = ( m21 + m12 ) * S;
985 final float S = 0.5f /
FloatUtil.
sqrt(1.0f + m22 - m00 - m11);
986 w = ( m10 - m01 ) * S;
987 x = ( m02 + m20 ) * S;
988 y = ( m21 + m12 ) * S;
1038 srecip = 2.0f / norm;
1041 final float xs = srecip * x;
1042 final float ys = srecip * y;
1043 final float zs = srecip * z;
1045 final float xx = x * xs;
1046 final float xy = x * ys;
1047 final float xz = x * zs;
1048 final float xw = xs * w;
1049 final float yy = y * ys;
1050 final float yz = y * zs;
1051 final float yw = ys * w;
1052 final float zz = z * zs;
1053 final float zw = zs * w;
1055 matrix[0+0*4] = 1f - ( yy + zz );
1056 matrix[0+1*4] = ( xy - zw );
1057 matrix[0+2*4] = ( xz + yw );
1060 matrix[1+0*4] = ( xy + zw );
1061 matrix[1+1*4] = 1f - ( xx + zz );
1062 matrix[1+2*4] = ( yz - xw );
1065 matrix[2+0*4] = ( xz - yw );
1066 matrix[2+1*4] = ( yz + xw );
1067 matrix[2+2*4] = 1f - ( xx + yy );
1108 return setFromMat(xAxis.x(), yAxis.x(), zAxis.x(),
1109 xAxis.y(), yAxis.y(), zAxis.y(),
1110 xAxis.z(), yAxis.z(), zAxis.z());
1137 final float epsilon = 0.01f;
1138 if (Math.abs(m[0] * m[3] + m[3] * m[4] + m[6] * m[7]) > epsilon)
1140 if (Math.abs(m[0] * m[2] + m[3] * m[5] + m[6] * m[8]) > epsilon)
1142 if (Math.abs(m[1] * m[2] + m[4] * m[5] + m[7] * m[8]) > epsilon)
1144 if (Math.abs(m[0] * m[0] + m[3] * m[3] + m[6] * m[6] - 1) > epsilon)
1146 if (Math.abs(m[1] * m[1] + m[4] * m[4] + m[7] * m[7] - 1) > epsilon)
1148 if (Math.abs(m[2] * m[2] + m[5] * m[5] + m[8] * m[8] - 1) > epsilon)
1150 return (Math.abs(determinant3f(m) - 1) < epsilon);
1154 private final float determinant3f(
final float[] m) {
1155 return m[0] * m[4] * m[8] + m[3] * m[7] * m[2] + m[6] * m[1] * m[5]
1156 - m[0] * m[7] * m[5] - m[3] * m[1] * m[8] - m[6] * m[4] * m[2];
1183 throw new InternalError(
"hashCode not designed");
1188 return "Quat[x "+x+
", y "+y+
", z "+z+
", w "+w+
"]";
Basic Float math utility functions.
static float acos(final float a)
static float sin(final float a)
static float atan2(final float y, final float x)
static final float EPSILON
Epsilon for floating point {@value}, as once computed via getMachineEpsilon() on an AMD-64 CPU.
static float cos(final float a)
static float asin(final float a)
static boolean isZero(final float a, final float epsilon)
Returns true if value is zero, i.e.
static boolean isEqual(final float a, final float b, final float epsilon)
Returns true if both values are equal, i.e.
static float[] makeIdentity(final float[] m)
Make matrix an identity matrix.
static final float HALF_PI
The value PI/2, i.e.
static float sqrt(final float a)
Basic 4x4 float matrix implementation using fields for intensive use-cases (host operations).
final Matrix4f setToRotation(final Quaternion q)
Set this matrix to rotation using the given Quaternion.
final Quaternion getRotation(final Quaternion res)
Returns the rotation [m00 .
Vec4f getColumn(final int column, final Vec4f v_out)
Get the named column of the given column-major matrix to v_out.
Quaternion implementation supporting Gimbal-Lock free rotations.
Quaternion setFromMat(final Matrix4f m)
Compute the quaternion from a 3x3 column rotation matrix from Matrix4f instance.
final float dot(final float x, final float y, final float z, final float w)
Returns the dot product of this quaternion with the given x,y,z and w components.
final Vec3f rotateVector(final Vec3f vecIn, final Vec3f vecOut)
final Quaternion rotateByEuler(final float bankX, final float headingY, final float attitudeZ)
Rotates this quaternion from the given Euler rotation angles in radians.
Quaternion conjugate()
Conjugates this quaternion [-x, -y, -z, w].
final Quaternion invert()
Invert the quaternion If rotational, will produce a the inverse rotation.
final Quaternion setFromNormalVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec)
Initialize this quaternion from two normalized vectors.
final Matrix4f toMatrix(final Matrix4f matrix)
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
final Quaternion rotateByEuler(final Vec3f angradXYZ)
Rotates this quaternion from the given Euler rotation array angradXYZ in radians.
final Quaternion scale(final float n)
Scale this quaternion by a constant.
Vec3f toEuler(final Vec3f result)
Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
boolean equals(final Object o)
final void setX(final float x)
final Quaternion setFromEuler(final Vec3f angradXYZ)
Initializes this quaternion from the given Euler rotation array angradXYZ in radians.
final Quaternion setSlerp(final Quaternion a, final Quaternion b, final float changeAmnt)
Set this quaternion to a spherical linear interpolation between the given start and end quaternions b...
Quaternion rotateByAngleNormalAxis(final float angle, final float axisX, final float axisY, final float axisZ)
Rotate this quaternion by the given angle and axis.
final float magnitudeSquared()
See magnitude() for special handling of epsilon, which is not applied here.
final Quaternion setFromAngleNormalAxis(final float angle, final Vec3f vector)
final void setY(final float y)
final void setZ(final float z)
final Quaternion setFromAngleAxis(final float angle, final Vec3f vector, final Vec3f tmpV3f)
final boolean isRotationMatrix3f(final float[] m)
Check if the the 3x3 matrix (param) is in fact an affine rotational matrix.
static final float ALLOWED_DEVIANCE
Quaternion Epsilon, used with equals method to determine if two Quaternions are close enough to be co...
final void setW(final float w)
final Quaternion setFromEuler(final float bankX, final float headingY, final float attitudeZ)
Initializes this quaternion from the given Euler rotation angles in radians.
Quaternion rotateByAngleY(final float angle)
Rotate this quaternion around Y axis with the given angle in radians.
Quaternion rotateByAngleZ(final float angle)
Rotate this quaternion around Z axis with the given angle in radians.
final Quaternion setFromAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis)
Initializes this quaternion to represent a rotation formed by the given three orthogonal axes.
final float toAngleAxis(final Vec3f axis)
Transform the rotational quaternion to axis based rotation angles.
final Quaternion subtract(final Quaternion q)
Subtract a quaternion.
Quaternion(final float x, final float y, final float z, final float w)
Quaternion setFromMat(final float m00, final float m01, final float m02, final float m10, final float m11, final float m12, final float m20, final float m21, final float m22)
Compute the quaternion from a 3x3 column rotation matrix.
final float dot(final Quaternion quat)
Returns the dot product of this quaternion with the given quaternion.
final Quaternion setFromVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec, final Vec3f tmpNormalVec)
Initialize this quaternion from two vectors.
final float[] toMatrix(final float[] matrix)
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
void toAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis, final Matrix4f tmpMat4)
Extracts this quaternion's orthogonal rotation axes.
Quaternion rotateByAngleNormalAxis(final float angle, final Vec3f axis)
Rotate this quaternion by the given angle and axis.
Quaternion setLookAt(final Vec3f directionIn, final Vec3f upIn, final Vec3f xAxisOut, final Vec3f yAxisOut, final Vec3f zAxisOut)
Set this quaternion to equal the rotation required to point the z-axis at direction and the y-axis to...
final float magnitude()
Return the magnitude of this quaternion, i.e.
final Quaternion mult(final Quaternion q)
Multiply this quaternion by the param quaternion.
final Quaternion setIdentity()
final Quaternion normalize()
Normalize a quaternion required if to be used as a rotational quaternion.
final Quaternion add(final Quaternion q)
Add a quaternion.
final boolean isIdentity()
Returns true if this quaternion has identity.
Quaternion rotateByAngleX(final float angle)
Rotate this quaternion around X axis with the given angle in radians.
Quaternion(final Quaternion q)
3D Vector based upon three float components.
boolean isZero()
Return true if all components are zero, i.e.
Vec3f normalize()
Normalize this vector in place.
float dot(final Vec3f o)
Return the dot product of this vector and the given one.
float length()
Return the length of this vector, a.k.a the norm or magnitude
float[] get(final float[] xyz)
xyz = this, returns xyz.
Vec3f cross(final Vec3f arg)
Returns this cross arg; creates new vector.
Vec3f set(final Vec3f o)
this = o, returns this.
static boolean isZero(final float x, final float y, final float z, final float epsilon)
Return true if all three vector components are zero, i.e.