Optimized Quat::RotateAxisX/Y/Z (#1670)

Added Vec3/Vec4::FlipSign

Jolt/Math/Mat44.inl

@@ -838,18 +838,18 @@ Quat Mat44::GetQuaternion() const
 Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ)
 {
 	return Mat44(
-		Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>(),
-		Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>(),
-		Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(),
+		inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().FlipSign<1, 1, -1, -1>(),
+		inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>().FlipSign<-1, 1, 1, -1>(),
+		inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<1, -1, 1, -1>(),
 		inQ.mValue);
 }
 
 Mat44 Mat44::sQuatRightMultiply(QuatArg inQ)
 {
 	return Mat44(
-		Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>(),
-		Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>(),
-		Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(),
+		inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().FlipSign<1, -1, 1, -1>(),
+		inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>().FlipSign<1, 1, -1, -1>(),
+		inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<-1, 1, 1, -1>(),
 		inQ.mValue);
 }
 

Jolt/Math/Quat.h

@@ -178,7 +178,7 @@ public:
 	JPH_INLINE float			Dot(QuatArg inRHS) const										{ return mValue.Dot(inRHS.mValue); }
 
 	/// The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions
-	JPH_INLINE Quat				Conjugated() const												{ return Quat(Vec4::sXor(mValue, UVec4(0x80000000, 0x80000000, 0x80000000, 0).ReinterpretAsFloat())); }
+	JPH_INLINE Quat				Conjugated() const												{ return Quat(mValue.FlipSign<-1, -1, -1, 1>()); }
 
 	/// Get inverse quaternion
 	JPH_INLINE Quat				Inversed() const												{ return Conjugated() / Length(); }
@@ -187,7 +187,7 @@ public:
 	JPH_INLINE Quat				EnsureWPositive() const											{ return Quat(Vec4::sXor(mValue, Vec4::sAnd(mValue.SplatW(), UVec4::sReplicate(0x80000000).ReinterpretAsFloat()))); }
 
 	/// Get a quaternion that is perpendicular to this quaternion
-	JPH_INLINE Quat				GetPerpendicular() const										{ return Quat(Vec4(1, -1, 1, -1) * mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>()); }
+	JPH_INLINE Quat				GetPerpendicular() const										{ return Quat(mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<1, -1, 1, -1>()); }
 
 	/// Get rotation angle around inAxis (uses Swing Twist Decomposition to get the twist quaternion and uses q(axis, angle) = [cos(angle / 2), axis * sin(angle / 2)])
 	JPH_INLINE float			GetRotationAngle(Vec3Arg inAxis) const							{ return GetW() == 0.0f? JPH_PI : 2.0f * ATan(GetXYZ().Dot(inAxis) / GetW()); }

Jolt/Math/Quat.inl

@@ -365,27 +365,24 @@ Vec3 Quat::RotateAxisX() const
 {
 	// This is *this * Vec3::sAxisX() written out:
 	JPH_ASSERT(IsNormalized());
-	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
-	float tx = 2.0f * x, tw = 2.0f * w;
-	return Vec3(tx * x + tw * w - 1.0f, tx * y + z * tw, tx * z - y * tw);
+	Vec4 t = mValue + mValue;
+	return Vec3(t.SplatX() * mValue + (t.SplatW() * mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>()).FlipSign<1, 1, -1, 1>() - Vec4(1, 0, 0, 0));
 }
 
 Vec3 Quat::RotateAxisY() const
 {
 	// This is *this * Vec3::sAxisY() written out:
 	JPH_ASSERT(IsNormalized());
-	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
-	float ty = 2.0f * y, tw = 2.0f * w;
-	return Vec3(x * ty - z * tw, tw * w + ty * y - 1.0f, x * tw + ty * z);
+	Vec4 t = mValue + mValue;
+	return Vec3(t.SplatY() * mValue + (t.SplatW() * mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>()).FlipSign<-1, 1, 1, 1>() - Vec4(0, 1, 0, 0));
 }
 
 Vec3 Quat::RotateAxisZ() const
 {
 	// This is *this * Vec3::sAxisZ() written out:
 	JPH_ASSERT(IsNormalized());
-	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
-	float tz = 2.0f * z, tw = 2.0f * w;
-	return Vec3(x * tz + y * tw, y * tz - x * tw, tw * w + tz * z - 1.0f);
+	Vec4 t = mValue + mValue;
+	return Vec3(t.SplatZ() * mValue + (t.SplatW() * mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>()).FlipSign<1, -1, 1, 1>() - Vec4(0, 0, 1, 0));
 }
 
 void Quat::StoreFloat3(Float3 *outV) const

Jolt/Math/Vec3.h

@@ -271,6 +271,10 @@ public:
 	/// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative)
 	JPH_INLINE Vec3				GetSign() const;
 
+	/// Flips the signs of the components, e.g. FlipSign<-1, 1, -1>() will flip the signs of the X and Z components
+	template <int X, int Y, int Z>
+	JPH_INLINE Vec3				FlipSign() const;
+
 	/// To String
 	friend ostream &			operator << (ostream &inStream, Vec3Arg inV)
 	{

Jolt/Math/Vec3.inl

@@ -857,4 +857,13 @@ Vec3 Vec3::GetSign() const
 #endif
 }
 
+template <int X, int Y, int Z>
+JPH_INLINE Vec3 Vec3::FlipSign() const
+{
+	static_assert(X == 1 || X == -1, "X must be 1 or -1");
+	static_assert(Y == 1 || Y == -1, "Y must be 1 or -1");
+	static_assert(Z == 1 || Z == -1, "Z must be 1 or -1");
+	return Vec3::sXor(*this, Vec3(X > 0? 0.0f : -0.0f, Y > 0? 0.0f : -0.0f, Z > 0? 0.0f : -0.0f));
+}
+
 JPH_NAMESPACE_END

Jolt/Math/Vec4.h

@@ -260,6 +260,10 @@ public:
 	/// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative)
 	JPH_INLINE Vec4				GetSign() const;
 
+	/// Flips the signs of the components, e.g. FlipSign<-1, 1, -1, 1>() will flip the signs of the X and Z components
+	template <int X, int Y, int Z, int W>
+	JPH_INLINE Vec4				FlipSign() const;
+
 	/// Calculate the sine and cosine for each element of this vector (input in radians)
 	inline void					SinCos(Vec4 &outSin, Vec4 &outCos) const;
 

Jolt/Math/Vec4.inl

@@ -756,6 +756,16 @@ Vec4 Vec4::GetSign() const
 #endif
 }
 
+template <int X, int Y, int Z, int W>
+JPH_INLINE Vec4 Vec4::FlipSign() const
+{
+	static_assert(X == 1 || X == -1, "X must be 1 or -1");
+	static_assert(Y == 1 || Y == -1, "Y must be 1 or -1");
+	static_assert(Z == 1 || Z == -1, "Z must be 1 or -1");
+	static_assert(W == 1 || W == -1, "W must be 1 or -1");
+	return Vec4::sXor(*this, Vec4(X > 0? 0.0f : -0.0f, Y > 0? 0.0f : -0.0f, Z > 0? 0.0f : -0.0f, W > 0? 0.0f : -0.0f));
+}
+
 Vec4 Vec4::Normalized() const
 {
 #if defined(JPH_USE_SSE4_1)

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -381,7 +381,7 @@ void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMa
 	}
 
 	// Preserve flip along y axis but make sure we're not inside out
-	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale;
 	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
 
 	AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);

Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp

@@ -536,7 +536,7 @@ void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, con
 	JPH_ASSERT(IsAligned(&ioContext, alignof(TCSGetTrianglesContext)));
 
 	// Make sure the scale is not inside out
-	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale;
 
 	// Mark top and bottom processed if their radius is too small
 	TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale));
@@ -645,7 +645,7 @@ int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int i
 void TaperedCylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	// Preserve flip along y axis but make sure we're not inside out
-	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale;
 	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
 
 	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -627,7 +627,7 @@ 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(Vec4::sXor(rotation.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(), Vec4(-0.0f, 0.0f, -0.0f, 0.0f)));
+		Quat q_e3_bar(rotation.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<-1, 1, -1, 1>());
 		rotation += (2.0f * r->mInvMass * r->mLength) * Quat::sMultiplyImaginary(delta, q_e3_bar);
 
 		// Renormalize

UnitTests/Math/Vec3Tests.cpp

@@ -318,6 +318,14 @@ TEST_SUITE("Vec3Tests")
 		CHECK(Vec3(0, 2.3456f, -7.8912f).GetSign() == Vec3(1, 1, -1));
 	}
 
+	TEST_CASE("TestVec3FlipSign")
+	{
+		Vec3 v(1, 2, 3);
+		CHECK(v.FlipSign<-1, 1, 1>() == Vec3(-1, 2, 3));
+		CHECK(v.FlipSign<1, -1, 1>() == Vec3(1, -2, 3));
+		CHECK(v.FlipSign<1, 1, -1>() == Vec3(1, 2, -3));
+	}
+
 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
 	TEST_CASE("TestVec3SyncW")
 	{

UnitTests/Math/Vec4Tests.cpp

@@ -500,6 +500,15 @@ TEST_SUITE("Vec4Tests")
 		CHECK(Vec4(0, 2.3456f, -7.8912f, -1).GetSign() == Vec4(1, 1, -1, -1));
 	}
 
+	TEST_CASE("TestVec4FlipSign")
+	{
+		Vec4 v(1, 2, 3, 4);
+		CHECK(v.FlipSign<-1, 1, 1, 1>() == Vec4(-1, 2, 3, 4));
+		CHECK(v.FlipSign<1, -1, 1, 1>() == Vec4(1, -2, 3, 4));
+		CHECK(v.FlipSign<1, 1, -1, 1>() == Vec4(1, 2, -3, 4));
+		CHECK(v.FlipSign<1, 1, 1, -1>() == Vec4(1, 2, 3, -4));
+	}
+
 	TEST_CASE("TestVec4SignBit")
 	{
 		CHECK(Vec4(2, -3, 4, -5).GetSignBits() == 0b1010);