#include "../Fusion/RuFusion_Sojourner.h"
#include "../Scene/Widget/RuEntity_Widget_Line.h"

#pragma unmanaged

// ************************************************************************************************************************************************************

ruRTTI_DEFINE_SUBCLASS(CRuFusion_Sojourner, IRuObject)
ruHEAP_DEFINE_SUBCLASS(CRuFusion_Sojourner, "CRuFusion_Sojourner", "IRuObject")
ruCLASSGUID_DEFINE(CRuFusion_Sojourner, 0x73F25A4E, 0x6FC94fb5, 0xAAFA0019, 0x9553B2C7)

// ************************************************************************************************************************************************************

CRuFusion_Sojourner::CRuFusion_Sojourner(IRuObject *collisionObject)
{
	m_collisionObject = collisionObject;
	m_lastQueryBounds.Set(CRuVector3(0.0f, 0.0f, 0.0f), 0.0f);
	m_sphere.Set(CRuVector3(0.0f, 0.0f, 0.0f), 1.0f);
	m_frictionConstant = 0.0f;
/*
	m_visualizerMesh = ruNEW CRuDynamicMesh();
	m_visualizerMesh->Initialize(ruPRIMTYPE_TRIANGLELIST, 3000, 3000, ruVERTEXFORMAT_POSITION | ruVERTEXFORMAT_DIFFUSE, 1);

	m_visualizerMaterial = ruNEW CRuMaterial();
	m_visualizerMaterial->SetShader(g_ruResourceManager->GetShader("ruSHADER_EDITOR_TRANSLUCENT"));

	m_visualizerModel = ruNEW CRuModel();
	m_visualizerModel->AddSubModel(m_visualizerMesh, m_visualizerMaterial);
*/
}

void CRuFusion_Sojourner::SetFrictionParameters(const CRuVector3 &frictionSurfaceNormal, REAL frictionSurfaceThreshold, REAL frictionConstant)
{
	m_frictionSurfaceNormal = frictionSurfaceNormal;
	m_frictionSurfaceThreshold = frictionSurfaceThreshold;
	m_frictionConstant = frictionConstant;
}

void CRuFusion_Sojourner::SetColisionObject(IRuObject *collisionObject)
{
	m_collisionObject = collisionObject;
}

CRuModel *CRuFusion_Sojourner::GetVisualizer()
{
//	return m_visualizerModel;
	return NULL;
}

BOOL CRuFusion_Sojourner::Slide(const CRuVector3 &origin, const CRuVector3 &velocity, const CRuVector3 &radius, CRuVector3 &adjustedPositionOut)
{
	// If velocity has magnitude zero, do nothing
	REAL speed = velocity.Magnitude();
	if(speed <= ruEPSILON)
	{
		adjustedPositionOut = origin;
		return TRUE;
	}

	REAL evaluationIncrement = 10.0f;
	REAL distanceRemaining = velocity.Magnitude();

	REAL threshold = 0.001f;

	while(distanceRemaining > threshold)
	{
		REAL speed = min(distanceRemaining, evaluationIncrement);
		distanceRemaining = distanceRemaining - speed;

		CRuVector3 travelDirection = velocity;
		travelDirection.Normalize();
		CRuVector3 evaluationVelocity = travelDirection * speed;

		// Calculate the bounding area -- use the ellipsoid plus velocity radius and a little bit of extra padding
//		REAL boundingRadius = max(max(radius.x, radius.y), radius.z) + max(max(fabs(velocity.x), fabs(velocity.y)), fabs(velocity.z)) * 1.1f;
		REAL boundingRadius = max(max(radius.x, radius.y), radius.z) + speed;
		CRuSphere boundingSphere(origin, boundingRadius);

		if(m_lastQueryBounds.Contains(boundingSphere) == FALSE)
		{
			// Enlarge the bounding sphere slightly
			boundingSphere.Radius() += 10.0f;
			m_lastQueryBounds = boundingSphere;

			// Clear prior data
			m_triangleVerts0.Clear();
			m_triangleNormals0.Clear();
			m_triangleFlags0.Clear();

			// Build triangle enumeration query based on the bounding area
			CRuCollisionQuery_Sphere_TriEnum sphereQuery;
			delete sphereQuery.m_colTriVerts;
			delete sphereQuery.m_colTriNormals;
			delete sphereQuery.m_colTriFlags;
//			sphereQuery.m_exclusionMask = 0x00A00000;		// Exclude water and foliage surfaces
			sphereQuery.m_exclusionMask = 0x00800000;		// Exclude foliage surfaces
			sphereQuery.m_colSphere = boundingSphere;
			sphereQuery.m_colTriVerts = &m_triangleVerts0;
			sphereQuery.m_colTriNormals = &m_triangleNormals0;
			sphereQuery.m_colTriFlags = &m_triangleFlags0;

			// Perform enumeration query
			if(m_collisionObject->GetType().IsSubClassOf(CRuEntity::Type))
			{
				static_cast<CRuEntity *>(m_collisionObject)->QueryCollision(&sphereQuery);
			}
			else if(m_collisionObject->GetType().IsSubClassOf(IRuCollisionHierarchy::Type))
			{
				static_cast<IRuCollisionHierarchy *>(m_collisionObject)->QueryCollision(&sphereQuery);
			}

			// Cleanup
			sphereQuery.m_colTriVerts = NULL;
			sphereQuery.m_colTriNormals = NULL;
			sphereQuery.m_colTriFlags = NULL;
/*
			// Visualizer thingy
			CRuVector3 *position = static_cast<CRuMorphTarget *>(m_visualizerMesh->GetMorphTarget(0))->GetPosition();
			DWORD *diffuse = static_cast<CRuMorphTarget *>(m_visualizerMesh->GetMorphTarget(0))->GetDiffuse();
			UINT16 *indices = m_visualizerMesh->GetIndices();
			for(INT32 i = 0; i < m_triangleVerts0.Count(); ++i)
			{
				position[i] = m_triangleVerts0[i];
				diffuse[i] = 0xFFFFFFFF;
				indices[i] = i;
			}

			m_visualizerMesh->SetNumVertices(m_triangleVerts0.Count());
			m_visualizerMesh->SetNumPrimitives(m_triangleVerts0.Count() / 3);

			m_visualizerMesh->CalculateBounds();

			m_visualizerModel->MarkLocalEntityBoundsDirty();
*/
/*
			m_visualizerModel->RemoveAllChildren();

			for(INT32 i = 0, j = 0; i < m_triangleNormals0.Count(); ++i)
			{
				CRuVector3 &v0 = m_triangleVerts0[j++];
				CRuVector3 &v1 = m_triangleVerts0[j++];
				CRuVector3 &v2 = m_triangleVerts0[j++];

				CRuVector3 centroid = (v0 + v1 + v2) / 3.0f;

				CRuEntity_Widget_Line *lineWidget = ruNEW CRuEntity_Widget_Line();

				lineWidget->SetOption(ruENTITYOPTION_BILLBOARD, ruBILLBOARDTYPE_AXIAL);
				lineWidget->SetLine(centroid, centroid + m_triangleNormals0[i] * 5.0f, FALSE, TRUE, 0xFFFF0000);
				m_visualizerModel->AddChild(lineWidget);
				ruSAFE_RELEASE(lineWidget);
			}
*/
		}
/*
		CRuVector3 *position = static_cast<CRuMorphTarget *>(m_visualizerMesh->GetMorphTarget(0))->GetPosition();
		DWORD *diffuse = static_cast<CRuMorphTarget *>(m_visualizerMesh->GetMorphTarget(0))->GetDiffuse();
		UINT16 *indices = m_visualizerMesh->GetIndices();
		for(INT32 i = 0; i < m_triangleVerts0.Count(); ++i)
		{
			diffuse[i] = 0xFFFFFFFF;
		}
*/

		// Construct transform to deform ellipsoid into unit sphere
		m_ellipsoidOrigin = origin;
		m_ellipsoidDeformation.SetToScaling(1.0f / radius.x, 1.0f / radius.y, 1.0f / radius.z);

		// Modify all colliding triangles as follows: Set origin at (0, 0, 0), and warp space such that the ellipsoid becomes a sphere
		m_triangleVerts.SetArraySize(m_triangleVerts0.Count());
		m_triangleVerts.SetNumEntries(m_triangleVerts0.Count());
		for(INT32 i = 0; i < m_triangleVerts0.Count(); ++i)
		{
			m_triangleVerts[i] = m_triangleVerts0[i] - m_ellipsoidOrigin;
			m_ellipsoidDeformation.TransformPoint(m_triangleVerts[i]);
		}

		// Modify all colliding normals and calculate triangle planes
		m_triangleNormals.SetArraySize(m_triangleNormals0.Count());
		m_triangleNormals.SetNumEntries(m_triangleNormals0.Count());
		m_trianglePlanes.SetArraySize(m_triangleNormals0.Count());
		m_trianglePlanes.SetNumEntries(m_triangleNormals0.Count());
		for(INT32 i = 0; i < m_triangleNormals0.Count(); ++i)
		{
			// Calculate normal in ellipsoid space
			CRuVector3 &v0 = m_triangleVerts[i * 3];
			CRuVector3 &v1 = m_triangleVerts[i * 3 + 1];
			CRuVector3 &v2 = m_triangleVerts[i * 3 + 2];

			CRuVector3 e0 = v1 - v0;
			CRuVector3 e1 = v2 - v1;

			m_triangleNormals[i] = CrossProduct(e0, e1).Normalize();

			// Insert triangle plane into list
			m_trianglePlanes[i].SetPlane(m_triangleNormals[i], v0);
		}

		// Build collision ray centered at (0, 0, 0) and scale it
		CRuRay3 collisionRay(CRuVector3(0.0f, 0.0f, 0.0f), evaluationVelocity, FALSE);
		m_ellipsoidDeformation.TransformVector(collisionRay.Direction());
		collisionRay.Direction().Normalize();

		CRuVector3 scaledVelocity = evaluationVelocity;
		m_ellipsoidDeformation.TransformVector(scaledVelocity);

		// Hand-off to collision processor
		adjustedPositionOut = CollideAndSlide(collisionRay, scaledVelocity.Magnitude());

		// Post-process adjusted position
		CRuMatrix4x4 invEllipsoidDeformation;
		invEllipsoidDeformation.SetToScaling(radius.x, radius.y, radius.z);
		invEllipsoidDeformation.TransformPoint(adjustedPositionOut);
		adjustedPositionOut = adjustedPositionOut + m_ellipsoidOrigin;
	}

	return TRUE;
}

BOOL CRuFusion_Sojourner::IntersectsWaterSurface(const CRuSegment3 &segment)
{
	// Convert segment into ellipsoid coordinate frame
	CRuSegment3 localSegment = segment;
	localSegment.Origin() -= m_ellipsoidOrigin;;
	m_ellipsoidDeformation.TransformPoint(localSegment.Origin());
	m_ellipsoidDeformation.TransformVector(localSegment.Direction());

	// Iterate through all potential colliding triangles and find first intersection
	for(INT32 i = 0, j = 0; i < m_triangleVerts.Count(); i += 3, ++j)
	{
		CRuVector3 intPt;
		REAL intT;

		// Return immediately on intersection
		if(FindIntersection(localSegment, m_triangleVerts[i], m_triangleVerts[i + 1], m_triangleVerts[i + 2], intPt, intT))
		{
			return TRUE;
		}
	}

	return FALSE;
}

CRuVector3 CRuFusion_Sojourner::CollideAndStop(const CRuRay3 &collisionRay, const REAL speed)
{
	CRuVector3 result;

	// Process colliders
	ProcessColliders(collisionRay, speed);

	if(m_colliders.Count() > 0)
	{
		char outputStr[2048];
		sprintf(outputStr, "[%d] %f, %d triangles examined. [%d:%d:%d:%d] %d.\n", m_colliders[0].m_collisionType, m_colliders[0].m_collisionDistance, m_triangleNormals.Count(), m_path_Embedded, m_path_Surface, m_path_Point, m_path_Edge, m_passed);
//		g_pChatFrame->AddMessage(eMessage_Channel_System, outputStr);
	}
	else
	{
		char outputStr[2048];
		sprintf(outputStr, "NO COL, %d triangles examined. [%d:%d:%d:%d] %d.\n", m_triangleNormals.Count(), m_path_Embedded, m_path_Surface, m_path_Point, m_path_Edge, m_passed);
//		g_pChatFrame->AddMessage(eMessage_Channel_System, outputStr);
	}

	// Set result to the entire traversal length
	result = collisionRay.Origin() + collisionRay.Direction() * speed;

	// If there was a valid collision, modify the result to fit
//	if(m_colliders.Count() > 0 && m_colliders[0].m_collisionDistance < speed)
	if(m_colliders.Count() > 0)
	{

		if(m_colliders[0].m_collisionType == ruCOLTYPE_EMBEDDED)
		{
//			result = collisionRay.Origin() - m_colliders[0].m_trianglePlane.GetNormal() * (speed * m_colliders[0].m_collisionDistance);
			result = collisionRay.Origin() - m_colliders[0].m_trianglePlane.GetNormal() * m_colliders[0].m_collisionDistance;
		}
		else
		{
			result = collisionRay.Origin() + collisionRay.Direction() * (speed * m_colliders[0].m_collisionDistance);
		}
/*
		char outputStr[2048];
		sprintf(outputStr, "Distance = %f, Type = %d.\n", m_colliders[0].m_collisionDistance, m_colliders[0].m_collisionType);
		OutputDebugStr(outputStr);
*/
//		if(fabs(m_colliders[0].m_collisionDistance) > 0.001f)
//			result = collisionRay.Origin() + collisionRay.Direction() * m_colliders[0].m_collisionDistance;
//		else
//			result = collisionRay.Origin();
//			result = collisionRay.Origin() + collisionRay.Direction() * (speed * m_colliders[0].m_collisionDistance);
	}

	return result;
}

CRuVector3 CRuFusion_Sojourner::CollideAndSlide(const CRuRay3 &collisionRay, const REAL speed)
{
	INT32 maxLoops = 30;

	CRuSegment3 inputSegment(collisionRay.Origin(), collisionRay.Origin() + collisionRay.Direction() * speed);
	CRuSegment3 outputSegment(inputSegment);

	if(m_triangleVerts.Count() == 0)
	{
		outputSegment.Origin() = outputSegment.Origin() + outputSegment.Direction();
		return outputSegment.Origin();
	}

	// Clear skip list
	m_skipList.Clear();

	while(maxLoops-- && outputSegment.Direction().Magnitude() > ruEPSILON)
	{
		// Prepare output ray based on output segment
		CRuRay3 outputRay(outputSegment.Origin(), outputSegment.Direction(), TRUE);

		// Process colliders
		REAL outputSpeed = outputSegment.Direction().Magnitude();
		ProcessColliders(outputRay, outputSpeed);

		// Clear skip list
		m_skipList.Clear();

		// If we didn't hit anything just return the destination
		if(m_colliders.Count() == 0)
		{
			outputSegment.Origin() = outputSegment.Origin() + outputSegment.Direction();
			break;
		}

		// If it was embedded, push away
		if(m_colliders[0].m_collisionType == ruCOLTYPE_EMBEDDED)
		{
			// If it's embedded, back up along the collision plane normal
			outputSegment.Origin() = outputSegment.Origin() - m_colliders[0].m_trianglePlane.GetNormal() * m_colliders[0].m_collisionDistance;
		}
		else
		{
			// We hit something, move as far as we can
			REAL colDist = m_colliders[0].m_collisionDistance * (1.0f - ruEPSILON);
			REAL frictionSurfaceAngle = m_colliders[0].m_trianglePlane.DotNormal(m_frictionSurfaceNormal);

			outputSegment.Origin() = outputSegment.Origin() + outputRay.Direction() * colDist;

			if(frictionSurfaceAngle >= m_frictionSurfaceThreshold)
			{
				outputSegment.Direction() = outputRay.Direction() * (outputSegment.Direction().Magnitude() - colDist) * (1.0f - m_frictionConstant);
			}
			else
			{
				outputSegment.Direction() = outputRay.Direction() * (outputSegment.Direction().Magnitude() - colDist);
			}

/*
//			REAL threshold = 0.1f;
//			if(m_colliders[0].m_collisionDistance * speed > threshold)
			{
				REAL colDist = m_colliders[0].m_collisionDistance * (1.0f - ruEPSILON);

				outputSegment.Origin() = outputSegment.Origin() + outputRay.Direction() * colDist;
				outputSegment.Direction() = outputRay.Direction() * (outputSegment.Direction().Magnitude() - colDist);
			}
*/
/*
			// If we hit two or more, find the two with the most "pressure"
			if(m_colliders.Count() >= 2)
			{
				INT32 c0;
				REAL d0 = 2;

				for(INT32 i = 0; i < m_colliders.Count(); ++i)
				{
					REAL dot = DotProduct(m_colliders[i].m_trianglePlane.GetNormal(), outputRay.Direction());
					if(dot < d0)
					{
						d0 = dot;
						c0 = i;
					}
				}

				INT32 c1;
				REAL d1 = 2;

				for(INT32 j = 0; j < m_colliders.Count(); ++j)
				{
					if(j == c0)
						continue;

					REAL dot = DotProduct(m_colliders[j].m_trianglePlane.GetNormal(), outputRay.Direction());
					if(dot < d1)
					{
						d1 = dot;
						c1 = j;
					}
				}

				// Temp vector
				CRuVector3 tempV = outputSegment.Direction();

				// Point along the crease
				CRuVector3 perp = CrossProduct(m_colliders[c1].m_trianglePlane.GetNormal(), m_colliders[c0].m_trianglePlane.GetNormal());
				perp.x = fabs(perp.x);
				perp.y = fabs(perp.y);
				perp.z = fabs(perp.z);
				tempV.x = tempV.x * perp.x;
				tempV.y = tempV.y * perp.y;
				tempV.z = tempV.z * perp.z;

				if(tempV.Magnitude() < ruEPSILON)
					continue;

				tempV = tempV.Normalize() * speed;

				// Find out how much to scale our vector that points along the crease
				CRuVector3 n1 = tempV;
				n1.Normalize();
				
				CRuVector3 n2 = outputRay.Direction();
				n2.Normalize();

				REAL scalar = DotProduct(n1, n2);

				// New vector is new direction vector, scaled
				outputSegment.Direction() = tempV * scalar;
//				outputSegment.Direction().Set(0, 0, 0);


//				if (filterPulseJumps)
//				{
//					if ((outputRay.vector() ^ inputRay.vector()) < 0) outputRay.vector() = vector3::zero();
//				}

//				lastDir = outputRay.vector();

				// Don't collide with these next time
				m_skipList.Add(m_colliders[c0].m_triangleIndex);
				m_skipList.Add(m_colliders[c1].m_triangleIndex);

				// Iterate
				continue;
			}
*/
			// Otherwise we slide
			CRuVector3 slideSrc = m_colliders[0].m_trianglePlane.GetClosestPointOnPlane(outputSegment.Origin());
			CRuVector3 slideDst = m_colliders[0].m_trianglePlane.GetClosestPointOnPlane(outputSegment.Origin() + outputSegment.Direction());

			outputSegment.Direction() = slideDst - slideSrc;
//			outputSegment.Direction().Set(0, 0, 0);
/*
			if (filterPulseJumps)
			{
				if ((outputRay.vector() ^ inputRay.vector()) < 0) outputRay.vector() = vector3::zero();
			}
*/
//			lastDir = outputRay.vector();

			// Don't collide with this one next time

			m_skipList.Add(m_colliders[0].m_triangleIndex);

//			CRuVector3 norm = m_colliders[0].m_trianglePlane.GetNormal();

			CRuVector3 norm = outputSegment.Direction();
/*
			char outputStr[2048];
			sprintf(outputStr, "[%d:%f:%d] Crease (%f, %f, %f) from (%f, %f, %f) -> (%f, %f, %f) -> (%f, %f, %f)\n", m_colliders[0].m_collisionType, m_colliders[0].m_collisionDistance, m_colliders[0].m_triangleIndex, norm.x, norm.y, norm.z, outputRay.Origin().x, outputRay.Origin().y, outputRay.Origin().z, slideSrc.x, slideSrc.y, slideSrc.z, slideDst.x, slideDst.y, slideDst.z);
//			sprintf(outputStr, "[%d:%f:%d] Velocity %f from (%f, %f, %f)\n", m_colliders[0].m_collisionType, m_colliders[0].m_collisionDistance, m_colliders[0].m_triangleIndex, norm.Magnitude(), outputRay.Origin().x, outputRay.Origin().y, outputRay.Origin().z);
			OutputDebugStr(outputStr);
//			g_pChatFrame->AddMessage(eMessage_Channel_System, outputStr);
*/
		}
	}

	return outputSegment.Origin();
}

BOOL CRuFusion_Sojourner::IsEmbedded(INT32 triangleIndex, const CRuVector3 &sphereCenter, CRuVector3 &closestPointOut)
{
	// Shortcut references
	const CRuVector3 &v0 = m_triangleVerts[triangleIndex * 3];
	const CRuVector3 &v1 = m_triangleVerts[triangleIndex * 3 + 1];
	const CRuVector3 &v2 = m_triangleVerts[triangleIndex * 3 + 2];
	const CRuPlane4 &plane = m_trianglePlanes[triangleIndex];

	// Find the closest point on the triangle to the sphere center
	closestPointOut = ClosestPointOnTriangle(v0, v1, v2, sphereCenter);

	// Find the vector v from the sphere center to the closest point on the triangle
	CRuVector3 v = closestPointOut - sphereCenter;

	// Sphere is embedded if the v has length less than the radius of the unit sphere
	return DotProduct(v, v) < (1.0f - ruEPSILON);
}

void CRuFusion_Sojourner::ProcessColliders(const CRuRay3 &collisionRay, const REAL speed)
{
	// Clear all colliders
	m_colliders.Clear();

//	DWORD *diffuse = static_cast<CRuMorphTarget *>(m_visualizerMesh->GetMorphTarget(0))->GetDiffuse();

	// Iterate through all colliding triangles and process each collision
	for(INT32 i = 0, j = 0; i < m_triangleVerts.Count(); i += 3, ++j)
	{
		REAL collisionDistance;
		CRuPlane4 collisionPlane;
		RuCollisionType	collisionType = ruCOLTYPE_NONE;

		const CRuVector3 &v0 = m_triangleVerts[i];
		const CRuVector3 &v1 = m_triangleVerts[i + 1];
		const CRuVector3 &v2 = m_triangleVerts[i + 2];
		const CRuVector3 &n = m_triangleNormals[j];
		const CRuPlane4 &plane = m_trianglePlanes[j];

		// Ignore water surfaces
		UINT32 &flags = m_triangleFlags0[j];
		if(flags & 0x00200000)
			continue;

		// Ignore polygons in the skip list
		if(m_skipList.Contains(j))
			continue;

		// Ignore back-facing triangles
		if(plane.GetDistance(collisionRay.Origin()) <= 0.0f)
			continue;

		// At this point, we know that the sphere is in front of the plane
		// Find sphere's collision point with the triangle plane
		CRuSphere collisionSphere(collisionRay.Origin(), 1.0f);
		CRuVector3 planeIntersectionPoint;
		if(!FindIntersection(collisionSphere, collisionRay.Direction(), plane, collisionDistance, planeIntersectionPoint))
			continue;

		CRuVector3 closestPoint;
		if(IsEmbedded(j, collisionRay.Origin(), closestPoint))
		{
			// Set collision distance and plane to be the shortest path for unembedding the collision sphere
			collisionDistance = (closestPoint - collisionRay.Origin()).Magnitude() - 1.0f;
			collisionPlane.SetPlane((collisionRay.Origin() - closestPoint).Normalize(), closestPoint);
			collisionType = ruCOLTYPE_EMBEDDED;

//			diffuse[i + 0] = 0xFFFF00FF;
//			diffuse[i + 1] = 0xFFFF00FF;
//			diffuse[i + 2] = 0xFFFF00FF;
		}
		else if(InsideTriangle(v0, v1, v2, n, planeIntersectionPoint))
		{
			// Our collision plane is our polygon's plane
			collisionPlane = plane;
			collisionType = ruCOLTYPE_SURFACE;

			// Is this an actual collision?
			if(collisionDistance > speed)
				continue;

//			diffuse[i + 0] = 0xFF0000FF;
//			diffuse[i + 1] = 0xFF0000FF;
//			diffuse[i + 2] = 0xFF0000FF;
		}
		else
		{
			BOOL found = FALSE;

			collisionPlane = plane;

			REAL lowerLimit = -10000.0f;
			REAL upperLimit = 50000.0f;

			collisionDistance = FLT_MAX;

			// Test against triangle edge (0, 1)
			CRuCylinder cyl_0_1(CRuSegment3(v0, v1), 1.0f);
			REAL t_0_1;
			if(FindIntersection(collisionRay, cyl_0_1, t_0_1) && (t_0_1 >= lowerLimit && t_0_1 <= upperLimit))
			{
				found = TRUE;
				collisionDistance = t_0_1;
				collisionType = ruCOLTYPE_EDGE;
			}

			// Test against triangle edge (1, 2)
			CRuCylinder cyl_1_2(CRuSegment3(v1, v2), 1.0f);
			REAL t_1_2;
			if(FindIntersection(collisionRay, cyl_1_2, t_1_2) && (t_1_2 >= lowerLimit && t_1_2 <= upperLimit) && (t_1_2 < collisionDistance || found == FALSE))
			{
				found = TRUE;
				collisionDistance = t_1_2;
				collisionType = ruCOLTYPE_EDGE;
			}

			// Test against triangle edge (2, 0)
			CRuCylinder cyl_2_0(CRuSegment3(v2, v0), 1.0f);
			REAL t_2_0;
			if(FindIntersection(collisionRay, cyl_2_0, t_2_0) && (t_2_0 >= lowerLimit && t_2_0 <= upperLimit) && (t_2_0 < collisionDistance || found == FALSE))
			{
				found = TRUE;
				collisionDistance = t_2_0;
				collisionType = ruCOLTYPE_EDGE;
			}

			// Test against triangle vertex 0
			CRuSphere sph_0(v0, 1.0f);
			REAL t_v_0;
			if(FindIntersection(collisionRay, sph_0, t_v_0) && (t_v_0 >= lowerLimit && t_v_0 <= upperLimit) && (t_v_0 < collisionDistance || found == FALSE))
			{
				found = TRUE;
				collisionDistance = t_v_0;
				collisionType = ruCOLTYPE_POINT;
			}

			// Test against triangle vertex 1
			CRuSphere sph_1(v1, 1.0f);
			REAL t_v_1;
			if(FindIntersection(collisionRay, sph_1, t_v_1) && (t_v_1 >= lowerLimit && t_v_1 <= upperLimit) && (t_v_1 < collisionDistance || found == FALSE))
			{
				found = TRUE;
				collisionDistance = t_v_1;
				collisionType = ruCOLTYPE_POINT;
			}

			// Test against triangle vertex 2
			CRuSphere sph_2(v2, 1.0f);
			REAL t_v_2;
			if(FindIntersection(collisionRay, sph_2, t_v_2) && (t_v_2 >= lowerLimit && t_v_2 <= upperLimit) && (t_v_2 < collisionDistance || found == FALSE))
			{
				found = TRUE;
				collisionDistance = t_v_2;
				collisionType = ruCOLTYPE_POINT;
			}

			if(found == FALSE)
			{
				continue;
			}

			collisionPlane = plane;

//			diffuse[i + 0] = 0xFFFF0000;
//			diffuse[i + 1] = 0xFFFF0000;
//			diffuse[i + 2] = 0xFFFF0000;
		}

		// Edge and point collisions return t in different unit systems.. so convert it
//		if(collisionType == ruCOLTYPE_POINT || collisionType == ruCOLTYPE_EDGE)
//			collisionDistance *= speed;

		// Was it an actual collision?
		if(collisionType != ruCOLTYPE_EMBEDDED && collisionDistance > speed)
			continue;

		// Ignore collision planes that we're traveling away from
		if(collisionType != ruCOLTYPE_EMBEDDED && DotProduct(collisionPlane.GetNormal(), collisionRay.Direction()) >= 0.0f)
			continue;

		// Is it closer, farther away, or the same distance as what we've found so far?
		if(m_colliders.Count())
		{
			REAL d = fabs(collisionDistance - m_colliders[0].m_collisionDistance);
			if(d > ruEPSILON)
			{
				if(collisionDistance < m_colliders[0].m_collisionDistance)
					m_colliders.Clear();
				else if (collisionDistance > m_colliders[0].m_collisionDistance)
					continue;
			}
			// Make sure it's not already in the list (this plane's normal)
			else
			{
				BOOL skipMe = FALSE;
				for(INT32 k = 0; k < m_colliders.Count(); ++k)
				{
					if(DotProduct(m_colliders[k].m_trianglePlane.GetNormal(), collisionPlane.GetNormal()) > 1.0f - ruEPSILON)
					{
						skipMe = TRUE;
						break;
					}
				}

				if(skipMe)
					continue;
			}
		}

//		diffuse[i + 0] = 0xFF00FF00;
//		diffuse[i + 1] = 0xFF00FF00;
//		diffuse[i + 2] = 0xFF00FF00;

		// Add this collider to the list
		RuCollider nearest;
		nearest.m_triangleIndex = j;
		nearest.m_trianglePlane = collisionPlane;
		nearest.m_collisionDistance = collisionDistance;
		nearest.m_collisionType = collisionType;
		m_colliders.Add(nearest);
	}
}

// ************************************************************************************************************************************************************

#pragma managed
