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