@bloomengine/engine 0.3.3 → 0.4.1

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

@@ -1,433 +0,0 @@
-// 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/CompoundShape.h>
-#include <Jolt/Physics/Collision/CollisionDispatch.h>
-#include <Jolt/Physics/Collision/ShapeCast.h>
-#include <Jolt/Physics/Collision/CastResult.h>
-#include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Core/Profiler.h>
-#include <Jolt/Core/StreamIn.h>
-#include <Jolt/Core/StreamOut.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-#ifdef JPH_DEBUG_RENDERER
-	#include <Jolt/Renderer/DebugRenderer.h>
-#endif // JPH_DEBUG_RENDERER
-
-JPH_NAMESPACE_BEGIN
-
-JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings)
-{
-	JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings)
-
-	JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings)
-{
-	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape)
-	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition)
-	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation)
-	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData)
-}
-
-void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData)
-{
-	// Add shape
-	SubShapeSettings shape;
-	shape.mPosition = inPosition;
-	shape.mRotation = inRotation;
-	shape.mShape = inShape;
-	shape.mUserData = inUserData;
-	mSubShapes.push_back(shape);
-}
-
-void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
-{
-	// Add shape
-	SubShapeSettings shape;
-	shape.mPosition = inPosition;
-	shape.mRotation = inRotation;
-	shape.mShapePtr = inShape;
-	shape.mUserData = inUserData;
-	mSubShapes.push_back(shape);
-}
-
-bool CompoundShape::MustBeStatic() const
-{
-	for (const SubShape &shape : mSubShapes)
-		if (shape.mShape->MustBeStatic())
-			return true;
-
-	return false;
-}
-
-MassProperties CompoundShape::GetMassProperties() const
-{
-	MassProperties p;
-
-	// Calculate mass and inertia
-	p.mMass = 0.0f;
-	p.mInertia = Mat44::sZero();
-	for (const SubShape &shape : mSubShapes)
-	{
-		// Rotate and translate inertia of child into place
-		MassProperties child = shape.mShape->GetMassProperties();
-		child.Rotate(Mat44::sRotation(shape.GetRotation()));
-		child.Translate(shape.GetPositionCOM());
-
-		// Accumulate mass and inertia
-		p.mMass += child.mMass;
-		p.mInertia += child.mInertia;
-	}
-
-	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
-	p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
-
-	return p;
-}
-
-AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{
-	if (mSubShapes.empty())
-	{
-		// If there are no sub-shapes, we must return an empty box to avoid overflows in the broadphase
-		return AABox(inCenterOfMassTransform.GetTranslation(), inCenterOfMassTransform.GetTranslation());
-	}
-	else if (mSubShapes.size() <= 10)
-	{
-		AABox bounds;
-		for (const SubShape &shape : mSubShapes)
-		{
-			Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
-			bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale)));
-		}
-		return bounds;
-	}
-	else
-	{
-		// If there are too many shapes, use the base class function (this will result in a slightly wider bounding box)
-		return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
-	}
-}
-
-uint CompoundShape::GetSubShapeIDBitsRecursive() const
-{
-	// Add max of child bits to our bits
-	uint child_bits = 0;
-	for (const SubShape &shape : mSubShapes)
-		child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive());
-	return child_bits + GetSubShapeIDBits();
-}
-
-const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const
-{
-	// Decode sub shape index
-	SubShapeID remainder;
-	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
-
-	// Pass call on
-	return mSubShapes[index].mShape->GetMaterial(remainder);
-}
-
-const Shape *CompoundShape::GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const
-{
-	// Decode sub shape index
-	SubShapeID remainder;
-	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
-	if (index >= mSubShapes.size())
-	{
-		// No longer valid index
-		outRemainder = SubShapeID();
-		return nullptr;
-	}
-
-	// Pass call on
-	return mSubShapes[index].mShape->GetLeafShape(remainder, outRemainder);
-}
-
-uint64 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const
-{
-	// Decode sub shape index
-	SubShapeID remainder;
-	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
-	if (index >= mSubShapes.size())
-		return 0; // No longer valid index
-
-	// Pass call on
-	return mSubShapes[index].mShape->GetSubShapeUserData(remainder);
-}
-
-TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
-{
-	// Get the sub shape
-	const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)];
-
-	// Calculate transform for sub shape
-	Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM());
-	Quat rotation = inRotation * sub_shape.GetRotation();
-	Vec3 scale = sub_shape.TransformScale(inScale);
-
-	// Return transformed shape
-	TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID());
-	ts.SetShapeScale(scale);
-	return ts;
-}
-
-Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
-{
-	// Decode sub shape index
-	SubShapeID remainder;
-	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
-
-	// Transform surface position to local space and pass call on
-	const SubShape &shape = mSubShapes[index];
-	Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
-	Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition);
-
-	// Transform normal to this shape's space
-	return transform.Multiply3x3Transposed(normal);
-}
-
-void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
-{
-	// Decode sub shape index
-	SubShapeID remainder;
-	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
-
-	// Apply transform and pass on to sub shape
-	const SubShape &shape = mSubShapes[index];
-	Mat44 transform = shape.GetLocalTransformNoScale(inScale);
-	shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
-}
-
-void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
-{
-	outTotalVolume = 0.0f;
-	outSubmergedVolume = 0.0f;
-	outCenterOfBuoyancy = Vec3::sZero();
-
-	for (const SubShape &shape : mSubShapes)
-	{
-		// Get center of mass transform of child
-		Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
-
-		// Recurse to child
-		float total_volume, submerged_volume;
-		Vec3 center_of_buoyancy;
-		shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
-
-		// Accumulate volumes
-		outTotalVolume += total_volume;
-		outSubmergedVolume += submerged_volume;
-
-		// The center of buoyancy is the weighted average of the center of buoyancy of our child shapes
-		outCenterOfBuoyancy += submerged_volume * center_of_buoyancy;
-	}
-
-	if (outSubmergedVolume > 0.0f)
-		outCenterOfBuoyancy /= outSubmergedVolume;
-
-#ifdef JPH_DEBUG_RENDERER
-	// Draw center of buoyancy
-	if (sDrawSubmergedVolumes)
-		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
-#endif // JPH_DEBUG_RENDERER
-}
-
-#ifdef JPH_DEBUG_RENDERER
-void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
-{
-	for (const SubShape &shape : mSubShapes)
-	{
-		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
-		shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
-	}
-}
-
-void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
-{
-	for (const SubShape &shape : mSubShapes)
-	{
-		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
-		shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection);
-	}
-}
-
-void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{
-	for (const SubShape &shape : mSubShapes)
-	{
-		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
-		shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale));
-	}
-}
-#endif // JPH_DEBUG_RENDERER
-
-void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
-{
-	for (const SubShape &shape : mSubShapes)
-	{
-		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
-		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex);
-	}
-}
-
-void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
-{
-	for (const SubShape &shape : mSubShapes)
-		shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector);
-}
-
-void CompoundShape::sCastCompoundVsShape(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();
-
-	// Fetch compound shape from cast shape
-	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound);
-	const CompoundShape *compound = static_cast<const CompoundShape *>(inShapeCast.mShape);
-
-	// Number of sub shapes
-	int n = (int)compound->mSubShapes.size();
-
-	// Determine amount of bits for sub shape
-	uint sub_shape_bits = compound->GetSubShapeIDBits();
-
-	// Recurse to sub shapes
-	for (int i = 0; i < n; ++i)
-	{
-		const SubShape &shape = compound->mSubShapes[i];
-
-		// Create ID for sub shape
-		SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits);
-
-		// Transform the shape cast and update the shape
-		Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale);
-		Vec3 scale = shape.TransformScale(inShapeCast.mScale);
-		ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection);
-
-		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector);
-
-		if (ioCollector.ShouldEarlyOut())
-			break;
-	}
-}
-
-void CompoundShape::SaveBinaryState(StreamOut &inStream) const
-{
-	Shape::SaveBinaryState(inStream);
-
-	inStream.Write(mCenterOfMass);
-	inStream.Write(mLocalBounds.mMin);
-	inStream.Write(mLocalBounds.mMax);
-	inStream.Write(mInnerRadius);
-
-	// Write sub shapes
-	inStream.Write(mSubShapes, [](const SubShape &inElement, StreamOut &inS) {
-		inS.Write(inElement.mUserData);
-		inS.Write(inElement.mPositionCOM);
-		inS.Write(inElement.mRotation);
-	});
-}
-
-void CompoundShape::RestoreBinaryState(StreamIn &inStream)
-{
-	Shape::RestoreBinaryState(inStream);
-
-	inStream.Read(mCenterOfMass);
-	inStream.Read(mLocalBounds.mMin);
-	inStream.Read(mLocalBounds.mMax);
-	inStream.Read(mInnerRadius);
-
-	// Read sub shapes
-	inStream.Read(mSubShapes, [](StreamIn &inS, SubShape &outElement) {
-		inS.Read(outElement.mUserData);
-		inS.Read(outElement.mPositionCOM);
-		inS.Read(outElement.mRotation);
-		outElement.mIsRotationIdentity = outElement.mRotation == Float3(0, 0, 0);
-	});
-}
-
-void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const
-{
-	outSubShapes.clear();
-	outSubShapes.reserve(mSubShapes.size());
-	for (const SubShape &shape : mSubShapes)
-		outSubShapes.push_back(shape.mShape);
-}
-
-void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
-{
-	JPH_ASSERT(mSubShapes.size() == inNumShapes);
-	for (uint i = 0; i < inNumShapes; ++i)
-		mSubShapes[i].mShape = inSubShapes[i];
-}
-
-Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
-{
-	// Get own stats
-	Stats stats = Shape::GetStatsRecursive(ioVisitedShapes);
-
-	// Add child stats
-	for (const SubShape &shape : mSubShapes)
-	{
-		Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes);
-		stats.mSizeBytes += child_stats.mSizeBytes;
-		stats.mNumTriangles += child_stats.mNumTriangles;
-	}
-
-	return stats;
-}
-
-float CompoundShape::GetVolume() const
-{
-	float volume = 0.0f;
-	for (const SubShape &shape : mSubShapes)
-		volume += shape.mShape->GetVolume();
-	return volume;
-}
-
-bool CompoundShape::IsValidScale(Vec3Arg inScale) const
-{
-	if (!Shape::IsValidScale(inScale))
-		return false;
-
-	for (const SubShape &shape : mSubShapes)
-	{
-		// Test if the scale is non-uniform and the shape is rotated
-		if (!shape.IsValidScale(inScale))
-			return false;
-
-		// Test the child shape
-		if (!shape.mShape->IsValidScale(shape.TransformScale(inScale)))
-			return false;
-	}
-
-	return true;
-}
-
-Vec3 CompoundShape::MakeScaleValid(Vec3Arg inScale) const
-{
-	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
-	if (CompoundShape::IsValidScale(scale))
-		return scale;
-
-	Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
-	Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
-	if (CompoundShape::IsValidScale(uniform_scale))
-		return uniform_scale;
-
-	return Sign(scale.GetX()) * abs_uniform_scale;
-}
-
-void CompoundShape::sRegister()
-{
-	for (EShapeSubType s1 : sCompoundSubShapeTypes)
-		for (EShapeSubType s2 : sAllSubShapeTypes)
-			CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape);
-}
-
-JPH_NAMESPACE_END