Additional 3d support

BeefySysLib/util/MathUtils.cpp

@@ -0,0 +1,145 @@
+#include "MathUtils.h"
+#include "Sphere.h"
+
+USING_NS_BF;
+
+bool Beefy::RayIntersectsTriangle(const Vector3& rayOrigin, const Vector3& rayVector, const Vector3& vtx0, const Vector3& vtx1, const Vector3& vtx2, Vector3* outIntersectionPoint, float* outDistance)
+{
+	const float EPSILON = 0.0000001f;	
+	Vector3 edge1, edge2, h, s, q;
+	float a, f, u, v;
+	edge1 = vtx1 - vtx0;
+	edge2 = vtx2 - vtx0;
+	
+	h = Vector3::CrossProduct(rayVector, edge2);//rayVector.crossProduct(edge2);
+	a = Vector3::Dot(edge1, h); //edge1.dotProduct(h);
+	if (a > -EPSILON && a < EPSILON)
+		return false;    // This ray is parallel to this triangle.
+	f = 1.0f / a;
+	s = rayOrigin - vtx0;
+	u = f * Vector3::Dot(s, h); //s.dotProduct(h);
+	if (u < 0.0 || u > 1.0)
+		return false;
+	q = Vector3::CrossProduct(s, edge1); //s.crossProduct(edge1);
+	v = f * Vector3::Dot(rayVector, q); //rayVector.dotProduct(q);
+	if (v < 0.0 || u + v > 1.0)
+		return false;
+	// At this stage we can compute t to find out where the intersection point is on the line.
+	float distance = f * Vector3::Dot(edge2, q); //edge2.dotProduct(q);
+	if (distance > EPSILON) // ray intersection
+	{
+		if (outIntersectionPoint != NULL)
+			*outIntersectionPoint = rayOrigin + rayVector * distance;
+		if (outDistance != NULL)
+			*outDistance = distance;
+		return true;
+	}
+	else // This means that there is a line intersection but not a ray intersection.
+		return false;
+}
+
+bool Beefy::RayIntersectsCircle(const Vector3& rayOrigin, const Vector3& rayVector, const Sphere& sphere, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance)
+{
+	Vector3 e = sphere.mCenter - rayOrigin;
+	float rSq = sphere.mRadius * sphere.mRadius;
+
+	float eSq = e.GetMagnitudeSquare();
+	float a = Vector3::Dot(e, rayVector); // ray.direction is assumed to be normalized
+	float bSq = /*sqrtf(*/eSq - (a * a)/*)*/;
+	float f = sqrtf(fabsf((rSq)- /*(b * b)*/bSq));
+
+	// Assume normal intersection!
+	float t = a - f;
+
+	// No collision has happened
+	if (rSq - (eSq - a * a) < 0.0f) 
+	{
+		return false;
+	}
+	// Ray starts inside the sphere
+	else if (eSq < rSq) 
+	{
+		// Just reverse direction
+		t = a + f;
+	}
+		
+	if (outDistance != NULL)
+		*outDistance = t; 
+	if ((outIntersectionPoint != NULL) || (outNormal != NULL))
+	{
+		Vector3 intersect = rayOrigin + rayVector * t;
+		if (outIntersectionPoint != NULL)
+			*outIntersectionPoint = intersect;
+		if (outNormal != NULL)
+			*outNormal = Vector3::Normalize(intersect - sphere.mCenter);
+	}
+
+	return true;
+}
+
+bool Beefy::RayIntersectsAABB(const Vector3& rayOrigin, const Vector3& rayVector, const AABB& aabb, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance)
+{
+	const Vector3& min = aabb.mMin;
+	const Vector3& max = aabb.mMax;
+
+	// Any component of direction could be 0!
+	// Address this by using a small number, close to
+	// 0 in case any of directions components are 0
+	float t1 = (min.mX - rayOrigin.mX) / ((fabs(rayVector.mX) < 0.00001f) ? 0.00001f : rayVector.mX);
+	float t2 = (max.mX - rayOrigin.mX) / ((fabs(rayVector.mX) < 0.00001f) ? 0.00001f : rayVector.mX);
+	float t3 = (min.mY - rayOrigin.mY) / ((fabs(rayVector.mY) < 0.00001f) ? 0.00001f : rayVector.mY);
+	float t4 = (max.mY - rayOrigin.mY) / ((fabs(rayVector.mY) < 0.00001f) ? 0.00001f : rayVector.mY);
+	float t5 = (min.mZ - rayOrigin.mZ) / ((fabs(rayVector.mZ) < 0.00001f) ? 0.00001f : rayVector.mZ);
+	float t6 = (max.mZ - rayOrigin.mZ) / ((fabs(rayVector.mZ) < 0.00001f) ? 0.00001f : rayVector.mZ);
+
+	float tmin = fmaxf(fmaxf(fminf(t1, t2), fminf(t3, t4)), fminf(t5, t6));
+	float tmax = fminf(fminf(fmaxf(t1, t2), fmaxf(t3, t4)), fmaxf(t5, t6));
+
+	// if tmax < 0, ray is intersecting AABB
+	// but entire AABB is being it's origin
+	if (tmax < 0) {
+		return false;
+	}
+
+	// if tmin > tmax, ray doesn't intersect AABB
+	if (tmin > tmax) {
+		return false;
+	}
+
+	float t_result = tmin;
+
+	// If tmin is < 0, tmax is closer
+	if (tmin < 0.0f) {
+		t_result = tmax;
+	}
+
+	if ((outIntersectionPoint != NULL) || (outDistance != NULL))
+	{
+		Vector3 hitPoint = rayOrigin + rayVector * t_result;
+		if (outIntersectionPoint != NULL)
+			*outIntersectionPoint = hitPoint;
+		if (outDistance != NULL)
+			*outDistance = (rayOrigin - hitPoint).GetMagnitude();		
+	}
+	
+	if (outNormal != NULL)
+	{
+		static Vector3 normals[] = {
+			Vector3(-1, 0, 0),
+			Vector3(1, 0, 0),
+			Vector3(0, -1, 0),
+			Vector3(0, 1, 0),
+			Vector3(0, 0, -1),
+			Vector3(0, 0, 1)
+		};
+		float t[] = { t1, t2, t3, t4, t5, t6 };
+
+		for (int i = 0; i < 6; ++i) 
+		{
+			if (t_result == t[i])
+				*outNormal = normals[i];
+		}
+	}
+
+	return true;	
+}

BeefySysLib/util/MathUtils.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "Vector.h"
+#include "Matrix4.h"
+
+NS_BF_BEGIN
+
+class AABB
+{
+public:
+	Vector3 mMin;
+	Vector3 mMax;
+};
+
+class Sphere;
+
+bool RayIntersectsTriangle(const Vector3& rayOrigin, const Vector3& rayVector, const Vector3& vtx0, const Vector3& vtx1, const Vector3& vtx2, Vector3* outIntersectionPoint, float* distance);
+bool RayIntersectsCircle(const Vector3& rayOrigin, const Vector3& rayVector, const Sphere& sphere, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance);
+bool RayIntersectsAABB(const Vector3& rayOrigin, const Vector3& rayVector, const AABB& aabb, Vector3* outIntersectionPoint, Vector3* outNormal, float* outDistance);
+
+NS_BF_END

BeefySysLib/util/Matrix4.h

@@ -109,6 +109,15 @@ public:
 	}
 
 	static Matrix4 CreateTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation);
+
+	static float Determinant(float m11, float m12, float m13,
+		float m21, float m22, float m23,
+		float m31, float m32, float m33)
+	{
+		return m11 * (m22 * m33 - m32 * m23) -
+			m21 * (m12 * m33 - m32 * m13) +
+			m31 * (m12 * m23 - m22 * m13);
+	}
 };
 
 NS_BF_END;

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);
+}

BeefySysLib/util/Sphere.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "Common.h"
+#include "Vector.h"
+
+NS_BF_BEGIN
+
+class Vector3;
+class Matrix4;
+
+class Sphere
+{
+public:
+	Vector3 mCenter;
+	float mRadius;
+
+	Sphere();
+	Sphere(const Sphere& X);
+	Sphere(const Vector3& O);   // Point-Sphere
+	Sphere(const Vector3& O, float R);   // Center and radius (not squared)
+	Sphere(const Vector3& O, const Vector3& A);   // Sphere through two points
+	Sphere(const Vector3& O, const Vector3& A, const Vector3& B);   // Sphere through three points
+	Sphere(const Vector3& O, const Vector3& A, const Vector3& B, const Vector3& C);   // Sphere through four points
+
+	Sphere& operator=(const Sphere& S);
+
+	float GetDistance(const Vector3& P) const;  // Distance from p to boundary of the Sphere
+	float GetDistanceSquare(const Vector3& P) const;  // Square distance from p to boundary of the Sphere
+
+	static float GetDistance(const Sphere& S, const Vector3& P);  // Distance from p to boundary of the Sphere
+	static float GetDistance(const Vector3& P, const Sphere& S);  // Distance from p to boundary of the Sphere
+
+	static float GetDistanceSquare(const Sphere& S, const Vector3& P);  // Square distance from p to boundary of the Sphere
+	static float GetDistanceSquare(const Vector3& P, const Sphere& S);  // Square distance from p to boundary of the Sphere
+
+	static Sphere MiniBall(Vector3 P[], int p);   // Smallest enclosing sphere
+	static Sphere SmallBall(Vector3 P[], int p);   // Enclosing sphere approximation
+
+private:
+	static Sphere RecurseMini(Vector3* P[], int p, int b = 0);
+};
+
+NS_BF_END

BeefySysLib/util/Vector.cpp

@@ -4,6 +4,13 @@
 
 USING_NS_BF;
 
+Vector3::Vector3()
+{
+	mX = 0;
+	mY = 0;
+	mZ = 0;
+}
+
 Vector3::Vector3(float x, float y, float z)
 {
 	mX = x;
@@ -16,6 +23,11 @@ float Vector3::GetMagnitude() const
 	return sqrtf(mX*mX + mY*mY + mZ*mZ);
 }
 
+float Vector3::GetMagnitudeSquare() const
+{
+	return mX * mX + mY * mY + mZ * mZ;
+}
+
 Vector3 Vector3::Normalize(const Vector3& vec)
 {
 	float mag = vec.GetMagnitude();
@@ -39,6 +51,14 @@ Vector3 Vector3::CrossProduct(const Vector3& vec1, const Vector3& vec2)
 }
 
 Vector3 Vector3::Transform(const Vector3& vec, const Matrix4& matrix)
+{
+	return Vector3(
+		(matrix.m00 * vec.mX + matrix.m01 * vec.mY + matrix.m02 * vec.mZ + matrix.m03),
+		(matrix.m10 * vec.mX + matrix.m11 * vec.mY + matrix.m12 * vec.mZ + matrix.m13),
+		(matrix.m20 * vec.mX + matrix.m21 * vec.mY + matrix.m22 * vec.mZ + matrix.m23));
+}
+
+Vector3 Vector3::TransformW(const Vector3& vec, const Matrix4& matrix)
 {
 	float fInvW = 1.0f / (matrix.m30 * vec.mX + matrix.m31 * vec.mY + matrix.m32 * vec.mZ + matrix.m33);
 

BeefySysLib/util/Vector.h

@@ -36,13 +36,25 @@ public:
 	float mZ;
 
 public:
-	Vector3(float x = 0, float y = 0, float z = 0);
+	Vector3();
+	Vector3(float x, float y, float z);
 
 	float GetMagnitude() const;
+	float GetMagnitudeSquare() const;
 	static Vector3 Normalize(const Vector3& vec);
 	static float Dot(const Vector3& vec1, const Vector3& vec2);
 	static Vector3 CrossProduct(const Vector3& vec1, const Vector3& vec2);
 	
+	static float GetDistance(const Vector3& v0, const Vector3& v1)
+	{
+		return (v0 - v1).GetMagnitude();
+	}
+
+	static float GetDistanceSquare(const Vector3& v0, const Vector3& v1)
+	{
+		return (v0 - v1).GetMagnitudeSquare();
+	}
+
 	bool operator==(const Vector3& check) const
 	{
 		return (mX == check.mX) && (mY == check.mY) && (mZ == check.mZ);			
@@ -54,8 +66,9 @@ public:
 	}
 
 	static Vector3 Transform(const Vector3& vec, const Matrix4& matrix);
+	static Vector3 TransformW(const Vector3& vec, const Matrix4& matrix);
 	static Vector3 Transform(const Vector3& vec, const Quaternion& quat);
-	static Vector3 Transform2(const Vector3& vec, const Quaternion& quat);
+	static Vector3 Transform2(const Vector3& vec, const Quaternion& quat);	
 
 	static Vector3 Scale(const Vector3& vec, float scale)
 	{
@@ -77,6 +90,24 @@ public:
 		return Vector3(mX * scale, mY * scale, mZ * scale);
 	}
 
+	Vector3 operator /(float scale) const
+	{
+		return Vector3(mX / scale, mY / scale, mZ / scale);
+	}
+
+	float operator^(const Vector3& v) // Angle between vectors
+	{
+		return acosf(Dot(*this / this->GetMagnitude(), v / v.GetMagnitude()));
+	}
+	
+	inline Vector3& operator += (const Vector3& vec)
+	{
+		mX += vec.mX;
+		mY += vec.mY;
+		mZ += vec.mZ;
+		return *this;
+	}
+
 	inline Vector3& operator -= (const Vector3& vec)
 	{
 		mX -= vec.mX;
@@ -85,6 +116,40 @@ public:
 		return *this;
 	}
 
+	inline Vector3& operator *= (float num)
+	{
+		mX *= num;
+		mY *= num;
+		mZ *= num;
+		return *this;
+	}
+
+	inline Vector3& operator /= (float num)
+	{
+		mX /= num;
+		mY /= num;
+		mZ /= num;
+		return *this;
+	}
+
+	inline Vector3 operator - (const Vector3& vec) const
+	{
+		Vector3 result;
+		result.mX = mX - vec.mX;
+		result.mY = mY - vec.mY;
+		result.mZ = mZ - vec.mZ;
+		return result;
+	}
+
+	inline Vector3 operator + (const Vector3& vec)
+	{
+		Vector3 result;
+		result.mX = mX + vec.mX;
+		result.mY = mY + vec.mY;
+		result.mZ = mZ + vec.mZ;
+		return result;
+	}
+
 	inline Vector3& operator *= (const Vector3& vec)
 	{
 		mX *= vec.mX;