@bloomengine/engine 0.3.1

package/native/third_party/JoltPhysics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h

@@ -0,0 +1,319 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Plane.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// This class calculates the intersection between a fluid surface and a polyhedron and returns the submerged volume and its center of buoyancy
+/// Construct this class and then one by one add all faces of the polyhedron using the AddFace function. After all faces have been added the result
+/// can be gotten through GetResult.
+class PolyhedronSubmergedVolumeCalculator
+{
+private:
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged
+	// inV1 .. inV4 are submerged
+	inline static void	sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// Calculate center of mass and mass of this tetrahedron,
+		// see: https://en.wikipedia.org/wiki/Tetrahedron#Volume
+		outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull
+		outCenterTimes4 = inV1 + inV2 + inV3 + inV4;
+	}
+
+	// Get the intersection point with a plane.
+	// inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane
+	inline static Vec3	sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2)
+	{
+		JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane");
+		float delta = inD1 - inD2;
+		if (abs(delta) < 1.0e-6f)
+			return inV1; // Parallel to plane, just pick a point
+		else
+			return inV1 + inD1 * (inV2 - inV1) / delta;
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged
+	// inV1 is submerged, inV2 .. inV4 are not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron
+		Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2);
+		Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
+		Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+			RVec3 v4w = mBaseOffset + v4;
+
+			DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4);
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged
+	// inV1, inV2 are submerged, inV3, inV4 are not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 2 points submerged is cut along 4 edges forming a quad
+		Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
+		Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+		Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
+		Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 cw = mBaseOffset + c;
+			RVec3 dw = mBaseOffset + d;
+			RVec3 ew = mBaseOffset + e;
+			RVec3 fw = mBaseOffset + f;
+
+			DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen);
+			DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		// We pick point c as reference (which is on the cut off surface)
+		// This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume)
+		Vec3 center1, center2, center3;
+		float volume1, volume2, volume3;
+		sTetrahedronVolume4(e, f, inV2, c, volume1, center1);
+		sTetrahedronVolume4(e, inV1, d, c, volume2, center2);
+		sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3);
+
+		// Tally up the totals
+		outVolumeTimes6 = volume1 + volume2 + volume3;
+		outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero();
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged
+	// inV1, inV2, inV3 are submerged, inV4 is not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron
+		Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+		Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
+		Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 v1w = mBaseOffset + v1;
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+
+			DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		Vec3 dry_center, total_center;
+		float dry_volume, total_volume;
+
+		// We first calculate the part that is above the surface
+		sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center);
+
+		// Calculate the total volume
+		sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center);
+
+		// From this we can calculate the center and volume of the submerged part
+		outVolumeTimes6 = max(total_volume - dry_volume, 0.0f);
+		outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero();
+	}
+
+public:
+	/// A helper class that contains cached information about a polyhedron vertex
+	class Point
+	{
+	public:
+		Vec3			mPosition;						///< World space position of vertex
+		float			mDistanceToSurface;				///< Signed distance to the surface (> 0 is above, < 0 is below)
+		bool			mAboveSurface;					///< If the point is above the surface (mDistanceToSurface > 0)
+	};
+
+	/// Constructor
+	/// @param inTransform Transform to transform all incoming points with
+	/// @param inPoints Array of points that are part of the polyhedron
+	/// @param inPointStride Amount of bytes between each point (should usually be sizeof(Vec3))
+	/// @param inNumPoints The amount of points
+	/// @param inSurface The plane that forms the fluid surface (normal should point up)
+	/// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive
+#ifdef JPH_DEBUG_RENDERER
+	/// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+#endif // JPH_DEBUG_RENDERER
+						PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3 inBaseOffset
+#endif // JPH_DEBUG_RENDERER
+		) :
+		mPoints(ioBuffer)
+#ifdef JPH_DEBUG_RENDERER
+		, mBaseOffset(inBaseOffset)
+#endif // JPH_DEBUG_RENDERER
+	{
+		// Convert the points to world space and determine the distance to the surface
+		float reference_dist = FLT_MAX;
+		for (int p = 0; p < inNumPoints; ++p)
+		{
+			// Calculate values
+			Vec3 transformed_point = inTransform * *reinterpret_cast<const Vec3 *>(reinterpret_cast<const uint8 *>(inPoints) + p * inPointStride);
+			float dist = inSurface.SignedDistance(transformed_point);
+			bool above = dist >= 0.0f;
+
+			// Keep track if all are above or below
+			mAllAbove &= above;
+			mAllBelow &= !above;
+
+			// Calculate lowest point, we use this to create tetrahedrons out of all faces
+			if (reference_dist > dist)
+			{
+				mReferencePointIdx = p;
+				reference_dist = dist;
+			}
+
+			// Store values
+			ioBuffer->mPosition = transformed_point;
+			ioBuffer->mDistanceToSurface = dist;
+			ioBuffer->mAboveSurface = above;
+			++ioBuffer;
+		}
+	}
+
+	/// Check if all points are above the surface. Should be used as early out.
+	inline bool			AreAllAbove() const
+	{
+		return mAllAbove;
+	}
+
+	/// Check if all points are below the surface. Should be used as early out.
+	inline bool			AreAllBelow() const
+	{
+		return mAllBelow;
+	}
+
+	/// Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a polyhedron face.
+	inline int			GetReferencePointIdx() const
+	{
+		return mReferencePointIdx;
+	}
+
+	/// Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise order).
+	void				AddFace(int inIdx1, int inIdx2, int inIdx3)
+	{
+		JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume");
+
+		// Find the points
+		const Point &ref = mPoints[mReferencePointIdx];
+		const Point &p1 = mPoints[inIdx1];
+		const Point &p2 = mPoints[inIdx2];
+		const Point &p3 = mPoints[inIdx3];
+
+		// Determine which vertices are submerged
+		uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100);
+
+		float volume;
+		Vec3 center;
+		switch (code)
+		{
+		case 0b000:
+			// One point submerged
+			sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b001:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b010:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b100:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b011:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b101:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b110:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b111:
+			// Four points submerged
+			sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center);
+			break;
+
+		default:
+			// Should not be possible
+			JPH_ASSERT(false);
+			volume = 0.0f;
+			center = Vec3::sZero();
+			break;
+		}
+
+		mSubmergedVolume += volume;
+		mCenterOfBuoyancy += volume * center;
+	}
+
+	/// Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the submerged volume.
+	void				GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+	{
+		outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor
+		outSubmergedVolume = mSubmergedVolume / 6.0f;
+	}
+
+private:
+	// The precalculated points for this polyhedron
+	const Point *		mPoints;
+
+	// If all points are above/below the surface
+	bool				mAllBelow = true;
+	bool				mAllAbove = true;
+
+	// The lowest point
+	int					mReferencePointIdx = 0;
+
+	// Aggregator for submerged volume and center of buoyancy
+	float				mSubmergedVolume = 0.0f;
+	Vec3				mCenterOfBuoyancy = Vec3::sZero();
+
+#ifdef JPH_DEBUG_RENDERER
+	// Base offset used for drawing
+	RVec3				mBaseOffset;
+#endif
+};
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -0,0 +1,333 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation)
+}
+
+ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new RotatedTranslatedShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult)
+{
+	if (outResult.HasError())
+		return;
+
+	// Calculate center of mass position
+	mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass();
+
+	// Store rotation (position is always zero because we center around the center of mass)
+	mRotation = inSettings.mRotation;
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+
+	outResult.Set(this);
+}
+
+RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) :
+	DecoratedShape(EShapeSubType::RotatedTranslated, inShape)
+{
+	// Calculate center of mass position
+	mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass();
+
+	// Store rotation (position is always zero because we center around the center of mass)
+	mRotation = inRotation;
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+}
+
+MassProperties RotatedTranslatedShape::GetMassProperties() const
+{
+	// Rotate inertia of child into place
+	MassProperties p = mInnerShape->GetMassProperties();
+	p.Rotate(Mat44::sRotation(mRotation));
+	return p;
+}
+
+AABox RotatedTranslatedShape::GetLocalBounds() const
+{
+	return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation));
+}
+
+AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
+	return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale));
+}
+
+TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(TransformScale(inScale));
+	return ts;
+}
+
+Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform surface position to local space and pass call on
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition);
+
+	// Transform normal to this shape's space
+	return transform.Multiply3x3Transposed(normal);
+}
+
+void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	Mat44 transform = Mat44::sRotation(mRotation);
+	mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
+}
+
+void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// Get center of mass transform of child
+	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
+
+	// Recurse to child
+	mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection);
+}
+
+void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale));
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Transform the ray
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	RayCast ray = inRay.Transformed(transform);
+
+	return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);
+}
+
+void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the ray
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	RayCast ray = inRay.Transformed(transform);
+
+	return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the point
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector);
+}
+
+void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(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, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
+
+	// Get world transform of 1
+	Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(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, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
+
+	// Get world transform of 2
+	Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(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, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
+
+	// Get world transform of 1 and 2
+	Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
+	Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// Fetch rotated translated shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
+
+	// Transform the shape cast and update the shape
+	Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation);
+	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
+	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection);
+
+	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)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform
+	Mat44 local_transform = Mat44::sRotation(shape->mRotation);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform of shape 2
+	Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation);
+	Mat44 local_transform2_transposed = local_transform2.Transposed3x3();
+
+	// Transform the shape cast and update the shape
+	Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation);
+	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
+	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection));
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mRotation);
+}
+
+void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mRotation);
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+}
+
+bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const
+{
+	if (!Shape::IsValidScale(inScale))
+		return false;
+
+	if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+		return mInnerShape->IsValidScale(inScale);
+
+	if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale))
+		return false;
+
+	return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale));
+}
+
+Vec3 RotatedTranslatedShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(scale))
+		return mInnerShape->MakeScaleValid(scale);
+
+	if (ScaleHelpers::CanScaleBeRotated(mRotation, scale))
+		return ScaleHelpers::RotateScale(mRotation.Conjugated(), mInnerShape->MakeScaleValid(ScaleHelpers::RotateScale(mRotation, scale)));
+
+	Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
+	Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
+	if (ScaleHelpers::CanScaleBeRotated(mRotation, uniform_scale))
+		return uniform_scale;
+
+	return Sign(scale.GetX()) * abs_uniform_scale;
+}
+
+void RotatedTranslatedShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated);
+	f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; };
+	f.mColor = Color::sBlue;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated);
+	}
+
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated);
+}
+
+JPH_NAMESPACE_END