GEEKING

Rendering/RbxG3D/Frustum.cpp

@@ -0,0 +1,148 @@
+#include "RbxG3D/Frustum.h"
+#include "rbx/Debug.h"
+#include "util/Math.h"
+#include "util/Extents.h"
+
+using G3D::Plane;
+using G3D::Vector3;
+
+using namespace RBX;
+
+// creates a frustum in a local coordinate space with the apex at the origin
+// translates the frustum into the space defined by apex, dir and up
+// performing the floating point operations in a local space helps reduce numerical errors when all of the points are far from the origin
+Frustum::Frustum(const Vector3& apex, const Vector3& dir, const Vector3& up, const float nearDist, const float farDist, const float fovx, const float fovy)
+{
+	Matrix3 rotation;
+
+	// up and dir should already be normalized and perpendicular to each other
+	RBXASSERT(dir.isUnit());
+	RBXASSERT(up.isUnit());
+
+	// due to rounding errors the cross product may not be a unit vector even though it should be.
+	const Vector3 right = up.cross(dir).unit();
+
+	rotation.setColumn(0, right);
+	rotation.setColumn(1, up);
+	rotation.setColumn(2, -dir);
+
+	// Near plane (wind backwards so normal faces into frustum)
+	// Recall that nearPlane, farPlane are positive numbers, so
+	// we need to negate them to produce actual z values.
+	faceArray.append(Plane(Vector3(0.0f, 0.0f, -1.0f), Vector3(0,0,-nearDist)));
+
+	// Right plane
+	faceArray.append(Plane(Vector3(-cosf(fovx/2.0f), 0.0f, -sinf(fovx/2.0f)), Vector3::zero()));
+
+	// Left plane
+	faceArray.append(Plane(Vector3(-faceArray.last().normal().x, 0.0f, faceArray.last().normal().z), Vector3::zero()));
+
+	// Bottom plane
+	faceArray.append(Plane(Vector3(0.0f, cosf(fovy/2.0f), -sinf(fovy/2.0f)), Vector3::zero()));
+
+	// Top plane
+	faceArray.append(Plane(Vector3(0.0f, -faceArray.last().normal().y, faceArray.last().normal().z), Vector3::zero()));
+
+	// Far plane
+	if (farDist < inf()) 
+	{
+		faceArray.append(Plane(Vector3(0.0f, 0.0f, 1.0f), Vector3(0.0f, 0.0f, -farDist)));
+	}
+
+	// Transform planes to world space
+	for (int face = 0; face < faceArray.size(); ++face) 
+	{
+		// Since there is no scale factor, we don't have to 
+		// worry about the inverse transpose of the normal.
+		Vector3 normal;
+		float d;
+
+		faceArray[face].getEquation(normal, d);
+
+		Vector3 newNormal = rotation * normal;
+
+		if (G3D::isFinite(d)) {
+			d = (newNormal * -d + apex).dot(newNormal);
+			faceArray[face] = Plane(newNormal, newNormal * d);
+		} else {
+			// When d is infinite, we can't multiply 0's by it without
+			// generating NaNs.
+			faceArray[face] = Plane::fromEquation(newNormal.x, newNormal.y, newNormal.z, d);
+		}
+	}
+}
+
+bool Frustum::containsAABB(const Extents& aabb) const
+{
+	for (int ii = 0; ii < 8; ++ii)
+	{
+		Vector3 corner = aabb.getCorner(ii);
+		if (!containsPoint(corner))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool Frustum::intersectsAABB(const RBX::Extents& aabb, const G3D::CoordinateFrame& extentsFrame) const
+{
+    RBX::Extents worldBox = aabb.toWorldSpace(extentsFrame);
+    Vector3 extent = worldBox.size() * 0.5f;
+    Vector3 center = worldBox.center();
+
+    for (int ii = 0; ii < 6; ++ii)
+    {
+        const Plane& plane = faceArray[ii];
+        
+        float d = plane.normal().dot(center) - plane.distance();
+        float r = Vector3(fabsf(plane.normal().x), fabsf(plane.normal().y), fabsf(plane.normal().z)).dot(extent);
+
+        // box is in negative half-space => outside the frustum
+        if (d + r < 0)
+            return false;
+    }
+    return true;
+}
+
+bool Frustum::containsAABB(const Extents& aabb, const G3D::CoordinateFrame& extentsFrame) const
+{
+	for (int ii = 0; ii < 8; ++ii)
+	{
+		Vector3 corner = aabb.getCorner(ii);
+		Vector3 world = extentsFrame.pointToWorldSpace(corner);
+		if (!containsPoint(world))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool Frustum::containsPoint(const Vector3& point) const
+{
+	for (int ii = 0; ii < faceArray.size(); ++ii)			
+	{
+		const Plane& plane = faceArray[ii];
+		if (!plane.halfSpaceContains(point))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+
+bool Frustum::intersectsSphere(const Vector3& center, float radius) const
+{
+	for (int ii = 0; ii < faceArray.size(); ++ii)			
+	{
+		const Plane& p = faceArray[ii];
+		Plane offsetplane(p.normal(), p.distance() - radius);
+		if (!offsetplane.halfSpaceContains(center))
+		{
+			return false;
+		}
+	}
+	return true;
+}