Optimized Quat Vec3 multiplication (#1669)

.github/workflows/determinism_check.yml

@@ -1,9 +1,9 @@
 name: Determinism Check
 
 env:
-  CONVEX_VS_MESH_HASH: '0x66969cc13ca88dbd'
-  RAGDOLL_HASH: '0x57f5421d498e0582'
-  PYRAMID_HASH: '0x1843ae1a62ba21ea'
+  CONVEX_VS_MESH_HASH: '0x4e5ff3fefc2a35fb'
+  RAGDOLL_HASH: '0x5c61578b3cfa4cd0'
+  PYRAMID_HASH: '0xafd93b295e75e3f6'
   CHARACTER_VIRTUAL_HASH: '0x19c55223035a8f1a'
   EMSCRIPTEN_VERSION: 4.0.2
   NODE_VERSION: 23.x

Jolt/Math/Quat.inl

@@ -328,15 +328,37 @@ Quat Quat::SLERP(QuatArg inDestination, float inFraction) const
 
 Vec3 Quat::operator * (Vec3Arg inValue) const
 {
-	// Rotating a vector by a quaternion is done by: p' = q * p * q^-1 (q^-1 = conjugated(q) for a unit quaternion)
+	// Rotating a vector by a quaternion is done by: p' = q * (p, 0) * q^-1 (q^-1 = conjugated(q) for a unit quaternion)
+	// Using Rodrigues formula: https://en.m.wikipedia.org/wiki/Euler%E2%80%93Rodrigues_formula
+	// This is equivalent to: p' = p + 2 * (q.w * q.xyz x p + q.xyz x (q.xyz x p))
+	//
+	// This is:
+	//
+	// Vec3 xyz = GetXYZ();
+	// Vec3 q_cross_p = xyz.Cross(inValue);
+	// Vec3 q_cross_q_cross_p = xyz.Cross(q_cross_p);
+	// Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p;
+	// return inValue + (v + v);
+	//
+	// But we can write out the cross products in a more efficient way:
 	JPH_ASSERT(IsNormalized());
-	return Vec3((*this * Quat::sMultiplyImaginary(inValue, Conjugated())).mValue);
+	Vec3 xyz = GetXYZ();
+	Vec3 yzx = xyz.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+	Vec3 q_cross_p = (inValue.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz - yzx * inValue).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+	Vec3 q_cross_q_cross_p = (q_cross_p.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz - yzx * q_cross_p).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+	Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p;
+	return inValue + (v + v);
 }
 
 Vec3 Quat::InverseRotate(Vec3Arg inValue) const
 {
 	JPH_ASSERT(IsNormalized());
-	return Vec3((Conjugated() * Quat::sMultiplyImaginary(inValue, *this)).mValue);
+	Vec3 xyz = GetXYZ(); // Needs to be negated, but we do this in the equations below
+	Vec3 yzx = xyz.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+	Vec3 q_cross_p = (yzx * inValue - inValue.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+	Vec3 q_cross_q_cross_p = (yzx * q_cross_p - q_cross_p.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+	Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p;
+	return inValue + (v + v);
 }
 
 Vec3 Quat::RotateAxisX() const

Jolt/Math/Vec4.h

@@ -203,6 +203,18 @@ public:
 	/// Replicate the W component to all components
 	JPH_INLINE Vec4				SplatW() const;
 
+	/// Replicate the X component to all components
+	JPH_INLINE Vec3				SplatX3() const;
+
+	/// Replicate the Y component to all components
+	JPH_INLINE Vec3				SplatY3() const;
+
+	/// Replicate the Z component to all components
+	JPH_INLINE Vec3				SplatZ3() const;
+
+	/// Replicate the W component to all components
+	JPH_INLINE Vec3				SplatW3() const;
+
 	/// Return the absolute value of each of the components
 	JPH_INLINE Vec4				Abs() const;
 

Jolt/Math/Vec4.inl

@@ -609,6 +609,50 @@ Vec4 Vec4::SplatW() const
 #endif
 }
 
+Vec3 Vec4::SplatX3() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 0);
+#else
+	return Vec3(mF32[0], mF32[0], mF32[0]);
+#endif
+}
+
+Vec3 Vec4::SplatY3() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 1);
+#else
+	return Vec3(mF32[1], mF32[1], mF32[1]);
+#endif
+}
+
+Vec3 Vec4::SplatZ3() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 2);
+#else
+	return Vec3(mF32[2], mF32[2], mF32[2]);
+#endif
+}
+
+Vec3 Vec4::SplatW3() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(3, 3, 3, 3));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 3);
+#else
+	return Vec3(mF32[3], mF32[3], mF32[3]);
+#endif
+}
+
 Vec4 Vec4::Abs() const
 {
 #if defined(JPH_USE_AVX512)

UnitTests/Math/QuatTests.cpp

@@ -131,6 +131,9 @@ TEST_SUITE("QuatTests")
 			Vec3 r1 = m1 * rv;
 			Vec3 r2 = q1 * rv;
 			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
+
+			Vec3 r3 = q1.InverseRotate(r2);
+			CHECK_APPROX_EQUAL(r3, rv, 1.0e-5f);
 		}
 	}
 

UnitTests/Math/Vec4Tests.cpp

@@ -193,6 +193,11 @@ TEST_SUITE("Vec4Tests")
 		CHECK(v.SplatZ() == Vec4::sReplicate(3));
 		CHECK(v.SplatW() == Vec4::sReplicate(4));
 
+		CHECK(v.SplatX3() == Vec3::sReplicate(1));
+		CHECK(v.SplatY3() == Vec3::sReplicate(2));
+		CHECK(v.SplatZ3() == Vec3::sReplicate(3));
+		CHECK(v.SplatW3() == Vec3::sReplicate(4));
+
 		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == Vec4(1, 1, 1, 1));
 		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == Vec4(1, 1, 1, 2));
 		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == Vec4(1, 1, 1, 3));