inline B32 TestSphereTriangleIntersection(V3 center, F32 radius, const V3& a, const V3& b, const V3& c, V3 *peneterationVector) {
V3 p = ClosestPointOnTriangle(center, a, b, c);
V3 displacement = p - center;
F32 distance = Magnitude(displacement);
if (distance <= radius) {
V3 direction = Normalize(displacement);
F32 diff = radius - distance;
*peneterationVector = direction * diff;
return true;
}
return false;
}
void Physics_Cloth::PyramidCollision(v3float _pyraPointA, v3float _pyraPointB, v3float _pyraPointC, v3float _pyraPointD)
{
// Cycle through all particles
for (int i = 0; i < m_particleCount; i++)
{
v3float particlePos = *m_pParticles[i].GetPosition();
// Starts the closest point as the position of the particle and the closest distance to infinity
v3float closestPt = particlePos;
float closestDist = FLT_MAX;
// Declare extra variables
v3float closestTriPoint;
float dist;
// Calculate the closest point on the first triangle and compare
closestTriPoint = ClosestPointOnTriangle(particlePos, _pyraPointA, _pyraPointB, _pyraPointC);
dist = pow((particlePos - closestTriPoint).Magnitude(), 2);
if (dist < closestDist)
{
// Save the new closest point and distance
closestDist = dist;
closestPt = closestTriPoint;
}
// Calculate the closest point on the second triangle and compare
closestTriPoint = ClosestPointOnTriangle(particlePos, _pyraPointA, _pyraPointC, _pyraPointD);
dist = pow((particlePos - closestTriPoint).Magnitude(), 2);
if (dist < closestDist)
{
// Save the new closest point and distance
closestDist = dist;
closestPt = closestTriPoint;
}
// Calculate the closest point on the third triangle and compare
closestTriPoint = ClosestPointOnTriangle(particlePos, _pyraPointA, _pyraPointD, _pyraPointB);
dist = pow((particlePos - closestTriPoint).Magnitude(), 2);
if (dist < closestDist)
{
// Save the new closest point and distance
closestDist = dist;
closestPt = closestTriPoint;
}
// Calculate the closest point on the fourth triangle and compare
closestTriPoint = ClosestPointOnTriangle(particlePos, _pyraPointB, _pyraPointD, _pyraPointC);
dist = pow((particlePos - closestTriPoint).Magnitude(), 2);
if (dist < closestDist)
{
// Save the new closest point and distance
closestDist = dist;
closestPt = closestTriPoint;
}
// If the point is outside all planes then the point is within the bounds of the pyramid
if ( (PointOutsideOfPlane(particlePos, _pyraPointA, _pyraPointB, _pyraPointC) == true)
&& (PointOutsideOfPlane(particlePos, _pyraPointA, _pyraPointC, _pyraPointD) == true)
&& (PointOutsideOfPlane(particlePos, _pyraPointA, _pyraPointD, _pyraPointB) == true)
&& (PointOutsideOfPlane(particlePos, _pyraPointB, _pyraPointD, _pyraPointC) == true))
{
// Move the particle outside the pyramid using the closest point
closestPt = closestPt + ((closestPt - particlePos).Normalise() * 0.5f);
m_pParticles[i].SetPosition(closestPt, true);
}
else
{
// Particle is not inside the pyramid
v3float diff = particlePos - closestPt;
dist = diff.Magnitude();
// check if the particle is too close to the pyramid
if (dist < 0.5f)
{
// Move the particle a small distance from the pyramid
closestPt = closestPt + ((particlePos - closestPt).Normalise() * 0.5f);
m_pParticles[i].SetPosition(closestPt, true);
}
}
}
}