static GJKoutput GJK(Collider a, Collider b)
{
Vector2 GetVertNearestToOrigin(Line line)
{
return(GetFarthestPointInDifference(GetNormalToLineFacingOrigin(line), a, b));
}
bool SimplexContainsOrigin(List <Vector2> simplex)
{
for (int i = 0; i < 3; i++)
{
int j = (i + 1) % 3;
int k = (i + 2) % 3;
//side jk third vert is i
//a_k * x + b_k * y + c_k = 0
float a_k = simplex[j].y - simplex[k].y;
float b_k = simplex[k].x - simplex[j].x;
float c_k = Vector2.CrossProduct(simplex[j], simplex[k]);
float half_space_simplex_i = sgn(a_k * simplex[i].x + b_k * simplex[i].y + c_k);
float half_space_origin = sgn(c_k);
if (half_space_simplex_i == -half_space_origin)//if Simplex[i] and origin
//are in different half-spaces
{
return(false);
}
}
return(true);
}
Vector2 init_direction = new Vector2(1, 0);//may be any instread of Vector2.zero
List <Vector2> Simplex = new List <Vector2>();
Simplex.Add(GetFarthestPointInDifference(init_direction, a, b));
Simplex.Add(GetFarthestPointInDifference(-init_direction, a, b));
Simplex.Add(GetVertNearestToOrigin(new Line(Simplex[0], Simplex[1])));
while (true)
{
if (SimplexContainsOrigin(Simplex))
{
return(new GJKoutput {
result = true, simplex = Simplex
});
}
//determine nearest side to origin
//
//if we have triangle the nearest side to origin is side such that another vertex of simplex
//and origin lying in different half-spaces relatively this side
//
//ax + by + c = 0
//if sgn(a * dot0.x + b * dot0.y + c) == - sgn(a * dot1.x + b * dot1.y + c) then dot 0 and dot 1
//lying on different sides of line ax + by + c = 0
for (int i = 0; i < 3; i++)
{
int j = (i + 1) % 3;
int k = (i + 2) % 3;
//side ij third vert is k
float a_k = Simplex[i].y - Simplex[j].y;
float b_k = Simplex[j].x - Simplex[i].x;
float c_k = Vector2.CrossProduct(Simplex[i], Simplex[j]);
if (sgn(a_k * Simplex[k].x + b_k * Simplex[k].y + c_k) == -sgn(c_k))
{
//needed side
Simplex[k] = GetVertNearestToOrigin(new Line(Simplex[i], Simplex[j]));
if (Simplex[k] * GetNormalToLineFacingOrigin(new Line(Simplex[i], Simplex[j])) <= 0)
{
//no origin in minkovski difference
return(new GJKoutput()
{
result = false
});
}
if (((Simplex[k].x == Simplex[i].x) && (Simplex[k].y == Simplex[i].y)) ||
((Simplex[k].x == Simplex[j].x) && (Simplex[k].y == Simplex[j].y)))
{
//no origin in minkovski difference
return(new GJKoutput()
{
result = false
});
}
break;
}
}
}
}
internal static void Resolve(Collider a, Collider b, CollisionInfo info)
{
Body a_body = a.gameobject.body;
Body b_body = b.gameobject.body;
float bouncity = Body.CalculateBouncity(a_body.Bouncity, b_body.Bouncity);
float static_friction = Body.CalculateStaticFriction(a_body.StaticFriction, b_body.StaticFriction);
float dynamic_friction = Body.CalculateDynamicFriction(a_body.DynamicFriction, b_body.DynamicFriction);
int ContactPointCount = info.ContactPoints.Count;
for (int i = 0; i < ContactPointCount; i++)
{
var contact = info.ContactPoints[i];
Vector2 ra = contact.point - (a_body.CenterOfMass_local + a.gameobject.Position);
Vector2 rb = contact.point - (b_body.CenterOfMass_local + b.gameobject.Position);
Vector2 RelativeVelocity = (b_body.Velocity + Vector2.CrossProduct(b_body.AngularVelocity, rb))
- (a_body.Velocity + Vector2.CrossProduct(a_body.AngularVelocity, ra));
float VelAlongNormal = (RelativeVelocity * contact.normal);
if (VelAlongNormal > 0)
{
continue;
}
float denominator = a_body.Inv_mass + b_body.Inv_mass
+ sqr(Vector2.CrossProduct(ra, contact.normal)) * a_body.Inv_MomentOfInertia
+ sqr(Vector2.CrossProduct(rb, contact.normal)) * b_body.Inv_MomentOfInertia;
denominator *= ContactPointCount;
//formula for impulse scalar
float ImpulseScalar = (-(1 + bouncity) * VelAlongNormal) / denominator;
Vector2 Impulse = contact.normal * ImpulseScalar;
//applying impulse
a_body.Velocity -= a_body.Inv_mass * Impulse;
a_body.AngularVelocity -= a_body.Inv_MomentOfInertia * Vector2.CrossProduct(ra, Impulse);
b_body.Velocity += b_body.Inv_mass * Impulse;
b_body.AngularVelocity += b_body.Inv_MomentOfInertia * Vector2.CrossProduct(rb, Impulse);
//**********************
//friction
//re-count relative velocity
RelativeVelocity = (b_body.Velocity + Vector2.CrossProduct(b_body.AngularVelocity, rb))
- (a_body.Velocity + Vector2.CrossProduct(a_body.AngularVelocity, ra));
Vector2 tangent = (RelativeVelocity - (RelativeVelocity * contact.normal)
* contact.normal).normalized;
float jt = -(RelativeVelocity * tangent) / denominator;
Vector2 FrictionImpulse;
if (abs(jt) < ImpulseScalar * static_friction)
{
FrictionImpulse = jt * tangent;
}
else
{
FrictionImpulse = -ImpulseScalar * tangent * dynamic_friction;
}
//applying friction impulse
a_body.Velocity -= a_body.Inv_mass * FrictionImpulse;
a_body.AngularVelocity -= a_body.Inv_MomentOfInertia * Vector2.CrossProduct(ra, FrictionImpulse);
b_body.Velocity += b_body.Inv_mass * FrictionImpulse;
b_body.AngularVelocity += b_body.Inv_MomentOfInertia * Vector2.CrossProduct(rb, FrictionImpulse);
}
foreach (var contact in info.ContactPoints)
{
//positional correction
float percent = 0.6f;
float slop = 0.01f;
Vector2 correction = ((max(contact.depth - slop, 0)
/ (a_body.Inv_mass + b_body.Inv_mass)) * contact.normal * percent) / ContactPointCount;
a.gameobject.Position -= a_body.Inv_mass * correction;
b.gameobject.Position += b_body.Inv_mass * correction;
//*******************
}
}
public static CollisionInfo PolygonvsPolygon(Polygon a, Polygon b)
{
int GetFarthestPointIndex(Vector2[] polygon, Vector2 direction)
{
int index = 0;
if (direction.y == 0)//x coord check
{
for (int vert = 0; vert < polygon.Length; vert++)
{
if (sgn(polygon[vert].x - polygon[index].x) == sgn(direction.x))//if direction == right searching max(x) else min
{
index = vert;
}
}
return(index);
}
float k = -direction.x / direction.y; //perpendicular direction angular coefficient
//y = kx + b line equation. we find line equation for each point and b
//is the intersection point of this line with Oy. Extremum of b means that there is the
//ultimate point in the direction, perpendicular to this line i.e. "direction" vector
for (int vert = 0; vert < polygon.Length; vert++)
{
if (sgn((polygon[vert].y - polygon[vert].x * k) - (polygon[index].y - polygon[index].x * k)) ==
sgn(direction.y))//if top direction searching max(b) else min(b)
{
index = vert;
}
}
return(index);
}
float GetSqrDistanceFromPointToLine(Vector2 point, Line line)
{
//ax + by + c = 0
//distance = |a * x1 + b * y1 + c| / sqrt(a ^ 2 + b ^ 2)
float a_k = line.start.y - line.end.y;
float b_k = line.end.x - line.start.x;
float c_k = Vector2.CrossProduct(line.start, line.end);
return(sqr(a_k * point.x + b_k * point.y + c_k) / (sqr(a_k) + sqr(b_k)));
}
bool PolygonContainsPoint(Vector2[] poly, Vector2 point)
{
for (int i = 0; i < poly.Length; i++)
{
int j = (i + 1) % poly.Length;
float a_k = poly[i].y - poly[j].y;
float b_k = poly[j].x - poly[i].x;
float c_k = Vector2.CrossProduct(poly[i], poly[j]);
int k = (i + 2) % poly.Length;
float vert_halfspace = sgn(a_k * poly[k].x + b_k * poly[k].y + c_k);
float point_halfspace = sgn(a_k * point.x + b_k * point.y + c_k);
if (point_halfspace == -vert_halfspace)
{
return(false);
}
}
return(true);
}
CollisionInfo info = new CollisionInfo();
GJKoutput GJKoutput = GJK(a, b);
if (!GJKoutput.result)
{
return new CollisionInfo()
{
ContactPoints = new List <CollisionInfo.ContactPoint>()
}
}
;
EPAoutput EPAoutput = EPA(GJKoutput.simplex, a, b);
#region shitcode
Vector2 normal = EPAoutput.normal;
float distance = EPAoutput.distance;
info.ContactPoints = new List <CollisionInfo.ContactPoint>();
bool flip = false;
int incident_edge_start = GetFarthestPointIndex(a.Verts, normal);
int incident_edge_end = (incident_edge_start + 1) % a.Verts.Length;
if (!(abs((a.Verts[incident_edge_start] - a.Verts[incident_edge_end]) * normal) < tolerance))
{
incident_edge_end = (incident_edge_start - 1 + a.Verts.Length) % a.Verts.Length;
}
if (!(abs((a.Verts[incident_edge_start] - a.Verts[incident_edge_end]) * normal) < tolerance))
{
flip = true;
incident_edge_start = GetFarthestPointIndex(b.Verts, -normal);
incident_edge_end = (incident_edge_start + 1) % b.Verts.Length;
if (!(abs((b.Verts[incident_edge_start] - b.Verts[incident_edge_end]) * normal) < tolerance))
{
incident_edge_end = (incident_edge_start - 1 + b.Verts.Length) % b.Verts.Length;
}
}
Line incident_edge;
if (!flip)
{ //a
incident_edge = new Line(a.Verts[incident_edge_start], a.Verts[incident_edge_end]);
}
else
{ //b
incident_edge = new Line(b.Verts[incident_edge_start], b.Verts[incident_edge_end]);
}
if (!flip)
{ //a
int b_farthest = GetFarthestPointIndex(b.Verts, -normal);
int b_farthest_left_neighb = (b_farthest + 1) % b.Verts.Length;
int b_farthest_right_neighb = (b_farthest - 1 + b.Verts.Length) % b.Verts.Length;
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = distance,
normal = normal,
point = b.Verts[b_farthest] + normal * distance
});
bool a_contains_left = PolygonContainsPoint(a.Verts, b.Verts[b_farthest_left_neighb]);
bool a_contains_right = PolygonContainsPoint(a.Verts, b.Verts[b_farthest_right_neighb]);
if (a_contains_left && a_contains_right)
{
float left_sqr_dist = GetSqrDistanceFromPointToLine(b.Verts[b_farthest_left_neighb], incident_edge);
float right_sqr_dist = GetSqrDistanceFromPointToLine(b.Verts[b_farthest_right_neighb], incident_edge);
int b_almostfarthest = (right_sqr_dist > left_sqr_dist) ? b_farthest_right_neighb : b_farthest_left_neighb;
float b_almost_farthest_distance = sqrt(max(right_sqr_dist, left_sqr_dist));
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = b_almost_farthest_distance,
normal = normal,
point = b.Verts[b_almostfarthest] + normal * b_almost_farthest_distance
});
}
else if (!a_contains_left && !a_contains_right)
{
return(info);
}
else if (a_contains_left)
{
float dist = sqrt(GetSqrDistanceFromPointToLine(b.Verts[b_farthest_left_neighb], incident_edge));
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = dist,
normal = normal,
point = b.Verts[b_farthest_left_neighb] + normal * dist
});
}
else
{
float dist = sqrt(GetSqrDistanceFromPointToLine(b.Verts[b_farthest_right_neighb], incident_edge));
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = dist,
normal = normal,
point = b.Verts[b_farthest_right_neighb] + normal * dist
});
}
return(info);
}
else
{ //b
int a_farthest = GetFarthestPointIndex(a.Verts, normal);
int a_farthest_left_neighb = (a_farthest + 1) % a.Verts.Length;
int a_farthest_right_neighb = (a_farthest - 1 + a.Verts.Length) % a.Verts.Length;
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = distance,
normal = normal,
point = a.Verts[a_farthest] + normal * distance
});
bool b_contains_left = PolygonContainsPoint(b.Verts, a.Verts[a_farthest_left_neighb]);
bool b_contains_right = PolygonContainsPoint(b.Verts, a.Verts[a_farthest_right_neighb]);
if (b_contains_left && b_contains_right)
{
float left_sqr_dist = GetSqrDistanceFromPointToLine(a.Verts[a_farthest_left_neighb], incident_edge);
float right_sqr_dist = GetSqrDistanceFromPointToLine(a.Verts[a_farthest_right_neighb], incident_edge);
int a_almostfarthest = (right_sqr_dist > left_sqr_dist) ? a_farthest_right_neighb : a_farthest_left_neighb;
float a_almost_farthest_distance = sqrt(max(right_sqr_dist, left_sqr_dist));
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = a_almost_farthest_distance,
normal = normal,
point = a.Verts[a_almostfarthest] + normal * a_almost_farthest_distance
});
}
else if (!b_contains_left && !b_contains_right)
{
return(info);
}
else if (b_contains_left)
{
float dist = sqrt(GetSqrDistanceFromPointToLine(a.Verts[a_farthest_left_neighb], incident_edge));
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = dist,
normal = normal,
point = a.Verts[a_farthest_left_neighb] + normal * dist
});
}
else
{
float dist = sqrt(GetSqrDistanceFromPointToLine(a.Verts[a_farthest_right_neighb], incident_edge));
info.ContactPoints.Add(new CollisionInfo.ContactPoint()
{
depth = dist,
normal = normal,
point = a.Verts[a_farthest_right_neighb] + normal * dist
});
}
return(info);
}
#endregion
}
