Additional 3d support

BeefySysLib/util/Sphere.cpp

@@ -0,0 +1,195 @@
+#include "Sphere.h"
+#include "Matrix4.h"
+
+USING_NS_BF;
+
+static const float radiusEpsilon = 1e-4f;   // NOTE: To avoid numerical inaccuracies
+
+Sphere::Sphere()
+{
+	mRadius = -1;
+}
+
+Sphere::Sphere(const Sphere& S)
+{
+	mRadius = S.mRadius;
+	mCenter = S.mCenter;
+}
+
+Sphere::Sphere(const Vector3& O)
+{
+	mRadius = 0 + radiusEpsilon;
+	mCenter = O;
+}
+
+Sphere::Sphere(const Vector3& O, float R)
+{
+	mRadius = R;
+	mCenter = O;
+}
+
+Sphere::Sphere(const Vector3& O, const Vector3& A)
+{
+	Vector3 a = A - O;
+
+	Vector3 o = a * 0.5f;
+
+	mRadius = o.GetMagnitude() + radiusEpsilon;
+	mCenter = O + o;
+}
+
+Sphere::Sphere(const Vector3& O, const Vector3& A, const Vector3& B)
+{
+	Vector3 a = A - O;
+	Vector3 b = B - O;
+
+	Vector3 axb = Vector3::CrossProduct(a, b);
+
+	float Denominator = 2.0f * Vector3::Dot(axb, axb);
+
+	Vector3 o =
+		((Vector3::CrossProduct(axb, a) * Vector3::Dot(b, b)) +	
+		(Vector3::CrossProduct(b, axb) * Vector3::Dot(a, a))) / Denominator;
+
+	mRadius = o.GetMagnitude() + radiusEpsilon;
+	mCenter = O + o;
+}
+
+Sphere::Sphere(const Vector3& O, const Vector3& A, const Vector3& B, const Vector3& C)
+{
+	Vector3 a = A - O;
+	Vector3 b = B - O;
+	Vector3 c = C - O;
+
+	float Denominator = 2.0f * Matrix4::Determinant(
+		a.mX, a.mY, a.mZ,
+		b.mX, b.mY, b.mZ,
+		c.mX, c.mY, c.mZ);
+
+	Vector3 o = (
+		(Vector3::CrossProduct(a, b) * Vector3::Dot(c, c)) +
+		(Vector3::CrossProduct(c, a) * Vector3::Dot(b, b)) +
+		(Vector3::CrossProduct(b, c) * Vector3::Dot(a, a))) / Denominator;
+
+	mRadius = o.GetMagnitude() + radiusEpsilon;
+	mCenter = O + o;
+}
+
+Sphere& Sphere::operator=(const Sphere& S)
+{
+	mRadius = S.mRadius;
+	mCenter = S.mCenter;
+
+	return *this;
+}
+
+float Sphere::GetDistance(const Vector3& P) const
+{
+	return Vector3::GetDistance(P, mCenter) - mRadius;
+}
+
+float Sphere::GetDistanceSquare(const Vector3& P) const
+{
+	return Vector3::GetDistanceSquare(P, mCenter) - mRadius * mRadius;
+}
+
+float Sphere::GetDistance(const Sphere& S, const Vector3& P)
+{
+	return Vector3::GetDistance(P, S.mCenter) - S.mRadius;
+}
+
+float Sphere::GetDistance(const Vector3& P, const Sphere& S)
+{
+	return Vector3::GetDistance(P, S.mCenter) - S.mRadius;
+}
+
+float Sphere::GetDistanceSquare(const Sphere& S, const Vector3& P)
+{
+	return Vector3::GetDistanceSquare(P, S.mCenter) - S.mRadius * S.mRadius;
+}
+
+float Sphere::GetDistanceSquare(const Vector3& P, const Sphere& S)
+{
+	return Vector3::GetDistanceSquare(P, S.mCenter) - S.mRadius * S.mRadius;
+}
+
+Sphere Sphere::RecurseMini(Vector3* P[], int p, int b)
+{
+	Sphere MB;
+
+	switch (b)
+	{
+	case 0:
+		MB = Sphere();
+		break;
+	case 1:
+		MB = Sphere(*P[-1]);
+		break;
+	case 2:
+		MB = Sphere(*P[-1], *P[-2]);
+		break;
+	case 3:
+		MB = Sphere(*P[-1], *P[-2], *P[-3]);
+		break;
+	case 4:
+		MB = Sphere(*P[-1], *P[-2], *P[-3], *P[-4]);
+		return MB;
+	}
+
+	for (int i = 0; i < p; i++)
+	{
+		if (MB.GetDistanceSquare(*P[i]) > 0)   // Signed square distance to sphere
+		{
+			for (int j = i; j > 0; j--)
+			{
+				Vector3* T = P[j];
+				P[j] = P[j - 1];
+				P[j - 1] = T;
+			}
+
+			MB = RecurseMini(P + 1, i, b + 1);
+		}
+	}
+
+	return MB;
+}
+
+Sphere Sphere::MiniBall(Vector3 P[], int p)
+{
+	Vector3** L = new Vector3* [p];
+
+	for (int i = 0; i < p; i++)
+		L[i] = &P[i];
+
+	Sphere MB = RecurseMini(L, p);
+
+	delete[] L;
+
+	return MB;
+}
+
+Sphere Sphere::SmallBall(Vector3 P[], int p)
+{
+	Vector3 mCenter;
+	float mRadius = -1;
+
+	if (p > 0)
+	{		
+		for (int i = 0; i < p; i++)
+			mCenter += P[i];
+
+		mCenter /= (float)p;
+
+		for (int i = 0; i < p; i++)
+		{
+			float d2 = Vector3::GetDistanceSquare(P[i], mCenter);
+
+			if (d2 > mRadius)
+				mRadius = d2;
+		}
+
+		mRadius = sqrtf(mRadius) + radiusEpsilon;
+	}
+
+	return Sphere(mCenter, mRadius);
+}