Made shape casting on shapes a bit more intuitive (#116)

* Renamed CollisionDispatch::sCastShapeVsShape to sCastShapeVsShapeLocalSpace
* Added CollisionDispatch::sCastShapeVsShapeWorldSpace
* Added CastShape::sFromWorldTransform
* Added Mat44::PreTranslated/PostTranslated helpers and used them in various places

Jolt/Geometry/OrientedBox.h

@@ -21,8 +21,7 @@ public:
 					OrientedBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents)			: mOrientation(inOrientation), mHalfExtents(inHalfExtents) { }
 
 	/// Construct from axis aligned box and transform. Only works for rotation/translation matrix (no scaling / shearing).
-					OrientedBox(Mat44Arg inOrientation, const AABox &inBox)				: OrientedBox(inOrientation * Mat44::sTranslation(inBox.GetCenter()), inBox.GetExtent()) { }
-
+					OrientedBox(Mat44Arg inOrientation, const AABox &inBox)				: OrientedBox(inOrientation.PreTranslated(inBox.GetCenter()), inBox.GetExtent()) { }
 
 	/// Test if oriented boxe overlaps with axis aligned box eachother
 	bool			Overlaps(const AABox &inBox, float inEpsilon = 1.0e-6f) const;

Jolt/Math/Mat44.h

@@ -182,6 +182,12 @@ public:
 	/// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved)
 	JPH_INLINE const Mat44		GetDirectionPreservingMatrix() const					{ return GetRotation().Inversed3x3().Transposed3x3(); }
 
+	/// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
+	JPH_INLINE Mat44			PreTranslated(Vec3Arg inTranslation) const;
+
+	/// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column)
+	JPH_INLINE Mat44			PostTranslated(Vec3Arg inTranslation) const;
+
 	/// Scale a matrix: result = this * Mat44::sScale(inScale)
 	JPH_INLINE Mat44			PreScaled(Vec3Arg inScale) const;
 

Jolt/Math/Mat44.inl

@@ -1052,6 +1052,16 @@ void Mat44::SetRotation(Mat44Arg inRotation)
 	mCol[2] = inRotation.mCol[2];
 }
 
+Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const
+{
+	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1));
+}
+
+Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const
+{
+	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1));
+}
+
 Mat44 Mat44::PreScaled(Vec3Arg inScale) const
 {
 	return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]);

Jolt/Physics/Body/Body.inl

@@ -9,7 +9,7 @@ Mat44 Body::GetWorldTransform() const
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
 
-	return Mat44::sRotationTranslation(mRotation, GetPosition());
+	return Mat44::sRotationTranslation(mRotation, mPosition).PreTranslated(mShape->GetCenterOfMass());
 }
 
 Mat44 Body::GetCenterOfMassTransform() const

Jolt/Physics/Character/Character.cpp

@@ -12,6 +12,11 @@
 
 JPH_NAMESPACE_BEGIN
 
+static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterface()) : static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterfaceNoLock());
+}
+
 static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies)
 {
 	return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock();
@@ -79,18 +84,26 @@ void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistan
 	// Ignore my own body
 	IgnoreSingleBodyFilter body_filter(mBodyID);
 
-	// Determine position to test
-	Vec3 position;
-	Quat rotation;
-	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
-	bi.GetPositionAndRotation(mBodyID, position, rotation);
-	Mat44 query_transform = Mat44::sRotationTranslation(rotation, position + rotation * inShape->GetCenterOfMass());
+	// Determine position and velocity of body
+	Mat44 query_transform;
+	Vec3 velocity;
+	{
+		BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID);
+		if (!lock.Succeeded())
+			return;
+
+		const Body &body = lock.GetBody();
+
+		// Correct the center of mass transform for the difference between the old and new center of mass shape
+		query_transform = body.GetCenterOfMassTransform().PreTranslated(inShape->GetCenterOfMass() - mShape->GetCenterOfMass());
+		velocity = body.GetLinearVelocity();
+	}
 
 	// Settings for collide shape
 	CollideShapeSettings settings;
 	settings.mMaxSeparationDistance = inMaxSeparationDistance;
 	settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
-	settings.mActiveEdgeMovementDirection = bi.GetLinearVelocity(mBodyID);
+	settings.mActiveEdgeMovementDirection = velocity;
 	settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
 
 	sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), query_transform, settings, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);

Jolt/Physics/Collision/CollisionDispatch.h

@@ -36,29 +36,38 @@ public:
 	}
 
 	/// Cast a shape againt this shape, passes any hits found to ioCollector.
-	/// Note that the shape cast should be relative to the center of mass of this shape (i.e. inShapeCast.mCenterOfMassStart = Start * Mat44::sTranslation(mShape->GetCenterOfMass()) if you want to cast the shape in the space it was created).
-	/// @param inShapeCast The shape to cast against the other shape and its start and direction
+	/// Note: This version takes the shape cast in local space relative to the center of mass of inShape, take a look at sCastShapeVsShapeWorldSpace if you have a shape cast in world space.
+	/// @param inShapeCastLocal The shape to cast against the other shape and its start and direction.
 	/// @param inShapeCastSettings Settings for performing the cast
 	/// @param inShape The shape to cast against.
 	/// @param inScale Local space scale for the shape to cast against.
 	/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
-	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local hit result quantities can be transformed into world space.
 	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
 	/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against
 	/// @param ioCollector The collector that receives the results.
-	static inline void		sCastShapeVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+	static inline void		sCastShapeVsShapeLocalSpace(const ShapeCast &inShapeCastLocal, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 	{
 		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCast.mShape->GetSubType()][(int)inShape->GetSubType()]);)
 
 		// Only test shape if it passes the shape filter
 		if (inShapeFilter.ShouldCollide(inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID()))
-			sCastShape[(int)inShapeCast.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+			sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCastLocal, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	}
+
+	/// See: sCastShapeVsShapeLocalSpace.
+	/// The only difference is that the shape cast (inShapeCastWorld) is provided in world space.
+	/// Note: A shape cast contains the center of mass start of the shape, if you have the world transform of the shape you probably want to construct it using ShapeCast::sFromWorldTransform.
+	static inline void		sCastShapeVsShapeWorldSpace(const ShapeCast &inShapeCastWorld, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+	{
+		ShapeCast local_shape_cast = inShapeCastWorld.PostTransformed(inCenterOfMassTransform2.InversedRotationTranslation());
+		sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 	}
 
 	/// Function that collides 2 shapes (see sCollideShapeVsShape) 
 	using CollideShape = void (*)(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector);
 
-	/// Function that casts a shape vs another shape (see sCastShapeVsShape)
+	/// Function that casts a shape vs another shape (see sCastShapeVsShapeLocalSpace)
 	using CastShape = void (*)(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	/// Register a collide shape function in the collision table

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -266,7 +266,7 @@ void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const Sha
 		Vec3 scale = shape.TransformScale(inShapeCast.mScale);
 		ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection);
 
-		CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector);
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector);
 
 		if (ioCollector.ShouldEarlyOut())
 			break;

Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h

@@ -210,7 +210,7 @@ struct CompoundShape::CastShapeVisitor
 		// Transform the shape cast
 		ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation());
 
-		CollisionDispatch::sCastShapeVsShape(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector);
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector);
 	}
 
 	RayInvDirection				mInvDirection;

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -48,7 +48,7 @@ AABox OffsetCenterOfMassShape::GetLocalBounds() const
 
 AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 { 
-	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform * Mat44::sTranslation(-mOffset), inScale);
+	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-mOffset), inScale);
 }
 
 TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
@@ -69,23 +69,23 @@ Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, V
 
 void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
 {
-	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform * Mat44::sTranslation(-mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy);
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy);
 }
 
 #ifdef JPH_DEBUG_RENDERER
 void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
-	mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sTranslation(-mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe);
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe);
 }
 
 void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
 {
-	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sTranslation(-mOffset), inScale, inColor, inDrawSupportDirection);
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inColor, inDrawSupportDirection);
 }
 
 void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
-	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sTranslation(-mOffset), inScale);
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-mOffset), inScale);
 }
 #endif // JPH_DEBUG_RENDERER
 
@@ -120,7 +120,7 @@ void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3A
 
 void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
 {
-	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sTranslation(-mOffset), ioCollector);
+	mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector);
 }
 
 void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector)
@@ -128,7 +128,7 @@ void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inS
 	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass);
 	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShape1);
 
-	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1 * Mat44::sTranslation(-shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector);
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector);
 }
 
 void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector)
@@ -136,7 +136,7 @@ void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inS
 	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass);
 	const OffsetCenterOfMassShape *shape2 = static_cast<const OffsetCenterOfMassShape *>(inShape2);
 
-	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2 * Mat44::sTranslation(-shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector);
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector);
 }
 
 void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
@@ -146,9 +146,9 @@ void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &in
 	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShapeCast.mShape);
 
 	// Transform the shape cast and update the shape
-	ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart * Mat44::sTranslation(-shape1->mOffset), inShapeCast.mDirection);
+	ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-shape1->mOffset), inShapeCast.mDirection);
 
-	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
 void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
@@ -159,7 +159,7 @@ void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &in
 	// Transform the shape cast
 	ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(shape->mOffset));
 
-	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2 * Mat44::sTranslation(-shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
 void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -178,7 +178,7 @@ void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inSh
 	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
 	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection);
 
-	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
 void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
@@ -192,7 +192,7 @@ void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inSh
 	// Transform the shape cast
 	ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
 
-	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
 void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -174,7 +174,7 @@ void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCa
 	const ScaledShape *shape = static_cast<const ScaledShape *>(inShapeCast.mShape);
 
 	ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection);
-	CollisionDispatch::sCastShapeVsShape(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
 void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
@@ -182,7 +182,7 @@ void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCa
 	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled);
 	const ScaledShape *shape = static_cast<const ScaledShape *>(inShape);
 
-	CollisionDispatch::sCastShapeVsShape(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
 void ScaledShape::sRegister()

Jolt/Physics/Collision/ShapeCast.h

@@ -28,6 +28,12 @@ struct ShapeCast
 	{
 	}
 
+	/// Construct a shape cast using a world transform for a shape instead of a center of mass transform
+	static inline ShapeCast		sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, Mat44Arg inWorldTransform, Vec3Arg inDirection)
+	{
+		return ShapeCast(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection);
+	}
+
 	/// Transform this shape cast using inTransform. Multiply transform on the left left hand side.
 	ShapeCast					PostTransformed(Mat44Arg inTransform) const
 	{
@@ -38,7 +44,7 @@ struct ShapeCast
 
 	const Shape *				mShape;								///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons.
 	const Vec3					mScale;								///< Scale in local space of the shape being cast
-	const Mat44					mCenterOfMassStart;					///< Start position and orientation of the center of mass of the shape (i.e. mCenterOfMassStart = Start * Mat44::sTranslation(mShape->GetCenterOfMass()) if you want to cast the shape in the space it was created)
+	const Mat44					mCenterOfMassStart;					///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape)
 	const Vec3					mDirection;							///< Direction and length of the cast (anything beyond this length will not be reported as a hit)
 	const AABox					mShapeWorldBounds;					///< Cached shape's world bounds, calculated in constructor
 };

Jolt/Physics/Collision/TransformedShape.cpp

@@ -99,11 +99,8 @@ void TransformedShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 		// Get center of mass of object we're casting against
 		Mat44 center_of_mass_transform2 = GetCenterOfMassTransform();
 
-		// Transform the shape cast to local space
-		ShapeCast local_shape = inShapeCast.PostTransformed(center_of_mass_transform2.InversedRotationTranslation());
-				
 		SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator);
-		CollisionDispatch::sCastShapeVsShape(local_shape, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector);
+		CollisionDispatch::sCastShapeVsShapeWorldSpace(inShapeCast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector);
 	}
 }
 

Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -205,7 +205,7 @@ void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices,
 			const Part &part1 = mParts[j1];
 			const Shape *shape1 = part1.GetShape();
 			Vec3 scale1;
-			Mat44 com1 = (inJointMatrices[j1] * Mat44::sTranslation(shape1->GetCenterOfMass())).Decompose(scale1);
+			Mat44 com1 = (inJointMatrices[j1].PreTranslated(shape1->GetCenterOfMass())).Decompose(scale1);
 
 			// Loop over all other joints
 			for (int j2 = j1 + 1; j2 < joint_count; ++j2)
@@ -215,7 +215,7 @@ void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices,
 					const Part &part2 = mParts[j2];
 					const Shape *shape2 = part2.GetShape();
 					Vec3 scale2;
-					Mat44 com2 = (inJointMatrices[j2] * Mat44::sTranslation(shape2->GetCenterOfMass())).Decompose(scale2);
+					Mat44 com2 = (inJointMatrices[j2].PreTranslated(shape2->GetCenterOfMass())).Decompose(scale2);
 					
 					// Collision settings
 					CollideShapeSettings settings;

Samples/SamplesApp.cpp

@@ -1109,8 +1109,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, Vec3 &outPos
 			// Create shape cast
 			RefConst<Shape> shape = CreateProbeShape();
 			Mat44 rotation = Mat44::sRotation(Vec3::sAxisX(), 0.1f * JPH_PI) * Mat44::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI);
-			Mat44 com = Mat44::sTranslation(shape->GetCenterOfMass());
-			ShapeCast shape_cast(shape, Vec3::sReplicate(1.0f), Mat44::sTranslation(start) * rotation * com, direction);
+			ShapeCast shape_cast = ShapeCast::sFromWorldTransform(shape, Vec3::sReplicate(1.0f), Mat44::sTranslation(start) * rotation, direction);
 
 			// Settings
 			ShapeCastSettings settings;
@@ -1175,7 +1174,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, Vec3 &outPos
 						// Draw shape
 						Color color = hit_body.IsDynamic()? Color::sYellow : Color::sOrange;
 					#ifdef JPH_DEBUG_RENDERER
-						shape_cast.mShape->Draw(mDebugRenderer, Mat44::sTranslation(position) * rotation * com, Vec3::sReplicate(1.0f), color, false, false);
+						shape_cast.mShape->Draw(mDebugRenderer, shape_cast.mCenterOfMassStart.PostTranslated(hit.mFraction * shape_cast.mDirection), Vec3::sReplicate(1.0f), color, false, false);
 					#endif // JPH_DEBUG_RENDERER
 
 						// Draw normal
@@ -1211,7 +1210,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, Vec3 &outPos
 				// Draw 'miss'
 				mDebugRenderer->DrawLine(start, start + direction, Color::sRed);
 			#ifdef JPH_DEBUG_RENDERER
-				shape_cast.mShape->Draw(mDebugRenderer, Mat44::sTranslation(outPosition) * rotation * com, Vec3::sReplicate(1.0f), Color::sRed, false, false);
+				shape_cast.mShape->Draw(mDebugRenderer, shape_cast.mCenterOfMassStart.PostTranslated(shape_cast.mDirection), Vec3::sReplicate(1.0f), Color::sRed, false, false);
 			#endif // JPH_DEBUG_RENDERER
 			}
 		}

TestFramework/Renderer/Renderer.cpp

@@ -245,13 +245,24 @@ void Renderer::Initialize()
 	
 #ifdef _DEBUG
 	// Enable breaking on errors
-	ComPtr<ID3D12InfoQueue> pInfoQueue;
-	if (SUCCEEDED(mDevice.As(&pInfoQueue)))
+	ComPtr<ID3D12InfoQueue> info_queue;
+	if (SUCCEEDED(mDevice.As(&info_queue)))
 	{
-		pInfoQueue->SetBreakOnSeverity(D3D12_MESSAGE_SEVERITY_CORRUPTION, TRUE);
-		pInfoQueue->SetBreakOnSeverity(D3D12_MESSAGE_SEVERITY_ERROR, TRUE);
-		pInfoQueue->SetBreakOnSeverity(D3D12_MESSAGE_SEVERITY_WARNING, TRUE);
+		info_queue->SetBreakOnSeverity(D3D12_MESSAGE_SEVERITY_CORRUPTION, TRUE);
+		info_queue->SetBreakOnSeverity(D3D12_MESSAGE_SEVERITY_ERROR, TRUE);
+		info_queue->SetBreakOnSeverity(D3D12_MESSAGE_SEVERITY_WARNING, TRUE);
 	}
+
+	// Disable an error that triggers on Windows 11 with a hybrid graphic system
+	// See: https://stackoverflow.com/questions/69805245/directx-12-application-is-crashing-in-windows-11
+	D3D12_MESSAGE_ID hide[] =
+	{
+		D3D12_MESSAGE_ID_RESOURCE_BARRIER_MISMATCHING_COMMAND_LIST_TYPE,
+	};
+	D3D12_INFO_QUEUE_FILTER filter = { };
+	filter.DenyList.NumIDs = static_cast<UINT>(std::size(hide));
+	filter.DenyList.pIDList = hide;
+	info_queue->AddStorageFilterEntries(&filter);
 #endif // _DEBUG
 
 	// Disable full screen transitions

UnitTests/Math/Mat44Tests.cpp

@@ -330,6 +330,24 @@ TEST_SUITE("Mat44Tests")
 		CHECK_APPROX_EQUAL(m1, m2);
 	}
 
+	TEST_CASE("TestMat44PrePostScaled")
+	{
+		Mat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), Vec4(11, 12, 13, 1));
+		Vec3 v(14, 15, 16);
+
+		CHECK(m.PreScaled(v) == m * Mat44::sScale(v));
+		CHECK(m.PostScaled(v) == Mat44::sScale(v) * m);
+	}
+
+	TEST_CASE("TestMat44PrePostTranslated")
+	{
+		Mat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), Vec4(11, 12, 13, 1));
+		Vec3 v(14, 15, 16);
+
+		CHECK(m.PreTranslated(v) == m * Mat44::sTranslation(v));
+		CHECK(m.PostTranslated(v) == Mat44::sTranslation(v) * m);
+	}
+
 	TEST_CASE("TestMat44Decompose")
 	{
 		// Create a rotation/translation matrix

UnitTests/Physics/ActiveEdgesTests.cpp

@@ -142,7 +142,7 @@ TEST_SUITE("ActiveEdgesTest")
 	{
 		AllHitCollisionCollector<CastShapeCollector> collector;
 		ShapeCast shape_cast(inProbeShape, Vec3::sReplicate(1.0f), Mat44::sTranslation(inProbeShapePos), inProbeShapeDirection);
-		CollisionDispatch::sCastShapeVsShape(shape_cast, inSettings, inTestShape, inTestShapeScale, ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inSettings, inTestShape, inTestShapeScale, ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 
 		sCheckMatch(collector.mHits, inExpectedHits, 1.0e-6f);
 	}

UnitTests/Physics/CastShapeTests.cpp

@@ -25,7 +25,7 @@ TEST_SUITE("CastShapeTests")
 		cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
 		cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
 		AllHitCollisionCollector<CastShapeCollector> collector;
-		CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 		CHECK(collector.mHits.size() == 1);
 		const ShapeCastResult &result = collector.mHits.back();
 		CHECK_APPROX_EQUAL(result.mFraction, 1.0f - 0.2f / inDirection.Length(), 1.0e-4f);
@@ -49,7 +49,7 @@ TEST_SUITE("CastShapeTests")
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
 			cast_settings.mReturnDeepestPoint = false;
 			AllHitCollisionCollector<CastShapeCollector> collector;
-			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.size() == 1);
 			const ShapeCastResult &result = collector.mHits.back();
 			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
@@ -68,13 +68,13 @@ TEST_SUITE("CastShapeTests")
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
 			cast_settings.mReturnDeepestPoint = false;
 			AllHitCollisionCollector<CastShapeCollector> collector;
-			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.empty());
 
 			// Hit back face -> collision
 			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
-			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.size() == 1);
 			const ShapeCastResult &result = collector.mHits.back();
 			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
@@ -93,13 +93,13 @@ TEST_SUITE("CastShapeTests")
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
 			cast_settings.mReturnDeepestPoint = true;
 			AllHitCollisionCollector<CastShapeCollector> collector;
-			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.empty());
 
 			// Hit back face while starting in collision -> collision
 			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
-			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.size() == 1);
 			const ShapeCastResult &result = collector.mHits.back();
 			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);