Created slightly faster version of multiplying an imaginary quat with a quat (#1665)

.github/workflows/determinism_check.yml

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

Jolt/Math/Quat.h

@@ -159,6 +159,9 @@ public:
 	/// Rotate a vector by this quaternion
 	JPH_INLINE Vec3				operator * (Vec3Arg inValue) const;
 
+	/// Multiply a quaternion with imaginary components and no real component (x, y, z, 0) with a quaternion
+	static JPH_INLINE Quat		sMultiplyImaginary(Vec3Arg inLHS, QuatArg inRHS);
+
 	/// Rotate a vector by the inverse of this quaternion
 	JPH_INLINE Vec3				InverseRotate(Vec3Arg inValue) const;
 

Jolt/Math/Quat.inl

@@ -71,6 +71,60 @@ Quat Quat::operator * (QuatArg inRHS) const
 #endif
 }
 
+Quat Quat::sMultiplyImaginary(Vec3Arg inLHS, QuatArg inRHS)
+{
+#if defined(JPH_USE_SSE4_1)
+	__m128 abc0 = inLHS.mValue;
+	__m128 xyzw = inRHS.mValue.mValue;
+
+	// [a,a,a,a] * [w,y,z,x] = [aw,ay,az,ax]
+	__m128 aaaa = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 xzyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 2, 0));
+	__m128 axazayaw = _mm_mul_ps(aaaa, xzyw);
+
+	// [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by]
+	__m128 bbbb = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 ywxz = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1));
+	__m128 bybwbxbz = _mm_mul_ps(bbbb, ywxz);
+
+	// [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy]
+	__m128 cccc = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(2, 2, 2, 2));
+	__m128 yxzw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1));
+	__m128 cycxczcw = _mm_mul_ps(cccc, yxzw);
+
+	// [+aw,+ay,-az,-ax]
+	__m128 e = _mm_xor_ps(axazayaw, _mm_set_ps(0.0f, 0.0f, -0.0f, -0.0f));
+
+	// [+aw,+ay,-az,-ax] + -[bz,bx,bw,by] = [+aw+bz,+ay-bx,-az+bw,-ax-by]
+	e = _mm_addsub_ps(e, bybwbxbz);
+
+	// [+ay-bx,-ax-by,-az+bw,+aw+bz]
+	e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3));
+
+	// [+ay-bx,-ax-by,-az+bw,+aw+bz] + -[cw,cz,cx,cy] = [+ay-bx+cw,-ax-by-cz,-az+bw+cx,+aw+bz-cy]
+	e = _mm_addsub_ps(e, cycxczcw);
+
+	// [-ax-by-cz,+ay-bx+cw,-az+bw+cx,+aw+bz-cy]
+	return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0))));
+#else
+	float lx = inLHS.GetX();
+	float ly = inLHS.GetY();
+	float lz = inLHS.GetZ();
+
+	float rx = inRHS.mValue.GetX();
+	float ry = inRHS.mValue.GetY();
+	float rz = inRHS.mValue.GetZ();
+	float rw = inRHS.mValue.GetW();
+
+	float x =  (lx * rw) + ly * rz - lz * ry;
+	float y = -(lx * rz) + ly * rw + lz * rx;
+	float z =  (lx * ry) - ly * rx + lz * rw;
+	float w = -(lx * rx) - ly * ry - lz * rz;
+
+	return Quat(x, y, z, w);
+#endif
+}
+
 Quat Quat::sRotation(Vec3Arg inAxis, float inAngle)
 {
 	// returns [inAxis * sin(0.5f * inAngle), cos(0.5f * inAngle)]
@@ -276,13 +330,13 @@ 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)
 	JPH_ASSERT(IsNormalized());
-	return Vec3((*this * Quat(Vec4(inValue, 0)) * Conjugated()).mValue);
+	return Vec3((*this * Quat::sMultiplyImaginary(inValue, Conjugated())).mValue);
 }
 
 Vec3 Quat::InverseRotate(Vec3Arg inValue) const
 {
 	JPH_ASSERT(IsNormalized());
-	return Vec3((Conjugated() * Quat(Vec4(inValue, 0)) * *this).mValue);
+	return Vec3((Conjugated() * Quat::sMultiplyImaginary(inValue, *this)).mValue);
 }
 
 Vec3 Quat::RotateAxisX() const

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -359,7 +359,7 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i
 
 		// Integrate
 		Quat rotation = r.mRotation;
-		Quat delta_rotation = half_dt * Quat(Vec4(r.mAngularVelocity, 0)) * rotation;
+		Quat delta_rotation = half_dt * Quat::sMultiplyImaginary(r.mAngularVelocity, rotation);
 		r.mPreviousRotationInternal = rotation; // Overwrites mAngularVelocity
 		r.mRotation = (rotation + delta_rotation).Normalized();
 	}
@@ -627,8 +627,8 @@ void SoftBodyMotionProperties::ApplyRodStretchShearConstraints(const SoftBodyUpd
 		v0.mPosition = x0 + v0.mInvMass * delta;
 		v1.mPosition = x1 - v1.mInvMass * delta;
 		// q * e3_bar = q * (0, 0, -1, 0) = [-qy, qx, -qw, qz]
-		Quat q_e3_bar(UVec4::sXor(rotation.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().ReinterpretAsInt(), UVec4(0x80000000u, 0, 0x80000000u, 0)).ReinterpretAsFloat());
-		rotation += (2.0f * r->mInvMass * r->mLength) * Quat(Vec4(delta, 0)) * q_e3_bar;
+		Quat q_e3_bar(Vec4::sXor(rotation.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(), Vec4(-0.0f, 0.0f, -0.0f, 0.0f)));
+		rotation += (2.0f * r->mInvMass * r->mLength) * Quat::sMultiplyImaginary(delta, q_e3_bar);
 
 		// Renormalize
 		rod_state->mRotation = rotation.Normalized();

UnitTests/Math/QuatTests.cpp

@@ -486,4 +486,18 @@ TEST_SUITE("QuatTests")
 		Quat v3 = Quat(1, 2, 3, 4).Normalized();
 		CHECK_APPROX_EQUAL(v3.SLERP(-v3, 0.5f), v3);
 	}
+
+	TEST_CASE("TestQuatMultiplyImaginary")
+	{
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 imaginary = Vec3::sRandom(random);
+			Quat quat = Quat::sRandom(random);
+
+			Quat r1 = Quat::sMultiplyImaginary(imaginary, quat);
+			Quat r2 = Quat(Vec4(imaginary, 0)) * quat;
+			CHECK_APPROX_EQUAL(r1, r2);
+		}
+	}
 }