public TriangleResults[] TestForCollsion(Triangle b)
{
int i = 0;
TriangleResults[] contacts = new TriangleResults[6];
foreach (Vector2 pb in b.vertexes)
{
contacts[i] = PointOfCollision(pb + b.position);
if (i != 0 && contacts[i - 1].contact && contacts[i].contact)
{
contacts[i].contact = false;
contacts[i - 1].p1 = contacts[i].point;
contacts[i - 1].p2 = contacts[i - 1].point;
contacts[i - 1].point += contacts[i].point;
contacts[i - 1].point /= 2;
contacts[i - 1].sameSide = true;
}
i++;
}
foreach (Vector2 pa in vertexes)
{
TriangleResults results = b.PointOfCollision(pa + position);
results.normal *= -1;
contacts[i] = results;
if (i != 4 && contacts[i - 1].contact && contacts[i].contact)
{
contacts[i].contact = false;
contacts[i - 1].p1 = contacts[i].point;
contacts[i - 1].p2 = contacts[i - 1].point;
contacts[i - 1].point += contacts[i].point;
contacts[i - 1].point /= 2;
contacts[i - 1].sameSide = true;
}
i++;
}
return(contacts);
}
public TriangleResults PointOfCollision(Vector2 vertex)
{
Vector2 A, B, C, v0, v1, v2;
A = vertexes[0] + position;
B = vertexes[1] + position;
C = vertexes[2] + position;
v0 = C - A;
v1 = B - A;
v2 = vertex - A;
float d00, d01, d02, d11, d12;
d00 = Vector2.DotProduct(v0, v0);
d01 = Vector2.DotProduct(v0, v1);
d02 = Vector2.DotProduct(v0, v2);
d11 = Vector2.DotProduct(v1, v1);
d12 = Vector2.DotProduct(v1, v2);
float denom, u, v, w;
denom = d00 * d11 - d01 * d01;
u = (d11 * d02 - d01 * d12) / denom;
v = (d00 * d12 - d01 * d02) / denom;
w = 1 - u - v;
TriangleResults results = new TriangleResults {
contact = false
};
if ((u > 0) && (v > 0) && (u + v < 1))
{
results.contact = true;
}
else
{
return(results);
}
int p1, p2;
if (u < v && u < w)
{
p1 = 0; p2 = 1;
}
else if (v < u && v < w)
{
p1 = 0; p2 = 2;
}
else
{
p1 = 1; p2 = 2;
}
Vector2 vp1, vp2;
vp1 = vertexes[p1] + position;
vp2 = vertexes[p2] + position;
results.normal = (vp2 - vp1).Normal();
if (PointOfIntersection(vp1, vp2, vertex, vertex - results.normal, out results.point))
{
results.depth = results.point.DistanceFrom(vertex);
}
else
{
results.depth = 0f;
}
results.normal = (vp2 - vp1).Normal(position + center, results.point);
return(results);
}