@bloomengine/engine 0.3.1

package/native/third_party/JoltPhysics/Jolt/Geometry/ConvexHullBuilder.h

@@ -0,0 +1,276 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+//#define JPH_CONVEX_BUILDER_DEBUG
+//#define JPH_CONVEX_BUILDER_DUMP_SHAPE
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	#include <Jolt/Core/Color.h>
+#endif
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder that tries to create hulls as accurately as possible. Used for offline processing.
+class JPH_EXPORT ConvexHullBuilder : public NonCopyable
+{
+public:
+	// Forward declare
+	class Face;
+
+	/// Class that holds the information of an edge
+	class Edge : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+						Edge(Face *inFace, int inStartIdx)	: mFace(inFace), mStartIdx(inStartIdx) { }
+
+		/// Get the previous edge
+		inline Edge *	GetPreviousEdge()
+		{
+			Edge *prev_edge = this;
+			while (prev_edge->mNextEdge != this)
+				prev_edge = prev_edge->mNextEdge;
+			return prev_edge;
+		}
+
+		Face *			mFace;								///< Face that this edge belongs to
+		Edge *			mNextEdge = nullptr;				///< Next edge of this face
+		Edge *			mNeighbourEdge = nullptr;			///< Edge that this edge is connected to
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+	};
+
+	using ConflictList = Array<int>;
+
+	/// Class that holds the information of one face
+	class Face : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Destructor
+						~Face();
+
+		/// Initialize a face with three indices
+		void			Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
+
+		/// Calculates the centroid and normal for this face
+		void			CalculateNormalAndCentroid(const Vec3 *inPositions);
+
+		/// Check if face inFace is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(inPosition - mCentroid) > 0.0f;
+		}
+
+		Vec3			mNormal;							///< Normal of this face, length is 2 times area of face
+		Vec3			mCentroid;							///< Center of the face
+		ConflictList	mConflictList;						///< Positions associated with this edge (that are closest to this edge). The last position in the list is the point that is furthest away from the face.
+		Edge *			mFirstEdge = nullptr;				///< First edge of this face
+		float			mFurthestPointDistanceSq = 0.0f;	///< Squared distance of furthest point from the conflict list to the face
+		bool			mRemoved = false;					///< Flag that indicates that face has been removed (face will be freed later)
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		int				mIteration;							///< Iteration that this face was created
+#endif
+	};
+
+	// Typedefs
+	using Positions = Array<Vec3>;
+	using Faces = Array<Face *>;
+
+	/// Constructor
+	explicit			ConvexHullBuilder(const Positions &inPositions);
+
+	/// Destructor
+						~ConvexHullBuilder()				{ FreeFaces(); }
+
+	/// Result enum that indicates how the hull got created
+	enum class EResult
+	{
+		Success,											///< Hull building finished successfully
+		MaxVerticesReached,									///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+		TooFewPoints,										///< Too few points to create a hull
+		TooFewFaces,										///< Too few faces in the created hull (signifies precision errors during building)
+		Degenerate,											///< Degenerate hull detected
+	};
+
+	/// Takes all positions as provided by the constructor and use them to build a hull
+	/// Any points that are closer to the hull than inTolerance will be discarded
+	/// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit.
+	/// @param inTolerance Max distance that a point is allowed to be outside of the hull
+	/// @param outError Error message when building fails
+	/// @return Status code that reports if the hull was created or not
+	EResult				Initialize(int inMaxVertices, float inTolerance, const char *&outError);
+
+	/// Returns the amount of vertices that are currently used by the hull
+	int					GetNumVerticesUsed() const;
+
+	/// Returns true if the hull contains a polygon with inIndices (counter clockwise indices in mPositions)
+	bool				ContainsFace(const Array<int> &inIndices) const;
+
+	/// Calculate the center of mass and the volume of the current convex hull
+	void				GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const;
+
+	/// Determines the point that is furthest outside of the hull and reports how far it is outside of the hull (which indicates a failure during hull building)
+	/// @param outFaceWithMaxError The face that caused the error
+	/// @param outMaxError The maximum distance of a point to the hull
+	/// @param outMaxErrorPositionIdx The index of the point that had this distance
+	/// @param outCoplanarDistance Points that are less than this distance from the hull are considered on the hull. This should be used as a lowerbound for the allowed error.
+	void				DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const;
+
+	/// Access to the created faces. Memory is owned by the convex hull builder.
+	const Faces &		GetFaces() const					{ return mFaces; }
+
+private:
+	/// Minimal square area of a triangle (used for merging and checking if a triangle is degenerate)
+	static constexpr float cMinTriangleAreaSq = 1.0e-12f;
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+	/// Class that holds an edge including start and end index
+	class FullEdge
+	{
+	public:
+		Edge *			mNeighbourEdge;						///< Edge that this edge is connected to
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+		int				mEndIdx;							///< Vertex index in mPosition that indicates the end vertex of this edge
+	};
+
+	// Private typedefs
+	using FullEdges = Array<FullEdge>;
+
+	// Determine a suitable tolerance for detecting that points are coplanar
+	float				DetermineCoplanarDistance() const;
+
+	/// Find the face for which inPoint is furthest to the front
+	/// @param inPoint Point to test
+	/// @param inFaces List of faces to test
+	/// @param outFace Returns the best face
+	/// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face
+	void				GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const;
+
+	/// @brief Calculates the distance between inPoint and inFace
+	/// @param inFace Face to test
+	/// @param inPoint Point to test
+	/// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge
+	float				GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const;
+
+	/// Assigns a position to one of the supplied faces based on which face is closest.
+	/// @param inPositionIdx Index of the position to add
+	/// @param inFaces List of faces to consider
+	/// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it
+	/// @return True if point was assigned, false if it was discarded or added to the coplanar list
+	bool				AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq);
+
+	/// Add a new point to the convex hull
+	void				AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces);
+
+	/// Remove all faces that have been marked 'removed' from mFaces list
+	void				GarbageCollectFaces();
+
+	/// Create a new face
+	Face *				CreateFace();
+
+	/// Create a new triangle
+	Face *				CreateTriangle(int inIdx1, int inIdx2, int inIdx3);
+
+	/// Delete a face (checking that it is not connected to any other faces)
+	void				FreeFace(Face *inFace);
+
+	/// Release all faces and edges
+	void				FreeFaces();
+
+	/// Link face edge to other face edge
+	static void			sLinkFace(Edge *inEdge1, Edge *inEdge2);
+
+	/// Unlink this face from all of its neighbours
+	static void			sUnlinkFace(Face *inFace);
+
+	/// Given one face that faces inVertex, find the edges of the faces that are not facing inVertex.
+	/// Will flag all those faces for removal.
+	void				FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const;
+
+	/// Merges the two faces that share inEdge into the face inEdge->mFace
+	void				MergeFaces(Edge *inEdge);
+
+	/// Merges inFace with a neighbour if it is degenerate (a sliver)
+	void				MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Merges any coplanar as well as neighbours that form a non-convex edge into inFace.
+	/// Faces are considered coplanar if the distance^2 of the other face's centroid is smaller than inToleranceSq.
+	void				MergeCoplanarOrConcaveFaces(Face *inFace, float inToleranceSq, Faces &ioAffectedFaces);
+
+	/// Mark face as affected if it is not already in the list
+	static void			sMarkAffected(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Removes all invalid edges.
+	/// 1. Merges inFace with faces that share two edges with it since this means inFace or the other face cannot be convex or the edge is colinear.
+	/// 2. Removes edges that are interior to inFace (that have inFace on both sides)
+	/// Any faces that need to be checked for validity will be added to ioAffectedFaces.
+	void				RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Removes inFace if it consists of only 2 edges, linking its neighbouring faces together
+	/// Any faces that need to be checked for validity will be added to ioAffectedFaces.
+	/// @return True if face was removed.
+	bool				RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Dumps the text representation of a face to the TTY
+	void				DumpFace(const Face *inFace) const;
+
+	/// Dumps the text representation of all faces to the TTY
+	void				DumpFaces() const;
+
+	/// Check consistency of 1 face
+	void				ValidateFace(const Face *inFace) const;
+
+	/// Check consistency of all faces
+	void				ValidateFaces() const;
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	/// Draw state of algorithm
+	void				DrawState(bool inDrawConflictList = false) const;
+
+	/// Draw a face for debugging purposes
+	void				DrawWireFace(const Face *inFace, ColorArg inColor) const;
+
+	/// Draw an edge for debugging purposes
+	void				DrawEdge(const Edge *inEdge, ColorArg inColor) const;
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE
+	void				DumpShape() const;
+#endif
+
+	const Positions &	mPositions;							///< List of positions (some of them are part of the hull)
+	Faces				mFaces;								///< List of faces that are part of the hull (if !mRemoved)
+
+	struct Coplanar
+	{
+		int				mPositionIdx;						///< Index in mPositions
+		float			mDistanceSq;						///< Distance to the edge of closest face (should be > 0)
+	};
+	using CoplanarList = Array<Coplanar>;
+
+	CoplanarList		mCoplanarList;						///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)
+	mutable RVec3		mOffset;							///< Offset to use for state drawing
+	Vec3				mDelta;								///< Delta offset between next states
+#endif
+};
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Geometry/ConvexHullBuilder2D.cpp

@@ -0,0 +1,335 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/ConvexHullBuilder2D.h>
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+void ConvexHullBuilder2D::Edge::CalculateNormalAndCenter(const Vec3 *inPositions)
+{
+	Vec3 p1 = inPositions[mStartIdx];
+	Vec3 p2 = inPositions[mNextEdge->mStartIdx];
+
+	// Center of edge
+	mCenter = 0.5f * (p1 + p2);
+
+	// Create outward pointing normal.
+	// We have two choices for the normal (which satisfies normal . edge = 0):
+	// normal1 = (-edge.y, edge.x, 0)
+	// normal2 = (edge.y, -edge.x, 0)
+	// We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition.
+	Vec3 edge = p2 - p1;
+	mNormal = Vec3(edge.GetY(), -edge.GetX(), 0);
+}
+
+ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) :
+	mPositions(inPositions)
+{
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	// Center the drawing of the first hull around the origin and calculate the delta offset between states
+	mOffset = RVec3::sZero();
+	if (mPositions.empty())
+	{
+		// No hull will be generated
+		mDelta = Vec3::sZero();
+	}
+	else
+	{
+		Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX);
+		for (Vec3 v : mPositions)
+		{
+			minv = Vec3::sMin(minv, v);
+			maxv = Vec3::sMax(maxv, v);
+			mOffset -= v;
+		}
+		mOffset /= Real(mPositions.size());
+		mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
+		mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
+	}
+#endif
+}
+
+ConvexHullBuilder2D::~ConvexHullBuilder2D()
+{
+	FreeEdges();
+}
+
+void ConvexHullBuilder2D::FreeEdges()
+{
+	if (mFirstEdge == nullptr)
+		return;
+
+	Edge *edge = mFirstEdge;
+	do
+	{
+		Edge *next = edge->mNextEdge;
+		delete edge;
+		edge = next;
+	} while (edge != mFirstEdge);
+
+	mFirstEdge = nullptr;
+	mNumEdges = 0;
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder2D::ValidateEdges() const
+{
+	if (mFirstEdge == nullptr)
+	{
+		JPH_ASSERT(mNumEdges == 0);
+		return;
+	}
+
+	int count = 0;
+
+	Edge *edge = mFirstEdge;
+	do
+	{
+		// Validate connectivity
+		JPH_ASSERT(edge->mNextEdge->mPrevEdge == edge);
+		JPH_ASSERT(edge->mPrevEdge->mNextEdge == edge);
+
+		++count;
+		edge = edge->mNextEdge;
+	} while (edge != mFirstEdge);
+
+	// Validate that count matches
+	JPH_ASSERT(count == mNumEdges);
+}
+
+#endif // JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder2D::AssignPointToEdge(int inPositionIdx, const Array<Edge *> &inEdges) const
+{
+	Vec3 point = mPositions[inPositionIdx];
+
+	Edge *best_edge = nullptr;
+	float best_dist_sq = 0.0f;
+
+	// Test against all edges
+	for (Edge *edge : inEdges)
+	{
+		// Determine distance to edge
+		float dot = edge->mNormal.Dot(point - edge->mCenter);
+		if (dot > 0.0f)
+		{
+			float dist_sq = dot * dot / edge->mNormal.LengthSq();
+			if (dist_sq > best_dist_sq)
+			{
+				best_edge = edge;
+				best_dist_sq = dist_sq;
+			}
+		}
+	}
+
+	// If this point is in front of the edge, add it to the conflict list
+	if (best_edge != nullptr)
+	{
+		if (best_dist_sq > best_edge->mFurthestPointDistanceSq)
+		{
+			// This point is further away than any others, update the distance and add point as last point
+			best_edge->mFurthestPointDistanceSq = best_dist_sq;
+			best_edge->mConflictList.push_back(inPositionIdx);
+		}
+		else
+		{
+			// Not the furthest point, add it as the before last point
+			best_edge->mConflictList.insert(best_edge->mConflictList.begin() + best_edge->mConflictList.size() - 1, inPositionIdx);
+		}
+	}
+}
+
+ConvexHullBuilder2D::EResult ConvexHullBuilder2D::Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges)
+{
+	// Clear any leftovers
+	FreeEdges();
+	outEdges.clear();
+
+	// Reset flag
+	EResult result = EResult::Success;
+
+	// Determine a suitable tolerance for detecting that points are colinear
+	// Formula as per: Implementing Quickhull - Dirk Gregorius.
+	Vec3 vmax = Vec3::sZero();
+	for (Vec3 v : mPositions)
+		vmax = Vec3::sMax(vmax, v.Abs());
+	float colinear_tolerance_sq = Square(2.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY()));
+
+	// Increase desired tolerance if accuracy doesn't allow it
+	float tolerance_sq = max(colinear_tolerance_sq, Square(inTolerance));
+
+	// Start with the initial indices in counter clockwise order
+	float z = (mPositions[inIdx2] - mPositions[inIdx1]).Cross(mPositions[inIdx3] - mPositions[inIdx1]).GetZ();
+	if (z < 0.0f)
+		std::swap(inIdx1, inIdx2);
+
+	// Create and link edges
+	Edge *e1 = new Edge(inIdx1);
+	Edge *e2 = new Edge(inIdx2);
+	Edge *e3 = new Edge(inIdx3);
+	e1->mNextEdge = e2;
+	e1->mPrevEdge = e3;
+	e2->mNextEdge = e3;
+	e2->mPrevEdge = e1;
+	e3->mNextEdge = e1;
+	e3->mPrevEdge = e2;
+	mFirstEdge = e1;
+	mNumEdges = 3;
+
+	// Build the initial conflict lists
+	Array<Edge *> edges { e1, e2, e3 };
+	for (Edge *edge : edges)
+		edge->CalculateNormalAndCenter(mPositions.data());
+	for (int idx = 0; idx < (int)mPositions.size(); ++idx)
+		if (idx != inIdx1 && idx != inIdx2 && idx != inIdx3)
+			AssignPointToEdge(idx, edges);
+
+	JPH_IF_ENABLE_ASSERTS(ValidateEdges();)
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	DrawState();
+#endif
+
+	// Add the remaining points to the hull
+	for (;;)
+	{
+		// Check if we've reached the max amount of vertices that are allowed
+		if (mNumEdges >= inMaxVertices)
+		{
+			result = EResult::MaxVerticesReached;
+			break;
+		}
+
+		// Find the edge with the furthest point on it
+		Edge *edge_with_furthest_point = nullptr;
+		float furthest_dist_sq = 0.0f;
+		Edge *edge = mFirstEdge;
+		do
+		{
+			if (edge->mFurthestPointDistanceSq > furthest_dist_sq)
+			{
+				furthest_dist_sq = edge->mFurthestPointDistanceSq;
+				edge_with_furthest_point = edge;
+			}
+			edge = edge->mNextEdge;
+		} while (edge != mFirstEdge);
+
+		// If there is none closer than our tolerance value, we're done
+		if (edge_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq)
+			break;
+
+		// Take the furthest point
+		int furthest_point_idx = edge_with_furthest_point->mConflictList.back();
+		edge_with_furthest_point->mConflictList.pop_back();
+		Vec3 furthest_point = mPositions[furthest_point_idx];
+
+		// Find the horizon of edges that need to be removed
+		Edge *first_edge = edge_with_furthest_point;
+		do
+		{
+			Edge *prev = first_edge->mPrevEdge;
+			if (!prev->IsFacing(furthest_point))
+				break;
+			first_edge = prev;
+		} while (first_edge != edge_with_furthest_point);
+
+		Edge *last_edge = edge_with_furthest_point;
+		do
+		{
+			Edge *next = last_edge->mNextEdge;
+			if (!next->IsFacing(furthest_point))
+				break;
+			last_edge = next;
+		} while (last_edge != edge_with_furthest_point);
+
+		// Create new edges
+		e1 = new Edge(first_edge->mStartIdx);
+		e2 = new Edge(furthest_point_idx);
+		e1->mNextEdge = e2;
+		e1->mPrevEdge = first_edge->mPrevEdge;
+		e2->mPrevEdge = e1;
+		e2->mNextEdge = last_edge->mNextEdge;
+		e1->mPrevEdge->mNextEdge = e1;
+		e2->mNextEdge->mPrevEdge = e2;
+		mFirstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge
+		mNumEdges += 2;
+
+		// Calculate normals
+		Array<Edge *> new_edges { e1, e2 };
+		for (Edge *new_edge : new_edges)
+			new_edge->CalculateNormalAndCenter(mPositions.data());
+
+		// Delete the old edges
+		for (;;)
+		{
+			Edge *next = first_edge->mNextEdge;
+
+			// Redistribute points in conflict list
+			for (int idx : first_edge->mConflictList)
+				AssignPointToEdge(idx, new_edges);
+
+			// Delete the old edge
+			delete first_edge;
+			--mNumEdges;
+
+			if (first_edge == last_edge)
+				break;
+			first_edge = next;
+		}
+
+		JPH_IF_ENABLE_ASSERTS(ValidateEdges();)
+	#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+		DrawState();
+	#endif
+	}
+
+	// Convert the edge list to a list of indices
+	outEdges.reserve(mNumEdges);
+	Edge *edge = mFirstEdge;
+	do
+	{
+		outEdges.push_back(edge->mStartIdx);
+		edge = edge->mNextEdge;
+	} while (edge != mFirstEdge);
+
+	return result;
+}
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+
+void ConvexHullBuilder2D::DrawState()
+{
+	int color_idx = 0;
+
+	const Edge *edge = mFirstEdge;
+	do
+	{
+		const Edge *next = edge->mNextEdge;
+
+		// Get unique color per edge
+		Color color = Color::sGetDistinctColor(color_idx++);
+
+		// Draw edge and normal
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f);
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
+
+		// Draw points that belong to this edge in the same color
+		for (int idx : edge->mConflictList)
+			DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), color, 0.05f);
+
+		edge = next;
+	} while (edge != mFirstEdge);
+
+	mOffset += mDelta;
+}
+
+#endif
+
+JPH_NAMESPACE_END