From 922f968dc52032eaa36206bf38797e3b42f3fc3a Mon Sep 17 00:00:00 2001 From: Dmitry Bubelnik <74250088+DmitryBubelnikSoftSmile@users.noreply.github.com> Date: Wed, 9 Aug 2023 12:04:19 +0300 Subject: [PATCH] Implement angular distance (#40) * Reformat Quaternionf file * Implement angular distance --- math/Quaternionf.cs | 426 ++++++++++++++++++++++++++++----------- test/QuaternionfTests.cs | 32 +++ 2 files changed, 339 insertions(+), 119 deletions(-) create mode 100644 test/QuaternionfTests.cs diff --git a/math/Quaternionf.cs b/math/Quaternionf.cs index c0999358..39ac9189 100644 --- a/math/Quaternionf.cs +++ b/math/Quaternionf.cs @@ -7,72 +7,134 @@ namespace g3 public struct Quaternionf : IComparable, IEquatable { // note: in Wm5 version, this is a 4-element array stored in order (w,x,y,z). - public float x, y, z, w; + // x,y,z -- imaginary, w -- real + public float x; + public float y; + public float z; + public float w; - public Quaternionf(float x, float y, float z, float w) { this.x = x; this.y = y; this.z = z; this.w = w; } - public Quaternionf(float[] v2) { x = v2[0]; y = v2[1]; z = v2[2]; w = v2[3]; } - public Quaternionf(Quaternionf q2) { x = q2.x; y = q2.y; z = q2.z; w = q2.w; } + public Quaternionf(float x, float y, float z, float w) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } - public Quaternionf(Vector3f axis, float AngleDeg) { - x = y = z = 0; w = 1; + public Quaternionf(float[] v2) + { + x = v2[0]; + y = v2[1]; + z = v2[2]; + w = v2[3]; + } + + public Quaternionf(Quaternionf q2) + { + x = q2.x; + y = q2.y; + z = q2.z; + w = q2.w; + } + + public Quaternionf(Vector3f axis, float AngleDeg) + { + x = y = z = 0; + w = 1; SetAxisAngleD(axis, AngleDeg); } - public Quaternionf(Vector3f vFrom, Vector3f vTo) { - x = y = z = 0; w = 1; + + public Quaternionf(Vector3f vFrom, Vector3f vTo) + { + x = y = z = 0; + w = 1; SetFromTo(vFrom, vTo); } - public Quaternionf(Quaternionf p, Quaternionf q, float t) { - x = y = z = 0; w = 1; + + public Quaternionf(Quaternionf p, Quaternionf q, float t) + { + x = y = z = 0; + w = 1; SetToSlerp(p, q, t); } - public Quaternionf(Matrix3f mat) { - x = y = z = 0; w = 1; + + public Quaternionf(Matrix3f mat) + { + x = y = z = 0; + w = 1; SetFromRotationMatrix(mat); } public static readonly Quaternionf Zero = new Quaternionf(0.0f, 0.0f, 0.0f, 0.0f); public static readonly Quaternionf Identity = new Quaternionf(0.0f, 0.0f, 0.0f, 1.0f); - public float this[int key] { - readonly get { if (key == 0) return x; else if (key == 1) return y; else if (key == 2) return z; else return w; } - set { if (key == 0) x = value; else if (key == 1) y = value; else if (key == 2) z = value; else w = value; } - + public float this[int key] + { + readonly get + { + if (key == 0) return x; + else if (key == 1) return y; + else if (key == 2) return z; + else return w; + } + set + { + if (key == 0) x = value; + else if (key == 1) y = value; + else if (key == 2) z = value; + else w = value; + } } - public readonly float LengthSquared { - get { return x * x + y * y + z * z + w*w; } + public readonly float LengthSquared + { + get { return x * x + y * y + z * z + w * w; } } - public readonly float Length { + + public readonly float Length + { get { return (float)Math.Sqrt(x * x + y * y + z * z + w * w); } } - public float Normalize(float epsilon = 0) { + public float Normalize(float epsilon = 0) + { float length = Length; - if (length > epsilon) { + if (length > epsilon) + { float invLength = 1.0f / length; x *= invLength; y *= invLength; z *= invLength; w *= invLength; - } else { + } + else + { length = 0; x = y = z = w = 0; } + return length; } - public readonly Quaternionf Normalized { - get { Quaternionf q = new Quaternionf(this); q.Normalize(); return q; } + + public readonly Quaternionf Normalized + { + get + { + Quaternionf q = new Quaternionf(this); + q.Normalize(); + return q; + } } - public readonly float Dot(Quaternionf q2) { + public readonly float Dot(Quaternionf q2) + { return x * q2.x + y * q2.y + z * q2.z + w * q2.w; } - - - public static Quaternionf operator*(Quaternionf a, Quaternionf b) { + public static Quaternionf operator *(Quaternionf a, Quaternionf b) + { float w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; float x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; float y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z; @@ -80,37 +142,70 @@ public readonly float Dot(Quaternionf q2) { return new Quaternionf(x, y, z, w); } + /// + /// Performs complex-conjugation on the quaternion. For unit quaternion it is the same as inverse, but + /// we do not re-normalize it here. + /// + /// complex-conjugate + public readonly Quaternionf Conjugate() => new Quaternionf(-x, -y, -z, w); + /// + /// The angle between the two points as seen by an observer + /// + /// the other quaternion + /// Angle in radians + public readonly float AngularDistanceR(in Quaternionf q) => 2.0f * (float)Math.Acos((this * q.Conjugate()).w); - public static Quaternionf operator -(Quaternionf q1, Quaternionf q2) { + public static Quaternionf operator -(Quaternionf q1, Quaternionf q2) + { return new Quaternionf(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); } - public static Vector3f operator *(Quaternionf q, Vector3f v) { + public static Vector3f operator *(Quaternionf q, Vector3f v) + { //return q.ToRotationMatrix() * v; // inline-expansion of above: - float twoX = 2 * q.x; float twoY = 2 * q.y; float twoZ = 2 * q.z; - float twoWX = twoX * q.w; float twoWY = twoY * q.w; float twoWZ = twoZ * q.w; - float twoXX = twoX * q.x; float twoXY = twoY * q.x; float twoXZ = twoZ * q.x; - float twoYY = twoY * q.y; float twoYZ = twoZ * q.y; float twoZZ = twoZ * q.z; + float twoX = 2 * q.x; + float twoY = 2 * q.y; + float twoZ = 2 * q.z; + float twoWX = twoX * q.w; + float twoWY = twoY * q.w; + float twoWZ = twoZ * q.w; + float twoXX = twoX * q.x; + float twoXY = twoY * q.x; + float twoXZ = twoZ * q.x; + float twoYY = twoY * q.y; + float twoYZ = twoZ * q.y; + float twoZZ = twoZ * q.z; return new Vector3f( v.x * (1 - (twoYY + twoZZ)) + v.y * (twoXY - twoWZ) + v.z * (twoXZ + twoWY), v.x * (twoXY + twoWZ) + v.y * (1 - (twoXX + twoZZ)) + v.z * (twoYZ - twoWX), - v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); ; + v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); + ; } // so convenient - public static Vector3d operator *(Quaternionf q, Vector3d v) { + public static Vector3d operator *(Quaternionf q, Vector3d v) + { //return q.ToRotationMatrix() * v; // inline-expansion of above: - double twoX = 2 * q.x; double twoY = 2 * q.y; double twoZ = 2 * q.z; - double twoWX = twoX * q.w; double twoWY = twoY * q.w; double twoWZ = twoZ * q.w; - double twoXX = twoX * q.x; double twoXY = twoY * q.x; double twoXZ = twoZ * q.x; - double twoYY = twoY * q.y; double twoYZ = twoZ * q.y; double twoZZ = twoZ * q.z; + double twoX = 2 * q.x; + double twoY = 2 * q.y; + double twoZ = 2 * q.z; + double twoWX = twoX * q.w; + double twoWY = twoY * q.w; + double twoWZ = twoZ * q.w; + double twoXX = twoX * q.x; + double twoXY = twoY * q.x; + double twoXZ = twoZ * q.x; + double twoYY = twoY * q.y; + double twoYZ = twoZ * q.y; + double twoZZ = twoZ * q.z; return new Vector3d( v.x * (1 - (twoYY + twoZZ)) + v.y * (twoXY - twoWZ) + v.z * (twoXZ + twoWY), v.x * (twoXY + twoWZ) + v.y * (1 - (twoXX + twoZZ)) + v.z * (twoYZ - twoWX), - v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); ; + v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); + ; } /// @@ -145,18 +240,28 @@ public Vector3d Rotate(in Vector3d v) public readonly Vector3f InverseMultiply(ref Vector3f v) { float norm = LengthSquared; - if (norm > 0) { + if (norm > 0) + { float invNorm = 1.0f / norm; - float qx = -x*invNorm, qy = -y*invNorm, qz = -z*invNorm, qw = w*invNorm; - float twoX = 2 * qx; float twoY = 2 * qy; float twoZ = 2 * qz; - float twoWX = twoX * qw; float twoWY = twoY * qw; float twoWZ = twoZ * qw; - float twoXX = twoX * qx; float twoXY = twoY * qx; float twoXZ = twoZ * qx; - float twoYY = twoY * qy; float twoYZ = twoZ * qy; float twoZZ = twoZ * qz; + float qx = -x * invNorm, qy = -y * invNorm, qz = -z * invNorm, qw = w * invNorm; + float twoX = 2 * qx; + float twoY = 2 * qy; + float twoZ = 2 * qz; + float twoWX = twoX * qw; + float twoWY = twoY * qw; + float twoWZ = twoZ * qw; + float twoXX = twoX * qx; + float twoXY = twoY * qx; + float twoXZ = twoZ * qx; + float twoYY = twoY * qy; + float twoYZ = twoZ * qy; + float twoZZ = twoZ * qz; return new Vector3f( v.x * (1 - (twoYY + twoZZ)) + v.y * (twoXY - twoWZ) + v.z * (twoXZ + twoWY), v.x * (twoXY + twoWZ) + v.y * (1 - (twoXX + twoZZ)) + v.z * (twoYZ - twoWX), - v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); - } else + v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); + } + else return Vector3f.Zero; } @@ -165,84 +270,135 @@ public readonly Vector3f InverseMultiply(ref Vector3f v) public readonly Vector3d InverseMultiply(ref Vector3d v) { float norm = LengthSquared; - if (norm > 0) { + if (norm > 0) + { float invNorm = 1.0f / norm; float qx = -x * invNorm, qy = -y * invNorm, qz = -z * invNorm, qw = w * invNorm; - double twoX = 2 * qx; double twoY = 2 * qy; double twoZ = 2 * qz; - double twoWX = twoX * qw; double twoWY = twoY * qw; double twoWZ = twoZ * qw; - double twoXX = twoX * qx; double twoXY = twoY * qx; double twoXZ = twoZ * qx; - double twoYY = twoY * qy; double twoYZ = twoZ * qy; double twoZZ = twoZ * qz; + double twoX = 2 * qx; + double twoY = 2 * qy; + double twoZ = 2 * qz; + double twoWX = twoX * qw; + double twoWY = twoY * qw; + double twoWZ = twoZ * qw; + double twoXX = twoX * qx; + double twoXY = twoY * qx; + double twoXZ = twoZ * qx; + double twoYY = twoY * qy; + double twoYZ = twoZ * qy; + double twoZZ = twoZ * qz; return new Vector3d( v.x * (1 - (twoYY + twoZZ)) + v.y * (twoXY - twoWZ) + v.z * (twoXZ + twoWY), v.x * (twoXY + twoWZ) + v.y * (1 - (twoXX + twoZZ)) + v.z * (twoYZ - twoWX), - v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); ; - } else + v.x * (twoXZ - twoWY) + v.y * (twoYZ + twoWX) + v.z * (1 - (twoXX + twoYY))); + ; + } + else return Vector3f.Zero; } - // these multiply quaternion by (1,0,0), (0,1,0), (0,0,1), respectively. // faster than full multiply, because of all the zeros - public readonly Vector3f AxisX { - get { - float twoY = 2 * y; float twoZ = 2 * z; - float twoWY = twoY * w; float twoWZ = twoZ * w; - float twoXY = twoY * x; float twoXZ = twoZ * x; - float twoYY = twoY * y; float twoZZ = twoZ * z; + public readonly Vector3f AxisX + { + get + { + float twoY = 2 * y; + float twoZ = 2 * z; + float twoWY = twoY * w; + float twoWZ = twoZ * w; + float twoXY = twoY * x; + float twoXZ = twoZ * x; + float twoYY = twoY * y; + float twoZZ = twoZ * z; return new Vector3f(1 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY); } } - public readonly Vector3f AxisY { - get { - float twoX = 2 * x; float twoY = 2 * y; float twoZ = 2 * z; - float twoWX = twoX * w; float twoWZ = twoZ * w; float twoXX = twoX * x; - float twoXY = twoY * x; float twoYZ = twoZ * y; float twoZZ = twoZ * z; + + public readonly Vector3f AxisY + { + get + { + float twoX = 2 * x; + float twoY = 2 * y; + float twoZ = 2 * z; + float twoWX = twoX * w; + float twoWZ = twoZ * w; + float twoXX = twoX * x; + float twoXY = twoY * x; + float twoYZ = twoZ * y; + float twoZZ = twoZ * z; return new Vector3f(twoXY - twoWZ, 1 - (twoXX + twoZZ), twoYZ + twoWX); } } - public readonly Vector3f AxisZ { - get { - float twoX = 2 * x; float twoY = 2 * y; float twoZ = 2 * z; - float twoWX = twoX * w; float twoWY = twoY * w; float twoXX = twoX * x; - float twoXZ = twoZ * x; float twoYY = twoY * y; float twoYZ = twoZ * y; + + public readonly Vector3f AxisZ + { + get + { + float twoX = 2 * x; + float twoY = 2 * y; + float twoZ = 2 * z; + float twoWX = twoX * w; + float twoWY = twoY * w; + float twoXX = twoX * x; + float twoXZ = twoZ * x; + float twoYY = twoY * y; + float twoYZ = twoZ * y; return new Vector3f(twoXZ + twoWY, twoYZ - twoWX, 1 - (twoXX + twoYY)); } } - - public readonly Quaternionf Inverse() { + public readonly Quaternionf Inverse() + { float norm = LengthSquared; - if (norm > 0) { + if (norm > 0) + { float invNorm = 1.0f / norm; return new Quaternionf( -x * invNorm, -y * invNorm, -z * invNorm, w * invNorm); - } else + } + else return Quaternionf.Zero; } - public static Quaternionf Inverse(Quaternionf q) { + + public static Quaternionf Inverse(Quaternionf q) + { return q.Inverse(); } - public readonly Matrix3f ToRotationMatrix() { - float twoX = 2 * x; float twoY = 2 * y; float twoZ = 2 * z; - float twoWX = twoX * w; float twoWY = twoY * w; float twoWZ = twoZ * w; - float twoXX = twoX * x; float twoXY = twoY * x; float twoXZ = twoZ * x; - float twoYY = twoY * y; float twoYZ = twoZ * y; float twoZZ = twoZ * z; + float twoX = 2 * x; + float twoY = 2 * y; + float twoZ = 2 * z; + float twoWX = twoX * w; + float twoWY = twoY * w; + float twoWZ = twoZ * w; + float twoXX = twoX * x; + float twoXY = twoY * x; + float twoXZ = twoZ * x; + float twoYY = twoY * y; + float twoYZ = twoZ * y; + float twoZZ = twoZ * z; Matrix3f m = Matrix3f.Zero; - m[0, 0] = 1 - (twoYY + twoZZ); m[0, 1] = twoXY - twoWZ; m[0, 2] = twoXZ + twoWY; - m[1, 0] = twoXY + twoWZ; m[1, 1] = 1 - (twoXX + twoZZ); m[1, 2] = twoYZ - twoWX; - m[2, 0] = twoXZ - twoWY; m[2, 1] = twoYZ + twoWX; m[2, 2] = 1 - (twoXX + twoYY); + m[0, 0] = 1 - (twoYY + twoZZ); + m[0, 1] = twoXY - twoWZ; + m[0, 2] = twoXZ + twoWY; + m[1, 0] = twoXY + twoWZ; + m[1, 1] = 1 - (twoXX + twoZZ); + m[1, 2] = twoYZ - twoWX; + m[2, 0] = twoXZ - twoWY; + m[2, 1] = twoYZ + twoWX; + m[2, 2] = 1 - (twoXX + twoYY); return m; } - - public void SetAxisAngleD(Vector3f axis, float AngleDeg) { + public void SetAxisAngleD(Vector3f axis, float AngleDeg) + { double angle_rad = MathUtil.Deg2Rad * AngleDeg; double halfAngle = 0.5 * angle_rad; double sn = Math.Sin(halfAngle); @@ -251,15 +407,20 @@ public void SetAxisAngleD(Vector3f axis, float AngleDeg) { y = (float)(sn * axis.y); z = (float)(sn * axis.z); } - public static Quaternionf AxisAngleD(Vector3f axis, float angleDeg) { + + public static Quaternionf AxisAngleD(Vector3f axis, float angleDeg) + { return new Quaternionf(axis, angleDeg); } - public static Quaternionf AxisAngleR(Vector3f axis, float angleRad) { + + public static Quaternionf AxisAngleR(Vector3f axis, float angleRad) + { return new Quaternionf(axis, angleRad * MathUtil.Rad2Degf); } // this function can take non-normalized vectors vFrom and vTo (normalizes internally) - public void SetFromTo(Vector3f vFrom, Vector3f vTo) { + public void SetFromTo(Vector3f vFrom, Vector3f vTo) + { // [TODO] this page seems to have optimized version: // http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors @@ -269,20 +430,26 @@ public void SetFromTo(Vector3f vFrom, Vector3f vTo) { Vector3f from = vFrom.Normalized, to = vTo.Normalized; Vector3f bisector = (from + to).Normalized; w = from.Dot(bisector); - if (w != 0) { + if (w != 0) + { Vector3f cross = from.Cross(bisector); x = cross.x; y = cross.y; z = cross.z; - } else { + } + else + { float invLength; - if (Math.Abs(from.x) >= Math.Abs(from.y)) { + if (Math.Abs(from.x) >= Math.Abs(from.y)) + { // V1.x or V1.z is the largest magnitude component. invLength = (float)(1.0 / Math.Sqrt(from.x * from.x + from.z * from.z)); x = -from.z * invLength; y = 0; z = +from.x * invLength; - } else { + } + else + { // V1.y or V1.z is the largest magnitude component. invLength = (float)(1.0 / Math.Sqrt(from.y * from.y + from.z * from.z)); x = 0; @@ -290,11 +457,15 @@ public void SetFromTo(Vector3f vFrom, Vector3f vTo) { z = -from.y * invLength; } } - Normalize(); // aaahhh just to be safe... + + Normalize(); // aaahhh just to be safe... } - public static Quaternionf FromTo(Vector3f vFrom, Vector3f vTo) { + + public static Quaternionf FromTo(Vector3f vFrom, Vector3f vTo) + { return new Quaternionf(vFrom, vTo); } + public static Quaternionf FromToConstrained(Vector3f vFrom, Vector3f vTo, Vector3f vAround) { float fAngle = MathUtil.PlaneAngleSignedD(vFrom, vTo, vAround); @@ -306,7 +477,8 @@ public void SetToSlerp(Quaternionf p, Quaternionf q, float t) { float cs = p.Dot(q); float angle = (float)Math.Acos(cs); - if (Math.Abs(angle) >= MathUtil.ZeroTolerance) { + if (Math.Abs(angle) >= MathUtil.ZeroTolerance) + { float sn = (float)Math.Sin(angle); float invSn = 1 / sn; float tAngle = t * angle; @@ -316,19 +488,22 @@ public void SetToSlerp(Quaternionf p, Quaternionf q, float t) y = coeff0 * p.y + coeff1 * q.y; z = coeff0 * p.z + coeff1 * q.z; w = coeff0 * p.w + coeff1 * q.w; - } else { + } + else + { x = p.x; y = p.y; z = p.z; w = p.w; } } - public static Quaternionf Slerp(Quaternionf p, Quaternionf q, float t) { + + public static Quaternionf Slerp(Quaternionf p, Quaternionf q, float t) + { return new Quaternionf(p, q, t); } - public void SetFromRotationMatrix(Matrix3f rot) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes @@ -338,23 +513,30 @@ public void SetFromRotationMatrix(Matrix3f rot) float trace = rot[0, 0] + rot[1, 1] + rot[2, 2]; float root; - if (trace > 0) { + if (trace > 0) + { // |w| > 1/2, may as well choose w > 1/2 - root = (float)Math.Sqrt(trace + (float)1); // 2w + root = (float)Math.Sqrt(trace + (float)1); // 2w w = ((float)0.5) * root; - root = ((float)0.5) / root; // 1/(4w) + root = ((float)0.5) / root; // 1/(4w) x = (rot[2, 1] - rot[1, 2]) * root; y = (rot[0, 2] - rot[2, 0]) * root; z = (rot[1, 0] - rot[0, 1]) * root; - } else { + } + else + { // |w| <= 1/2 int i = 0; - if (rot[1, 1] > rot[0, 0]) { + if (rot[1, 1] > rot[0, 0]) + { i = 1; } - if (rot[2, 2] > rot[i, i]) { + + if (rot[2, 2] > rot[i, i]) + { i = 2; } + int j = next[i]; int k = next[j]; @@ -366,27 +548,30 @@ public void SetFromRotationMatrix(Matrix3f rot) w = (rot[k, j] - rot[j, k]) * root; quat[j] = (rot[j, i] + rot[i, j]) * root; quat[k] = (rot[k, i] + rot[i, k]) * root; - x = quat.x; y = quat.y; z = quat.z; + x = quat.x; + y = quat.y; + z = quat.z; } - Normalize(); // we prefer normalized quaternions... + Normalize(); // we prefer normalized quaternions... } - - public static bool operator ==(Quaternionf a, Quaternionf b) { return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); } + public static bool operator !=(Quaternionf a, Quaternionf b) { return (a.x != b.x || a.y != b.y || a.z != b.z || a.w != b.w); } + public override readonly bool Equals(object obj) { return this == (Quaternionf)obj; } + public override readonly int GetHashCode() { unchecked // Overflow is fine, just wrap @@ -400,6 +585,7 @@ public override readonly int GetHashCode() return hash; } } + public readonly int CompareTo(Quaternionf other) { if (x != other.x) @@ -412,28 +598,30 @@ public readonly int CompareTo(Quaternionf other) return w < other.w ? -1 : 1; return 0; } + public readonly bool Equals(Quaternionf other) { return (x == other.x && y == other.y && z == other.z && w == other.w); } - - - - public readonly bool EpsilonEqual(Quaternionf q2, float epsilon) { - return (float)Math.Abs(x - q2.x) <= epsilon && + public readonly bool EpsilonEqual(Quaternionf q2, float epsilon) + { + return (float)Math.Abs(x - q2.x) <= epsilon && (float)Math.Abs(y - q2.y) <= epsilon && (float)Math.Abs(z - q2.z) <= epsilon && (float)Math.Abs(w - q2.w) <= epsilon; } - public override readonly string ToString() { + public override readonly string ToString() + { return string.Format("{0:F8} {1:F8} {2:F8} {3:F8}", x, y, z, w); } - public readonly string ToString(string fmt) { + + public readonly string ToString(string fmt) + { return string.Format("{0} {1} {2} {3}", x.ToString(fmt), y.ToString(fmt), z.ToString(fmt), w.ToString(fmt)); } } -} +} \ No newline at end of file diff --git a/test/QuaternionfTests.cs b/test/QuaternionfTests.cs new file mode 100644 index 00000000..868772bf --- /dev/null +++ b/test/QuaternionfTests.cs @@ -0,0 +1,32 @@ +using System; +using FluentAssertions; +using g3; +using Xunit; + +namespace geometry3sharp.Tests; + +public class QuaternionfTests +{ + private const float Precision = 1e-5f; + private static readonly Random Random = new((int)DateTime.UtcNow.Ticks); + + [Theory] + [InlineData(120)] + [InlineData(60)] + [InlineData(30)] + [InlineData(15)] + public void ShouldCalculateAngularDistance(float degrees) => + Quaternionf.Identity.AngularDistanceR(Quaternionf.AxisAngleD(Vector3f.AxisX, angleDeg: degrees)).Should() + .BeApproximately(expectedValue: degrees * MathUtil.Deg2Radf, precision: Precision); + + [Fact] + public void ShouldReturnIdentity_WhenMultipliedByConjugate() + { + Quaternionf q = new Quaternionf(Random.Next(), Random.Next(), Random.Next(), Random.Next()).Normalized; + Quaternionf product = q * q.Conjugate(); + product.x.Should().BeApproximately(0, Precision); + product.y.Should().BeApproximately(0, Precision); + product.z.Should().BeApproximately(0, Precision); + product.w.Should().BeApproximately(1, Precision); + } +} \ No newline at end of file