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