private static List <SupportTriangle> AddTriangle(
List <Edge> edge,
List <SupportTriangle> triangles,
Support p,
Vector3d centroid)
{
var result = new List <SupportTriangle>(triangles);
for (int i = 0; i < edge.Count; i++)
{
var normal = GeometryUtils.CalculateTriangleNormal(edge[i].A.s, edge[i].B.s, p.s);
var tri = new SupportTriangle(
edge[i].A,
edge[i].B,
p,
0.0,
0.0,
normal);
tri = TurnClockWiseNormal(tri, centroid);
result.Add(tri);
}
return(result);
}
private Vector3d GetMinDistance(
ref List <SupportTriangle> triangles,
Vector3d point,
ref int minTriangleIndex)
{
var result = new Vector3d();
var distanceBuf = new Vector3d();
double s = 0; double t = 0;
var buffer = new SupportTriangle();
double minDistance = double.MaxValue;
for (int i = 0; i < triangles.Count; i++)
{
buffer = triangles[i];
if (!GeometryUtils.TestCollinearity(
buffer.A.s,
buffer.B.s,
buffer.C.s))
{
distanceBuf = GeometryUtils.GetPointTriangleIntersection(
buffer.A.s,
buffer.B.s,
buffer.C.s,
point,
ref s,
ref t).Value;
buffer.SetValueS(s);
buffer.SetValueT(t);
}
else
{
continue;
}
triangles[i] = buffer;
double distance = Vector3d.Length(distanceBuf);
if (distance < minDistance)
{
minDistance = distance;
minTriangleIndex = i;
result = distanceBuf;
}
}
return(result);
}
private static SupportTriangle AddTriangle(
Support a,
Support b,
Support c)
{
var normal = GeometryUtils.CalculateTriangleNormal(a.s, b.s, c.s);
var epaTriangle = new SupportTriangle(
a,
b,
c,
0.0,
0.0,
normal);
return(epaTriangle);
}
public static void GetVertexFromMinkowsky(
SupportTriangle triangle,
VertexProperties[] vertexShape1,
VertexProperties[] vertexShape2,
ref CollisionPointOutput collisionPoint)
{
Vector3d a1 = vertexShape1[triangle.A.a].Vertex;
Vector3d ba1 = vertexShape1[triangle.B.a].Vertex - a1;
Vector3d ca1 = vertexShape1[triangle.C.a].Vertex - a1;
Vector3d a2 = vertexShape2[triangle.A.b].Vertex;
Vector3d ba2 = vertexShape2[triangle.B.b].Vertex - a2;
Vector3d ca2 = vertexShape2[triangle.C.b].Vertex - a2;
collisionPoint.SetA(
new VertexProperties(a1 + (ba1 * triangle.S) + (ca1 * triangle.T),
new int?[] { vertexShape1[triangle.A.a].ID, vertexShape1[triangle.B.a].ID, vertexShape1[triangle.C.a].ID }));
collisionPoint.SetB(
new VertexProperties(a2 + (ba2 * triangle.S) + (ca2 * triangle.T),
new int?[] { vertexShape2[triangle.A.b].ID, vertexShape2[triangle.B.b].ID, vertexShape2[triangle.C.b].ID }));
}
public static SupportTriangle TurnClockWiseNormal(
SupportTriangle triangle,
Vector3d v)
{
Vector3d centroidDiff = triangle.A.s - v;
Vector3d normal = triangle.Normal;
if (Vector3d.Dot(triangle.Normal, centroidDiff) < -1E-10)
{
normal = triangle.Normal * -1.0;
}
var tr = new SupportTriangle(
triangle.A,
triangle.B,
triangle.C,
triangle.S,
triangle.T,
normal);
return(tr);
}