diff --git a/src/core/math/quat.js b/src/core/math/quat.js index 8eb9d121aa9..e0d3570cde5 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,63 @@ 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; + // 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()); } /** diff --git a/test/core/math/quat.test.mjs b/test/core/math/quat.test.mjs index 43c864ff591..4f57546131e 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,54 @@ describe('Quat', function () { expect(eulers.z).to.be.closeTo(30, 0.00001); }); + 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 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 () { const q = new Quat(); const m = new Mat4();