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