From 0ce1b2ad04174eb8048aaff8485a2290e9dd06cf Mon Sep 17 00:00:00 2001 From: Donovan Hutchence Date: Wed, 19 Jun 2024 12:22:52 +0100 Subject: [PATCH 1/5] update setFromMat4 and add tests --- src/core/math/quat.js | 155 ++++++++++++++--------------------- test/core/math/quat.test.mjs | 123 +++++++++++++++++++++++++++ 2 files changed, 186 insertions(+), 92 deletions(-) diff --git a/src/core/math/quat.js b/src/core/math/quat.js index 8eb9d121aa9..18ed6db6b47 100644 --- a/src/core/math/quat.js +++ b/src/core/math/quat.js @@ -288,6 +288,21 @@ class Quat { return this; } + /** + * Multiplies each element of a quaternion by a number. + * + * @param {number} scalar - The number to multiply by. + * @param {Quat} [src] - The quaternion to scale. If not set, the operation is done in place. + * @returns {Quat} Self for chaining. + */ + mulScalar(scalar, src = this) { + this.x = src.x * scalar; + this.y = src.y * scalar; + this.z = src.z * scalar; + this.w = src.w * scalar; + return this; + } + /** * Returns the result of multiplying the specified quaternions together. * @@ -461,103 +476,59 @@ class Quat { * const q = new pc.Quat().setFromMat4(rot); */ setFromMat4(m) { - let m00, m01, m02, m10, m11, m12, m20, m21, m22, - s, rs, lx, ly, lz; - - m = m.data; - - // Cache matrix values for super-speed - m00 = m[0]; - m01 = m[1]; - m02 = m[2]; - m10 = m[4]; - m11 = m[5]; - m12 = m[6]; - m20 = m[8]; - m21 = m[9]; - m22 = m[10]; - - // Remove the scale from the matrix - lx = m00 * m00 + m01 * m01 + m02 * m02; - if (lx === 0) - return this; - lx = 1 / Math.sqrt(lx); - ly = m10 * m10 + m11 * m11 + m12 * m12; - if (ly === 0) - return this; - ly = 1 / Math.sqrt(ly); - lz = m20 * m20 + m21 * m21 + m22 * m22; - if (lz === 0) - return this; - lz = 1 / Math.sqrt(lz); - - m00 *= lx; - m01 *= lx; - m02 *= lx; - m10 *= ly; - m11 *= ly; - m12 *= ly; - m20 *= lz; - m21 *= lz; - m22 *= lz; - - // http://www.cs.ucr.edu/~vbz/resources/quatut.pdf - - const tr = m00 + m11 + m22; - if (tr >= 0) { - s = Math.sqrt(tr + 1); - this.w = s * 0.5; - s = 0.5 / s; - this.x = (m12 - m21) * s; - this.y = (m20 - m02) * s; - this.z = (m01 - m10) * s; - } else { + const d = m.data; + + let m00 = d[0]; + let m01 = d[1]; + let m02 = d[2]; + let m10 = d[4]; + let m11 = d[5]; + let m12 = d[6]; + let m20 = d[8]; + let m21 = d[9]; + let m22 = d[10]; + + let l; + + // remove scaling from axis vectors + l = m00 * m00 + m01 * m01 + m02 * m02; + if (l === 0) return this.set(0, 0, 0, 1); + l = 1 / Math.sqrt(l); + m00 *= l; + m01 *= l; + m02 *= l; + + l = m10 * m10 + m11 * m11 + m12 * m12; + if (l === 0) return this.set(0, 0, 0, 1); + l = 1 / Math.sqrt(l); + m10 *= l; + m11 *= l; + m12 *= l; + + l = m20 * m20 + m21 * m21 + m22 * m22; + if (l === 0) return this.set(0, 0, 0, 1); + l = 1 / Math.sqrt(l); + m20 *= l; + m21 *= l; + m22 *= l; + + // https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf + + if (m22 < 0) { if (m00 > m11) { - if (m00 > m22) { - // XDiagDomMatrix - rs = (m00 - (m11 + m22)) + 1; - rs = Math.sqrt(rs); - - this.x = rs * 0.5; - rs = 0.5 / rs; - this.w = (m12 - m21) * rs; - this.y = (m01 + m10) * rs; - this.z = (m02 + m20) * rs; - } else { - // ZDiagDomMatrix - rs = (m22 - (m00 + m11)) + 1; - rs = Math.sqrt(rs); - - this.z = rs * 0.5; - rs = 0.5 / rs; - this.w = (m01 - m10) * rs; - this.x = (m20 + m02) * rs; - this.y = (m21 + m12) * rs; - } - } else if (m11 > m22) { - // YDiagDomMatrix - rs = (m11 - (m22 + m00)) + 1; - rs = Math.sqrt(rs); - - this.y = rs * 0.5; - rs = 0.5 / rs; - this.w = (m20 - m02) * rs; - this.z = (m12 + m21) * rs; - this.x = (m10 + m01) * rs; + this.set(1 + m00 - m11 - m22, m01 + m10, m20 + m02, m12 - m21); } else { - // ZDiagDomMatrix - rs = (m22 - (m00 + m11)) + 1; - rs = Math.sqrt(rs); - - this.z = rs * 0.5; - rs = 0.5 / rs; - this.w = (m01 - m10) * rs; - this.x = (m20 + m02) * rs; - this.y = (m21 + m12) * rs; + this.set(m01 + m10, 1 - m00 + m11 - m22, m12 + m21, m20 - m02); + } + } else { + if (m00 < -m11) { + this.set(m20 + m02, m12 + m21, 1 - m00 - m11 + m22, m01 - m10); + } else { + this.set(m12 - m21, m20 - m02, m01 - m10, 1 + m00 + m11 + m22); } } - return this; + return this.mulScalar(1.0 / this.length()); } /** diff --git a/test/core/math/quat.test.mjs b/test/core/math/quat.test.mjs index 43c864ff591..bd3cb3385f7 100644 --- a/test/core/math/quat.test.mjs +++ b/test/core/math/quat.test.mjs @@ -543,6 +543,32 @@ describe('Quat', function () { expect(q.equals(Quat.IDENTITY)).to.be.true; }); + const nq = new Quat(); + const q = new Quat(); + const m = new Mat4(); + + const quatToMatToQuat = (w, x, y, z, epsilon = 1e-6) => { + nq.set(x, y, z, w).normalize(); + m.setTRS(Vec3.ZERO, nq, Vec3.ONE); + q.setFromMat4(m); + const result = + (Math.abs(nq.x - q.x) < epsilon && + Math.abs(nq.y - q.y) < epsilon && + Math.abs(nq.z - q.z) < epsilon && + Math.abs(nq.w - q.w) < epsilon) || + (Math.abs(nq.x + q.x) < epsilon && + Math.abs(nq.y + q.y) < epsilon && + Math.abs(nq.z + q.z) < epsilon && + Math.abs(nq.w + q.w) < epsilon); + + if (!result) { + // helpful for debugging + console.log(`Failed Quat [${x}, ${y}, ${z}, ${w}] -> [${nq.x}, ${nq.y}, ${nq.z}, ${nq.w}] != [${q.x}, ${q.y}, ${q.z}, ${q.w}]`); + } + + return result; + }; + it('set the quaternion from a non-identity matrix', function () { const q = new Quat(); const m = new Mat4(); @@ -554,6 +580,103 @@ describe('Quat', function () { expect(eulers.z).to.be.closeTo(30, 0.00001); }); + // tests taken from Blender: + // https://github.com/blender/blender/blob/main/source/blender/blenlib/tests/BLI_math_rotation_test.cc + + it('converts rot180', function () { + expect(quatToMatToQuat(1, 0, 0, 0)).to.be.true; + expect(quatToMatToQuat(0, 1, 0, 0)).to.be.true; + expect(quatToMatToQuat(0, 0, 1, 0)).to.be.true; + expect(quatToMatToQuat(0, 0, 0, 1)).to.be.true; + }); + + it('converts rot180n', function () { + expect(quatToMatToQuat(-1, 0, 0, 0)).to.be.true; + expect(quatToMatToQuat(-1e-20, -1, 0, 0)).to.be.true; + expect(quatToMatToQuat(-1e-20, 0, -1, 0)).to.be.true; + expect(quatToMatToQuat(-1e-20, 0, 0, -1)).to.be.true; + }); + + const s2 = 1 / Math.sqrt(2); + + it('converts rot90', function () { + expect(quatToMatToQuat(s2, s2, 0, 0 )).to.be.true; + expect(quatToMatToQuat(s2, -s2, 0, 0)).to.be.true; + expect(quatToMatToQuat(s2, 0, s2, 0 )).to.be.true; + expect(quatToMatToQuat(s2, 0, -s2, 0)).to.be.true; + expect(quatToMatToQuat(s2, 0, 0, s2 )).to.be.true; + expect(quatToMatToQuat(s2, 0, 0, -s2)).to.be.true; + }); + + it('converts rot90n', function () { + expect(quatToMatToQuat(-s2, s2, 0, 0 )).to.be.true; + expect(quatToMatToQuat(-s2, -s2, 0, 0)).to.be.true; + expect(quatToMatToQuat(-s2, 0, s2, 0 )).to.be.true; + expect(quatToMatToQuat(-s2, 0, -s2, 0)).to.be.true; + expect(quatToMatToQuat(-s2, 0, 0, s2 )).to.be.true; + expect(quatToMatToQuat(-s2, 0, 0, -s2)).to.be.true; + }); + + it('converts bad_T83196', function () { + expect(quatToMatToQuat(0.0032, 0.9999, -0.0072, -0.0100)).to.be.true; + expect(quatToMatToQuat(0.0058, 0.9999, -0.0090, -0.0101)).to.be.true; + expect(quatToMatToQuat(0.0110, 0.9998, -0.0140, -0.0104)).to.be.true; + expect(quatToMatToQuat(0.0142, 0.9997, -0.0192, -0.0107)).to.be.true; + expect(quatToMatToQuat(0.0149, 0.9996, -0.0212, -0.0107)).to.be.true; + }); + + it('converts near_1000', function () { + expect(quatToMatToQuat(0.9999, 0.01, -0.001, -0.01)).to.be.true; + expect(quatToMatToQuat(0.9999, 0.02, -0.002, -0.02)).to.be.true; + expect(quatToMatToQuat(0.9999, 0.03, -0.003, -0.03)).to.be.true; + expect(quatToMatToQuat(0.9999, 0.04, -0.004, -0.04)).to.be.true; + expect(quatToMatToQuat(0.9999, 0.05, -0.005, -0.05)).to.be.true; + expect(quatToMatToQuat(0.999, 0.10, -0.010, -0.10)).to.be.true; + expect(quatToMatToQuat(0.99, 0.15, -0.015, -0.15)).to.be.true; + expect(quatToMatToQuat(0.98, 0.20, -0.020, -0.20)).to.be.true; + expect(quatToMatToQuat(0.97, 0.25, -0.025, -0.25)).to.be.true; + expect(quatToMatToQuat(0.95, 0.30, -0.030, -0.30)).to.be.true; + }); + + it('converts near_0100', function () { + expect(quatToMatToQuat(0.01, 0.9999, -0.001, -0.01)).to.be.true; + expect(quatToMatToQuat(0.02, 0.9999, -0.002, -0.02)).to.be.true; + expect(quatToMatToQuat(0.03, 0.9999, -0.003, -0.03)).to.be.true; + expect(quatToMatToQuat(0.04, 0.9999, -0.004, -0.04)).to.be.true; + expect(quatToMatToQuat(0.05, 0.9999, -0.005, -0.05)).to.be.true; + expect(quatToMatToQuat(0.10, 0.999, -0.010, -0.10)).to.be.true; + expect(quatToMatToQuat(0.15, 0.99, -0.015, -0.15)).to.be.true; + expect(quatToMatToQuat(0.20, 0.98, -0.020, -0.20)).to.be.true; + expect(quatToMatToQuat(0.25, 0.97, -0.025, -0.25)).to.be.true; + expect(quatToMatToQuat(0.30, 0.95, -0.030, -0.30)).to.be.true; + }); + + it('converts near_0010', function () { + expect(quatToMatToQuat(0.01, -0.001, 0.9999, -0.01)).to.be.true; + expect(quatToMatToQuat(0.02, -0.002, 0.9999, -0.02)).to.be.true; + expect(quatToMatToQuat(0.03, -0.003, 0.9999, -0.03)).to.be.true; + expect(quatToMatToQuat(0.04, -0.004, 0.9999, -0.04)).to.be.true; + expect(quatToMatToQuat(0.05, -0.005, 0.9999, -0.05)).to.be.true; + expect(quatToMatToQuat(0.10, -0.010, 0.999, -0.10)).to.be.true; + expect(quatToMatToQuat(0.15, -0.015, 0.99, -0.15)).to.be.true; + expect(quatToMatToQuat(0.20, -0.020, 0.98, -0.20)).to.be.true; + expect(quatToMatToQuat(0.25, -0.025, 0.97, -0.25)).to.be.true; + expect(quatToMatToQuat(0.30, -0.030, 0.95, -0.30)).to.be.true; + }); + + it('converts near_0001', function () { + expect(quatToMatToQuat(0.01, -0.001, -0.01, 0.9999)).to.be.true; + expect(quatToMatToQuat(0.02, -0.002, -0.02, 0.9999)).to.be.true; + expect(quatToMatToQuat(0.03, -0.003, -0.03, 0.9999)).to.be.true; + expect(quatToMatToQuat(0.04, -0.004, -0.04, 0.9999)).to.be.true; + expect(quatToMatToQuat(0.05, -0.005, -0.05, 0.9999)).to.be.true; + expect(quatToMatToQuat(0.10, -0.010, -0.10, 0.999)).to.be.true; + expect(quatToMatToQuat(0.15, -0.015, -0.15, 0.99)).to.be.true; + expect(quatToMatToQuat(0.20, -0.020, -0.20, 0.98)).to.be.true; + expect(quatToMatToQuat(0.25, -0.025, -0.25, 0.97)).to.be.true; + expect(quatToMatToQuat(0.30, -0.030, -0.30, 0.95)).to.be.true; + }); + it('returns this', function () { const q = new Quat(); const m = new Mat4(); From c68bff2c2fc2fcf51d5cb4265db22e38eed0499b Mon Sep 17 00:00:00 2001 From: Donovan Hutchence Date: Wed, 19 Jun 2024 12:37:05 +0100 Subject: [PATCH 2/5] update tests --- test/core/math/quat.test.mjs | 70 +++++++----------------------------- 1 file changed, 12 insertions(+), 58 deletions(-) diff --git a/test/core/math/quat.test.mjs b/test/core/math/quat.test.mjs index bd3cb3385f7..34cadd87f7d 100644 --- a/test/core/math/quat.test.mjs +++ b/test/core/math/quat.test.mjs @@ -617,64 +617,18 @@ describe('Quat', function () { expect(quatToMatToQuat(-s2, 0, 0, -s2)).to.be.true; }); - it('converts bad_T83196', function () { - expect(quatToMatToQuat(0.0032, 0.9999, -0.0072, -0.0100)).to.be.true; - expect(quatToMatToQuat(0.0058, 0.9999, -0.0090, -0.0101)).to.be.true; - expect(quatToMatToQuat(0.0110, 0.9998, -0.0140, -0.0104)).to.be.true; - expect(quatToMatToQuat(0.0142, 0.9997, -0.0192, -0.0107)).to.be.true; - expect(quatToMatToQuat(0.0149, 0.9996, -0.0212, -0.0107)).to.be.true; - }); - - it('converts near_1000', function () { - expect(quatToMatToQuat(0.9999, 0.01, -0.001, -0.01)).to.be.true; - expect(quatToMatToQuat(0.9999, 0.02, -0.002, -0.02)).to.be.true; - expect(quatToMatToQuat(0.9999, 0.03, -0.003, -0.03)).to.be.true; - expect(quatToMatToQuat(0.9999, 0.04, -0.004, -0.04)).to.be.true; - expect(quatToMatToQuat(0.9999, 0.05, -0.005, -0.05)).to.be.true; - expect(quatToMatToQuat(0.999, 0.10, -0.010, -0.10)).to.be.true; - expect(quatToMatToQuat(0.99, 0.15, -0.015, -0.15)).to.be.true; - expect(quatToMatToQuat(0.98, 0.20, -0.020, -0.20)).to.be.true; - expect(quatToMatToQuat(0.97, 0.25, -0.025, -0.25)).to.be.true; - expect(quatToMatToQuat(0.95, 0.30, -0.030, -0.30)).to.be.true; - }); - - it('converts near_0100', function () { - expect(quatToMatToQuat(0.01, 0.9999, -0.001, -0.01)).to.be.true; - expect(quatToMatToQuat(0.02, 0.9999, -0.002, -0.02)).to.be.true; - expect(quatToMatToQuat(0.03, 0.9999, -0.003, -0.03)).to.be.true; - expect(quatToMatToQuat(0.04, 0.9999, -0.004, -0.04)).to.be.true; - expect(quatToMatToQuat(0.05, 0.9999, -0.005, -0.05)).to.be.true; - expect(quatToMatToQuat(0.10, 0.999, -0.010, -0.10)).to.be.true; - expect(quatToMatToQuat(0.15, 0.99, -0.015, -0.15)).to.be.true; - expect(quatToMatToQuat(0.20, 0.98, -0.020, -0.20)).to.be.true; - expect(quatToMatToQuat(0.25, 0.97, -0.025, -0.25)).to.be.true; - expect(quatToMatToQuat(0.30, 0.95, -0.030, -0.30)).to.be.true; - }); - - it('converts near_0010', function () { - expect(quatToMatToQuat(0.01, -0.001, 0.9999, -0.01)).to.be.true; - expect(quatToMatToQuat(0.02, -0.002, 0.9999, -0.02)).to.be.true; - expect(quatToMatToQuat(0.03, -0.003, 0.9999, -0.03)).to.be.true; - expect(quatToMatToQuat(0.04, -0.004, 0.9999, -0.04)).to.be.true; - expect(quatToMatToQuat(0.05, -0.005, 0.9999, -0.05)).to.be.true; - expect(quatToMatToQuat(0.10, -0.010, 0.999, -0.10)).to.be.true; - expect(quatToMatToQuat(0.15, -0.015, 0.99, -0.15)).to.be.true; - expect(quatToMatToQuat(0.20, -0.020, 0.98, -0.20)).to.be.true; - expect(quatToMatToQuat(0.25, -0.025, 0.97, -0.25)).to.be.true; - expect(quatToMatToQuat(0.30, -0.030, 0.95, -0.30)).to.be.true; - }); - - it('converts near_0001', function () { - expect(quatToMatToQuat(0.01, -0.001, -0.01, 0.9999)).to.be.true; - expect(quatToMatToQuat(0.02, -0.002, -0.02, 0.9999)).to.be.true; - expect(quatToMatToQuat(0.03, -0.003, -0.03, 0.9999)).to.be.true; - expect(quatToMatToQuat(0.04, -0.004, -0.04, 0.9999)).to.be.true; - expect(quatToMatToQuat(0.05, -0.005, -0.05, 0.9999)).to.be.true; - expect(quatToMatToQuat(0.10, -0.010, -0.10, 0.999)).to.be.true; - expect(quatToMatToQuat(0.15, -0.015, -0.15, 0.99)).to.be.true; - expect(quatToMatToQuat(0.20, -0.020, -0.20, 0.98)).to.be.true; - expect(quatToMatToQuat(0.25, -0.025, -0.25, 0.97)).to.be.true; - expect(quatToMatToQuat(0.30, -0.030, -0.30, 0.95)).to.be.true; + it ('converts suit', function () { + const vals = [0.9999, -0.002, -0.999, 0.01, 0, 1]; + + vals.forEach((x) => { + vals.forEach((y) => { + vals.forEach((z) => { + vals.forEach((w) => { + expect(quatToMatToQuat(w, x, y, z)).to.be.true; + }); + }); + }); + }); }); it('returns this', function () { From 7048c6dc660c10a904a2cc93aeeb2b6a27b9b859 Mon Sep 17 00:00:00 2001 From: Donovan Hutchence Date: Wed, 19 Jun 2024 12:38:04 +0100 Subject: [PATCH 3/5] update tests --- test/core/math/quat.test.mjs | 3 --- 1 file changed, 3 deletions(-) diff --git a/test/core/math/quat.test.mjs b/test/core/math/quat.test.mjs index 34cadd87f7d..8c03b836f76 100644 --- a/test/core/math/quat.test.mjs +++ b/test/core/math/quat.test.mjs @@ -580,9 +580,6 @@ describe('Quat', function () { expect(eulers.z).to.be.closeTo(30, 0.00001); }); - // tests taken from Blender: - // https://github.com/blender/blender/blob/main/source/blender/blenlib/tests/BLI_math_rotation_test.cc - it('converts rot180', function () { expect(quatToMatToQuat(1, 0, 0, 0)).to.be.true; expect(quatToMatToQuat(0, 1, 0, 0)).to.be.true; From 9be81b504ba36ba8e6f47e8edc5271a6f594771f Mon Sep 17 00:00:00 2001 From: Donovan Hutchence Date: Wed, 19 Jun 2024 12:42:42 +0100 Subject: [PATCH 4/5] lint --- test/core/math/quat.test.mjs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/test/core/math/quat.test.mjs b/test/core/math/quat.test.mjs index 8c03b836f76..4f57546131e 100644 --- a/test/core/math/quat.test.mjs +++ b/test/core/math/quat.test.mjs @@ -597,24 +597,24 @@ describe('Quat', function () { const s2 = 1 / Math.sqrt(2); it('converts rot90', function () { - expect(quatToMatToQuat(s2, s2, 0, 0 )).to.be.true; + expect(quatToMatToQuat(s2, s2, 0, 0)).to.be.true; expect(quatToMatToQuat(s2, -s2, 0, 0)).to.be.true; - expect(quatToMatToQuat(s2, 0, s2, 0 )).to.be.true; + expect(quatToMatToQuat(s2, 0, s2, 0)).to.be.true; expect(quatToMatToQuat(s2, 0, -s2, 0)).to.be.true; - expect(quatToMatToQuat(s2, 0, 0, s2 )).to.be.true; + expect(quatToMatToQuat(s2, 0, 0, s2)).to.be.true; expect(quatToMatToQuat(s2, 0, 0, -s2)).to.be.true; }); it('converts rot90n', function () { - expect(quatToMatToQuat(-s2, s2, 0, 0 )).to.be.true; + expect(quatToMatToQuat(-s2, s2, 0, 0)).to.be.true; expect(quatToMatToQuat(-s2, -s2, 0, 0)).to.be.true; - expect(quatToMatToQuat(-s2, 0, s2, 0 )).to.be.true; + expect(quatToMatToQuat(-s2, 0, s2, 0)).to.be.true; expect(quatToMatToQuat(-s2, 0, -s2, 0)).to.be.true; - expect(quatToMatToQuat(-s2, 0, 0, s2 )).to.be.true; + expect(quatToMatToQuat(-s2, 0, 0, s2)).to.be.true; expect(quatToMatToQuat(-s2, 0, 0, -s2)).to.be.true; }); - it ('converts suit', function () { + it('converts suit', function () { const vals = [0.9999, -0.002, -0.999, 0.01, 0, 1]; vals.forEach((x) => { From 97533033296b2fa01ff77072d208a2432c4e2041 Mon Sep 17 00:00:00 2001 From: Donovan Hutchence Date: Wed, 19 Jun 2024 12:53:44 +0100 Subject: [PATCH 5/5] add comment --- src/core/math/quat.js | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/core/math/quat.js b/src/core/math/quat.js index 18ed6db6b47..e0d3570cde5 100644 --- a/src/core/math/quat.js +++ b/src/core/math/quat.js @@ -528,6 +528,10 @@ class Quat { } } + // Instead of scaling by 0.5 / Math.sqrt(t) (to match the original implementation), + // instead we normalize the result. It costs 3 more adds and muls, but we get + // a stable result and in some cases normalization is required anyway, see + // https://github.com/blender/blender/blob/v4.1.1/source/blender/blenlib/intern/math_rotation.c#L368 return this.mulScalar(1.0 / this.length()); }