bool RaySphere(const Vector3& rayStart, const Vector3& rayDir,
const Vector3& sphereCenter, float sphereRadius,
float& t)
{
++Application::mStatistics.mRaySphereTests;
//project sphere center on to line
Math::Vector3 sphereVec = sphereCenter - rayStart;
float floatingPointEpsilon = 0.0001f;
float tClosestToRay = sphereVec.Dot(rayDir);
if(tClosestToRay >= 0.0f)
{
Math::Vector3 pointClosestToSphere = rayStart + (rayDir * tClosestToRay);
if((sphereCenter - pointClosestToSphere).LengthSq() <= (sphereRadius * sphereRadius))
{
//may be inside sphere, so we must calculate the length of ray that is inside the sphere and subtract
//we can use cosine for this.
// adjacent hypotenuse
float lenInSphere;
if((sphereCenter - pointClosestToSphere).LengthSq() > floatingPointEpsilon)
{
lenInSphere = Math::ArcCos((sphereCenter - pointClosestToSphere).Length() / sphereRadius);
}
else
{
lenInSphere = sphereRadius;
}
//since raydir should be unit length, we can simply subtract this from our closest t
//we max to make sure that we don't subtract past the start of the ray
t = Math::Max(tClosestToRay - lenInSphere, 0.0f);
return true;
}
}
return false;
}
float DistFromPlane(const Vector3& point, const Vector3& normal, float planeDistance)
{
//compute the vector from plane's point to poi
Math::Vector3 vectorToPoint = point - (normal * planeDistance);
//project this vector on to the normal to find the distance from the plane
return vectorToPoint.Dot(normal);
}
// Checks if the ray intersects with a triangle defined by points p1, p2, p3
// Returns true if it intersects and the 't' distace of the intersection point
// from the ray origin
bool Ray::CheckIntersection(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, float &t)
{
Math::Vector3 vecAB = p2 - p1;
Math::Vector3 vecAC = p3 - p1;
Math::Vector3 cross;
cross = mDirection.Cross(vecAC);
float det = vecAB.Dot(cross);
/*
if(cull && det < 0.0001f)
{
return false;
}
else
*/
if(det < 0.0001f && det > -0.0001f)
return false;
Math::Vector3 rayPointVec = mOrigin - p1;
float test1 = rayPointVec.Dot(cross);
if(test1 < 0.0f || test1 > det)
return false;
Math::Vector3 cross2;
cross2 = rayPointVec.Cross(vecAB);
float test2 = mDirection.Dot(cross2);
if(test2 < 0.0f || test1 + test2 > det)
return false;
float inverseDet = 1.0f / det;
t = vecAC.Dot(cross2);
t *= inverseDet;
return true;
}
bool RayTriangle(const Vector3& rayStart, const Vector3& rayDir,
const Vector3& triP0, const Vector3& triP1, const Vector3& triP2,
float& t, float triExpansionEpsilon)
{
++Application::mStatistics.mRayTriangleTests;
Math::Vector3 normal = (triP1 - triP0).Cross(triP2 - triP0).Normalized();
if(RayPlane(rayStart, rayDir, Math::Vector4(normal.x, normal.y, normal.z, normal.Dot(triP0)), t))
{
Math::Vector3 pointOnPlane = (rayDir * t) + rayStart;
Math::Vector3 barycentric;
return BarycentricCoordinates(pointOnPlane, triP0, triP1, triP2, barycentric.x, barycentric.y, barycentric.z, triExpansionEpsilon);
}
return false;
}
bool BarycentricCoordinates(const Vector3& point, const Vector3& a, const Vector3& b,
float& u, float& v, float epsilon)
{
if(a != b)
{
Math::Vector3 ba = a - b;
float baLen = ba.Length();
Math::Vector3 normal = ba / baLen;
u = normal.Dot(point - b) / baLen;
v = 1.0f - u;
if(u == Math::Clamp(u, -epsilon, 1.0f + epsilon))
{
if(v == Math::Clamp(v, -epsilon, 1.0f + epsilon))
{
return true;
}
}
}
return false;
}
bool PolyhedronColliderGeometry::RayCast(const Ray3 &ray, float maxDistance, float &t, Math::Vector3 &n) const
{
Ray3 localRay;
auto &body = mParent->Parent();
localRay.pos = body.GlobalToLocal(ray.pos);
localRay.dir = body.GlobalToLocalVec(ray.dir);
const Math::Vector3 &p = localRay.pos;
const Math::Vector3 &d = maxDistance * localRay.dir;
const Math::Vector3 q = p + d;
float tEnter = 0.0f;
float tExit = 1.0f;
auto &verts = mAdjacency.Verts();
auto &edges = mAdjacency.Edges();
auto &faces = mAdjacency.Faces();
for (auto &face : faces)
{
if (!face.active)
continue;
// triangle edges
auto &edge0 = edges[face.edge];
auto &edge1 = edges[edge0.next];
auto &edge2 = edges[edge1.next];
// triangle verts & normal
const Math::Vector3 &a = verts[edge0.vert].position;
const Math::Vector3 &b = verts[edge1.vert].position;
const Math::Vector3 &c = verts[edge2.vert].position;
const Math::Vector3 normal = (b - a).Cross(c - a);
float denom = normal.Dot(d);
float dist = normal.Dot(p - a); // positive: outside plane
// test if segment runs parallel to the plane
if (std::fabs(denom) < EPSILON && dist > 0.0f)
return false;
const float tempT = -dist / denom;
if (denom < -EPSILON)
{
if (tempT > tEnter)
{
n = normal;
tEnter = tempT;
}
}
else if (denom > EPSILON)
{
tExit = (tExit < tempT) ? tExit : tempT;
}
// early out
if (tEnter > tExit) return false;
}
n = body.LocalToGlobalVec(n);
n.Normalize();
t = tEnter;
return true;
}