JoltPhysics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h

// 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