initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp

@@ -0,0 +1,674 @@
+// 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/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/ScopeExit.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(StaticCompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(StaticCompoundShapeSettings, CompoundShapeSettings)
+}
+
+ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create(TempAllocator &inTempAllocator) const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		if (mSubShapes.size() == 0)
+		{
+			// It's an error to create a compound with no subshapes (the compound cannot encode this)
+			mCachedResult.SetError("Compound needs a sub shape!");
+		}
+		else if (mSubShapes.size() == 1)
+		{
+			// If there's only 1 part we don't need a StaticCompoundShape
+			const SubShapeSettings &s = mSubShapes[0];
+			if (s.mPosition == Vec3::sZero()
+				&& s.mRotation == Quat::sIdentity())
+			{
+				// No rotation or translation, we can use the shape directly
+				if (s.mShapePtr != nullptr)
+					mCachedResult.Set(const_cast<Shape *>(s.mShapePtr.GetPtr()));
+				else if (s.mShape != nullptr)
+					mCachedResult = s.mShape->Create();
+				else
+					mCachedResult.SetError("Sub shape is null!");
+			}
+			else
+			{
+				// We can use a RotatedTranslatedShape instead
+				RotatedTranslatedShapeSettings settings;
+				settings.mPosition = s.mPosition;
+				settings.mRotation = s.mRotation;
+				settings.mInnerShape = s.mShape;
+				settings.mInnerShapePtr = s.mShapePtr;
+				Ref<Shape> shape = new RotatedTranslatedShape(settings, mCachedResult);
+			}
+		}
+		else
+		{
+			// Build a regular compound shape
+			Ref<Shape> shape = new StaticCompoundShape(*this, inTempAllocator, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create() const
+{
+	TempAllocatorMalloc allocator;
+	return Create(allocator);
+}
+
+void StaticCompoundShape::Node::SetChildInvalid(uint inIndex)
+{
+	// Make this an invalid node
+	mNodeProperties[inIndex] = INVALID_NODE;
+
+	// Make bounding box invalid
+	mBoundsMinX[inIndex] = HALF_FLT_MAX;
+	mBoundsMinY[inIndex] = HALF_FLT_MAX;
+	mBoundsMinZ[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxX[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxY[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxZ[inIndex] = HALF_FLT_MAX;
+}
+
+void StaticCompoundShape::Node::SetChildBounds(uint inIndex, const AABox &inBounds)
+{
+	mBoundsMinX[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetX());
+	mBoundsMinY[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetY());
+	mBoundsMinZ[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetZ());
+	mBoundsMaxX[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetX());
+	mBoundsMaxY[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetY());
+	mBoundsMaxZ[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetZ());
+}
+
+void StaticCompoundShape::sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint)
+{
+	// Handle trivial case
+	if (inNumber <= 4)
+	{
+		outMidPoint = inNumber / 2;
+		return;
+	}
+
+	// Calculate bounding box of box centers
+	Vec3 center_min = Vec3::sReplicate(FLT_MAX);
+	Vec3 center_max = Vec3::sReplicate(-FLT_MAX);
+	for (const AABox *b = ioBounds, *b_end = ioBounds + inNumber; b < b_end; ++b)
+	{
+		Vec3 center = b->GetCenter();
+		center_min = Vec3::sMin(center_min, center);
+		center_max = Vec3::sMax(center_max, center);
+	}
+
+	// Calculate split plane
+	int dimension = (center_max - center_min).GetHighestComponentIndex();
+	float split = 0.5f * (center_min + center_max)[dimension];
+
+	// Divide bodies
+	int start = 0, end = inNumber;
+	while (start < end)
+	{
+		// Search for first element that is on the right hand side of the split plane
+		while (start < end && ioBounds[start].GetCenter()[dimension] < split)
+			++start;
+
+		// Search for the first element that is on the left hand side of the split plane
+		while (start < end && ioBounds[end - 1].GetCenter()[dimension] >= split)
+			--end;
+
+		if (start < end)
+		{
+			// Swap the two elements
+			std::swap(ioBodyIdx[start], ioBodyIdx[end - 1]);
+			std::swap(ioBounds[start], ioBounds[end - 1]);
+			++start;
+			--end;
+		}
+	}
+	JPH_ASSERT(start == end);
+
+	if (start > 0 && start < inNumber)
+	{
+		// Success!
+		outMidPoint = start;
+	}
+	else
+	{
+		// Failed to divide bodies
+		outMidPoint = inNumber / 2;
+	}
+}
+
+void StaticCompoundShape::sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit)
+{
+	uint *body_idx = ioBodyIdx + inBegin;
+	AABox *node_bounds = ioBounds + inBegin;
+	int number = inEnd - inBegin;
+
+	// Partition entire range
+	sPartition(body_idx, node_bounds, number, outSplit[2]);
+
+	// Partition lower half
+	sPartition(body_idx, node_bounds, outSplit[2], outSplit[1]);
+
+	// Partition upper half
+	sPartition(body_idx + outSplit[2], node_bounds + outSplit[2], number - outSplit[2], outSplit[3]);
+
+	// Convert to proper range
+	outSplit[0] = inBegin;
+	outSplit[1] += inBegin;
+	outSplit[2] += inBegin;
+	outSplit[3] += outSplit[2];
+	outSplit[4] = inEnd;
+}
+
+StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult) :
+	CompoundShape(EShapeSubType::StaticCompound, inSettings, outResult)
+{
+	// Check that there's at least 1 shape
+	uint num_subshapes = (uint)inSettings.mSubShapes.size();
+	if (num_subshapes < 2)
+	{
+		outResult.SetError("Compound needs at least 2 sub shapes, otherwise you should use a RotatedTranslatedShape!");
+		return;
+	}
+
+	// Keep track of total mass to calculate center of mass
+	float mass = 0.0f;
+
+	mSubShapes.resize(num_subshapes);
+	for (uint i = 0; i < num_subshapes; ++i)
+	{
+		const CompoundShapeSettings::SubShapeSettings &shape = inSettings.mSubShapes[i];
+
+		// Start constructing the runtime sub shape
+		SubShape &out_shape = mSubShapes[i];
+		if (!out_shape.FromSettings(shape, outResult))
+			return;
+
+		// Calculate mass properties of child
+		MassProperties child = out_shape.mShape->GetMassProperties();
+
+		// Accumulate center of mass
+		mass += child.mMass;
+		mCenterOfMass += out_shape.GetPositionCOM() * child.mMass;
+	}
+
+	if (mass > 0.0f)
+		mCenterOfMass /= mass;
+
+	// Cache the inner radius as it can take a while to recursively iterate over all sub shapes
+	CalculateInnerRadius();
+
+	// Temporary storage for the bounding boxes of all shapes
+	uint bounds_size = num_subshapes * sizeof(AABox);
+	AABox *bounds = (AABox *)inTempAllocator.Allocate(bounds_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, bounds, bounds_size]{ inTempAllocator.Free(bounds, bounds_size); });
+
+	// Temporary storage for body indexes (we're shuffling them)
+	uint body_idx_size = num_subshapes * sizeof(uint);
+	uint *body_idx = (uint *)inTempAllocator.Allocate(body_idx_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, body_idx, body_idx_size]{ inTempAllocator.Free(body_idx, body_idx_size); });
+
+	// Shift all shapes so that the center of mass is now at the origin and calculate bounds
+	for (uint i = 0; i < num_subshapes; ++i)
+	{
+		SubShape &shape = mSubShapes[i];
+
+		// Shift the shape so it's centered around our center of mass
+		shape.SetPositionCOM(shape.GetPositionCOM() - mCenterOfMass);
+
+		// Transform the shape's bounds into our local space
+		Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
+		AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sOne());
+
+		// Store bounds and body index for tree construction
+		bounds[i] = shape_bounds;
+		body_idx[i] = i;
+
+		// Update our local bounds
+		mLocalBounds.Encapsulate(shape_bounds);
+	}
+
+	// The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here
+	struct StackEntry
+	{
+		uint32			mNodeIdx;					// Node index of node that is generated
+		int				mChildIdx;					// Index of child that we're currently processing
+		int				mSplit[5];					// Indices where the node ID's have been split to form 4 partitions
+		AABox			mBounds;					// Bounding box of this node
+	};
+	uint stack_size = num_subshapes * sizeof(StackEntry);
+	StackEntry *stack = (StackEntry *)inTempAllocator.Allocate(stack_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, stack, stack_size]{ inTempAllocator.Free(stack, stack_size); });
+	int top = 0;
+
+	// Reserve enough space so that every sub shape gets its own leaf node
+	uint next_node_idx = 0;
+	mNodes.resize(num_subshapes + (num_subshapes + 2) / 3); // = Sum(num_subshapes * 4^-i) with i = [0, Inf].
+
+	// Create root node
+	stack[0].mNodeIdx = next_node_idx++;
+	stack[0].mChildIdx = -1;
+	stack[0].mBounds = AABox();
+	sPartition4(body_idx, bounds, 0, num_subshapes, stack[0].mSplit);
+
+	for (;;)
+	{
+		StackEntry &cur_stack = stack[top];
+
+		// Next child
+		cur_stack.mChildIdx++;
+
+		// Check if all children processed
+		if (cur_stack.mChildIdx >= 4)
+		{
+			// Terminate if there's nothing left to pop
+			if (top <= 0)
+				break;
+
+			// Add our bounds to our parents bounds
+			StackEntry &prev_stack = stack[top - 1];
+			prev_stack.mBounds.Encapsulate(cur_stack.mBounds);
+
+			// Store this node's properties in the parent node
+			Node &parent_node = mNodes[prev_stack.mNodeIdx];
+			parent_node.mNodeProperties[prev_stack.mChildIdx] = cur_stack.mNodeIdx;
+			parent_node.SetChildBounds(prev_stack.mChildIdx, cur_stack.mBounds);
+
+			// Pop entry from stack
+			--top;
+		}
+		else
+		{
+			// Get low and high index to bodies to process
+			int low = cur_stack.mSplit[cur_stack.mChildIdx];
+			int high = cur_stack.mSplit[cur_stack.mChildIdx + 1];
+			int num_bodies = high - low;
+
+			if (num_bodies == 0)
+			{
+				// Mark invalid
+				Node &node = mNodes[cur_stack.mNodeIdx];
+				node.SetChildInvalid(cur_stack.mChildIdx);
+			}
+			else if (num_bodies == 1)
+			{
+				// Get body info
+				uint child_node_idx = body_idx[low];
+				const AABox &child_bounds = bounds[low];
+
+				// Update node
+				Node &node = mNodes[cur_stack.mNodeIdx];
+				node.mNodeProperties[cur_stack.mChildIdx] = child_node_idx | IS_SUBSHAPE;
+				node.SetChildBounds(cur_stack.mChildIdx, child_bounds);
+
+				// Encapsulate bounding box in parent
+				cur_stack.mBounds.Encapsulate(child_bounds);
+			}
+			else
+			{
+				// Allocate new node
+				StackEntry &new_stack = stack[++top];
+				JPH_ASSERT(top < (int)num_subshapes);
+				new_stack.mNodeIdx = next_node_idx++;
+				new_stack.mChildIdx = -1;
+				new_stack.mBounds = AABox();
+				sPartition4(body_idx, bounds, low, high, new_stack.mSplit);
+			}
+		}
+	}
+
+	// Resize nodes to actual size
+	JPH_ASSERT(next_node_idx <= mNodes.size());
+	mNodes.resize(next_node_idx);
+	mNodes.shrink_to_fit();
+
+	// Check if we ran out of bits for addressing a node
+	if (next_node_idx > IS_SUBSHAPE)
+	{
+		outResult.SetError("Compound hierarchy has too many nodes");
+		return;
+	}
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+template <class Visitor>
+inline void StaticCompoundShape::WalkTree(Visitor &ioVisitor) const
+{
+	uint32 node_stack[cStackSize];
+	node_stack[0] = 0;
+	int top = 0;
+	do
+	{
+		// Test if the node is valid, the node should rarely be invalid but it is possible when testing
+		// a really large box against the tree that the invalid nodes will intersect with the box
+		uint32 node_properties = node_stack[top];
+		if (node_properties != INVALID_NODE)
+		{
+			// Test if node contains triangles
+			bool is_node = (node_properties & IS_SUBSHAPE) == 0;
+			if (is_node)
+			{
+				const Node &node = mNodes[node_properties];
+
+				// Unpack bounds
+				UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMinX[0]));
+				Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy);
+				Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMinZ[0]));
+				Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx);
+				Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMaxY[0]));
+				Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz);
+				Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				// Load properties for 4 children
+				UVec4 properties = UVec4::sLoadInt4(&node.mNodeProperties[0]);
+
+				// Check which sub nodes to visit
+				int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, top);
+
+				// Push them onto the stack
+				JPH_ASSERT(top + 4 < cStackSize);
+				properties.StoreInt4(&node_stack[top]);
+				top += num_results;
+			}
+			else
+			{
+				// Points to a sub shape
+				uint32 sub_shape_idx = node_properties ^ IS_SUBSHAPE;
+				const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+
+				ioVisitor.VisitShape(sub_shape, sub_shape_idx);
+			}
+
+			// Check if we're done
+			if (ioVisitor.ShouldAbort())
+				break;
+		}
+
+		// Fetch next node until we find one that the visitor wants to see
+		do
+			--top;
+		while (top >= 0 && !ioVisitor.ShouldVisitNode(top));
+	}
+	while (top >= 0);
+}
+
+bool StaticCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitor
+	{
+		using CastRayVisitor::CastRayVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mHit.mFraction;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit);
+	WalkTree(visitor);
+	return visitor.mReturnValue;
+}
+
+void StaticCompoundShape::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;
+
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitorCollector
+	{
+		using CastRayVisitorCollector::CastRayVisitorCollector;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollidePointVisitor
+	{
+		using CollidePointVisitor::CollidePointVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test if point overlaps with box
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+void StaticCompoundShape::sCastShapeVsCompound(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_PROFILE_FUNCTION();
+
+	struct Visitor : public CastShapeVisitor
+	{
+		using CastShapeVisitor::CastShapeVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape = static_cast<const StaticCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkTree(visitor);
+}
+
+void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollectTransformedShapesVisitor
+	{
+		using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+int StaticCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorSC<AABox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkTree(visitor);
+	return visitor.GetNumResults();
+}
+
+int StaticCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorSC<OrientedBox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkTree(visitor);
+	return visitor.GetNumResults();
+}
+
+void StaticCompoundShape::sCollideCompoundVsShape(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_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape1 = static_cast<const StaticCompoundShape *>(inShape1);
+
+	struct Visitor : public CollideCompoundVsShapeVisitor
+	{
+		using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape1->WalkTree(visitor);
+}
+
+void StaticCompoundShape::sCollideShapeVsCompound(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_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideShapeVsCompoundVisitor
+	{
+		using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape2 = static_cast<const StaticCompoundShape *>(inShape2);
+
+	Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape2->WalkTree(visitor);
+}
+
+void StaticCompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	CompoundShape::SaveBinaryState(inStream);
+
+	inStream.Write(mNodes);
+}
+
+void StaticCompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	CompoundShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mNodes);
+}
+
+void StaticCompoundShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::StaticCompound);
+	f.mConstruct = []() -> Shape * { return new StaticCompoundShape; };
+	f.mColor = Color::sOrange;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound);
+	}
+}
+
+JPH_NAMESPACE_END