initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Geometry/Sphere.h

@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+class [[nodiscard]] Sphere
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	inline				Sphere() = default;
+	inline				Sphere(const Float3 &inCenter, float inRadius)			: mCenter(inCenter), mRadius(inRadius) { }
+	inline				Sphere(Vec3Arg inCenter, float inRadius)				: mRadius(inRadius) { inCenter.StoreFloat3(&mCenter); }
+
+	/// Calculate the support vector for this convex shape.
+	inline Vec3			GetSupport(Vec3Arg inDirection) const
+	{
+		float length = inDirection.Length();
+		return length > 0.0f ? Vec3::sLoadFloat3Unsafe(mCenter) + (mRadius/ length) * inDirection : Vec3::sLoadFloat3Unsafe(mCenter);
+	}
+
+	// Properties
+	inline Vec3			GetCenter() const										{ return Vec3::sLoadFloat3Unsafe(mCenter); }
+	inline float		GetRadius() const										{ return mRadius; }
+
+	/// Test if two spheres overlap
+	inline bool			Overlaps(const Sphere &inB) const
+	{
+		return (Vec3::sLoadFloat3Unsafe(mCenter) - Vec3::sLoadFloat3Unsafe(inB.mCenter)).LengthSq() <= Square(mRadius + inB.mRadius);
+	}
+
+	/// Check if this sphere overlaps with a box
+	inline bool			Overlaps(const AABox &inOther) const
+	{
+		return inOther.GetSqDistanceTo(GetCenter()) <= Square(mRadius);
+	}
+
+	/// Create the minimal sphere that encapsulates this sphere and inPoint
+	inline void			EncapsulatePoint(Vec3Arg inPoint)
+	{
+		// Calculate distance between point and center
+		Vec3 center = GetCenter();
+		Vec3 d_vec = inPoint - center;
+		float d_sq = d_vec.LengthSq();
+		if (d_sq > Square(mRadius))
+		{
+			// It is further away than radius, we need to widen the sphere
+			// The diameter of the new sphere is radius + d, so the new radius is half of that
+			float d = sqrt(d_sq);
+			float radius = 0.5f * (mRadius + d);
+
+			// The center needs to shift by new radius - old radius in the direction of d
+			center += (radius - mRadius) / d * d_vec;
+
+			// Store new sphere
+			center.StoreFloat3(&mCenter);
+			mRadius = radius;
+		}
+	}
+
+private:
+	Float3				mCenter;
+	float				mRadius;
+};
+
+JPH_NAMESPACE_END