void CollisionSolver::UpdateDiagnostics(const Simplex& simplex,
const D3DXVECTOR3& furthestPoint)
{
if(m_engine->diagnostic()->AllowDiagnostics(Diagnostic::COLLISION))
{
const float radius = 0.1f;
const float normalLength = 1.5f;
D3DXVECTOR3 origin(0.0, 0.0, 0.0);
m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
"OriginPoint", Diagnostic::WHITE, origin, radius);
m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
"FurthestPoint", Diagnostic::MAGENTA, furthestPoint, radius);
const auto& borders = simplex.GetBorderEdges();
for(unsigned int i = 0; i < borders.size(); ++i)
{
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"BorderEdge" + StringCast(i), Diagnostic::RED,
simplex.GetPoint(borders[i].indices[0]),
simplex.GetPoint(borders[i].indices[1]));
}
const auto& faces = simplex.GetFaces();
for(unsigned int i = 0; i < faces.size(); ++i)
{
if(faces[i].alive)
{
std::string id = StringCast(i);
const Face& face = faces[i];
const D3DXVECTOR3 center = simplex.GetFaceCenter(i);
const D3DXVECTOR3& normal = face.normal * normalLength;
const D3DXVECTOR3& pointA = simplex.GetPoint(face.indices[0]);
const D3DXVECTOR3& pointB = simplex.GetPoint(face.indices[1]);
const D3DXVECTOR3& pointC = simplex.GetPoint(face.indices[2]);
m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
"sCenter" + id, Diagnostic::BLUE, center, radius);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sNormal" + id, Diagnostic::BLUE, center, center + normal);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sLine1" + id, Diagnostic::YELLOW, pointA, pointB);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sLine2" + id, Diagnostic::YELLOW, pointA, pointC);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sLine3" + id, Diagnostic::YELLOW, pointC, pointB);
}
}
}
}
bool CollisionSolver::SolveTetrahedronSimplex(Simplex& simplex, D3DXVECTOR3& direction)
{
const D3DXVECTOR3& pointA = simplex.GetPoint(3);
const D3DXVECTOR3& pointB = simplex.GetPoint(0);
const D3DXVECTOR3& pointC = simplex.GetPoint(1);
const D3DXVECTOR3& pointD = simplex.GetPoint(2);
const D3DXVECTOR3 AB = pointB - pointA;
const D3DXVECTOR3 AC = pointC - pointA;
const D3DXVECTOR3 AD = pointD - pointA;
const D3DXVECTOR3 AO = -pointA;
// Check if within the three surrounding planes
// The forth plane has been previously tested with the plane simplex
// All normals will point to the center of the tetrahedron
D3DXVECTOR3 CBnormal, BDnormal, DCnormal;
D3DXVec3Cross(&CBnormal, &AC, &AB);
D3DXVec3Cross(&BDnormal, &AB, &AD);
D3DXVec3Cross(&DCnormal, &AD, &AC);
const float CBdistance = D3DXVec3Dot(&CBnormal, &AO);
const float BDdistance = D3DXVec3Dot(&BDnormal, &AO);
const float DCdistance = D3DXVec3Dot(&DCnormal, &AO);
bool originInsideSimplex = true;
if(CBdistance < 0.0f)
{
// Origin is outside of the CB plane
// D is furthest point, remove it and search towards the origin
simplex.RemovePoint(pointD);
direction = -CBnormal;
originInsideSimplex = false;
}
else if(BDdistance < 0.0f)
{
// Origin is outside of the BD plane
// C is furthest point, remove it and search towards the origin
simplex.RemovePoint(pointC);
direction = -BDnormal;
originInsideSimplex = false;
}
else if(DCdistance < 0.0f)
{
// Origin is outside of the DC plane
// C is furthest point, remove it and search towards the origin
simplex.RemovePoint(pointB);
direction = -DCnormal;
originInsideSimplex = false;
}
return originInsideSimplex;
}
void CollisionSolver::SolvePlaneSimplex(Simplex& simplex, D3DXVECTOR3& direction)
{
const D3DXVECTOR3& pointA = simplex.GetPoint(2);
const D3DXVECTOR3& pointB = simplex.GetPoint(0);
const D3DXVECTOR3& pointC = simplex.GetPoint(1);
const D3DXVECTOR3 AB = pointB - pointA;
const D3DXVECTOR3 AC = pointC - pointA;
const D3DXVECTOR3 AO = -pointA;
// Determine which side of the plane the origin is on
D3DXVECTOR3 planeNormal;
D3DXVec3Cross(&planeNormal, &AB, &AC);
// Determine the new search direction towards the origin
const float distanceToPlane = D3DXVec3Dot(&planeNormal, &AO);
direction = (distanceToPlane < 0.0f) ? -planeNormal : planeNormal;
}
void CollisionSolver::SolveLineSimplex(const Simplex& simplex, D3DXVECTOR3& direction)
{
const D3DXVECTOR3& pointA = simplex.GetPoint(1);
const D3DXVECTOR3& pointB = simplex.GetPoint(0);
const D3DXVECTOR3 AB = pointB - pointA;
const D3DXVECTOR3 AO = -pointA;
// Generate a new direction for the next point
// perpendicular to the line using triple product
D3DXVECTOR3 ABcrossAO;
D3DXVec3Cross(&ABcrossAO, &AB, &AO);
D3DXVec3Cross(&direction, &ABcrossAO, &AB);
}
D3DXVECTOR3 CollisionSolver::GetConvexHullPenetration(const CollisionMesh& particle,
const CollisionMesh& hull,
Simplex& simplex)
{
D3DXVECTOR3 furthestPoint;
D3DXVECTOR3 penetrationDirection;
float penetrationDistance = 0.0f;
bool penetrationFound = false;
const float minDistance = 0.1f;
const int maxIterations = 10;
int iteration = 0;
while(!penetrationFound && iteration < maxIterations)
{
++iteration;
const Face& face = simplex.GetClosestFaceToOrigin();
penetrationDirection = face.normal;
penetrationDistance = face.distanceToOrigin;
penetrationFound = penetrationDistance == 0.0f;
if(!penetrationFound)
{
// Check if there are any edge points beyond the closest face
furthestPoint = GetMinkowskiSumEdgePoint(face.normal, particle, hull);
const D3DXVECTOR3 faceToPoint = furthestPoint - simplex.GetPoint(face.indices[0]);
const float distance = fabs(D3DXVec3Dot(&faceToPoint, &face.normal));
penetrationFound = distance < minDistance;
if(!penetrationFound)
{
// Add the new point and extend the convex hull
simplex.ExtendFace(furthestPoint);
}
}
}
if(!penetrationFound)
{
// Fallback on the initial closest face
const Face& face = simplex.GetClosestFaceToOrigin();
penetrationDirection = face.normal;
penetrationDistance = face.distanceToOrigin;
}
if(particle.RenderSolverDiagnostics())
{
UpdateDiagnostics(simplex, furthestPoint);
}
return -(penetrationDirection * penetrationDistance);
}
