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); } } } }
bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult) { SubSimplexClosestResult tempResult; // Start out assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = true; finalResult.m_usedVertices.usedVertexB = true; finalResult.m_usedVertices.usedVertexC = true; finalResult.m_usedVertices.usedVertexD = true; int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) { finalResult.m_degenerate = true; return false; } if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) { return false; } float bestSqDist = FLT_MAX; // If point outside face abc then compute closest point on abc if (pointOutsideABC) { ClosestPtPointTriangle(p, a, b, c,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).dot( q - p); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTB], tempResult.m_barycentricCoords[VERTC], 0 ); } } // Repeat test for face acd if (pointOutsideACD) { ClosestPtPointTriangle(p, a, c, d,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! float sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], 0, tempResult.m_barycentricCoords[VERTB], tempResult.m_barycentricCoords[VERTC] ); } } // Repeat test for face adb if (pointOutsideADB) { ClosestPtPointTriangle(p, a, d, b,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! float sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], 0, tempResult.m_barycentricCoords[VERTB] ); } } // Repeat test for face bdc if (pointOutsideBDC) { ClosestPtPointTriangle(p, b, d, c,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! float sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( 0, tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], tempResult.m_barycentricCoords[VERTB] ); } } //help! we ended up full ! if (finalResult.m_usedVertices.usedVertexA && finalResult.m_usedVertices.usedVertexB && finalResult.m_usedVertices.usedVertexC && finalResult.m_usedVertices.usedVertexD) { return true; } return true; }