private static FEdge GetBestEdge_sat(ref Vector2[] vertices, Vector2 normal)
{
float maxDot = Vector2.Dot(vertices[0], normal);
int vertIndex = 0;
for (int i = 1; i < vertices.Length; i++)
{
float dot = Vector2.Dot(vertices[i], normal);
if (dot > maxDot)
{
maxDot = dot;
vertIndex = i;
}
}
FEdge[] ngb = CollMeshUtils.GetVertexEdges(vertIndex, ref vertices);
Vector2 e1Dir = ngb[0].BV - ngb[0].AV;
Vector2 e2Dir = ngb[1].AV - ngb[1].BV;
if (Vector2.Dot(normal, e1Dir) <= Vector2.Dot(normal, e2Dir))
{
return(ngb[0]);
}
else
{
return(ngb[1]);
}
}
private static FSATAxis[] GetAxis_sat(ref Vector2[] vertices)
{
FSATAxis[] res = new FSATAxis[vertices.Length];
for (int i = 0; i < res.Length; i++)
{
int nxtVertex = CollMeshUtils.RightIndex(i, vertices.Length);
res[i] = new FSATAxis();
res[i].Edge = new FEdge(vertices[i], vertices[nxtVertex], i, nxtVertex);
res[i].Axis = MeshUtils.CalcNormal(res[i].Edge.AV, res[i].Edge.BV);
res[i].Edge.Normal = res[i].Axis;
}
return(res);
}
protected override void UpdateBodyInformation_internal(float density)
{
Inertia = 0f;
Mass = 0f;
Area = 0f;
for (int i = 0; i < CollisionBody.Vertices.Length; i++)
{
Vector2 a = CollisionBody.Vertices[i];
Vector2 b = CollisionBody.Vertices[CollMeshUtils.RightIndex(i, CollisionBody.Vertices.Length)];
float areaTri = Mathf.Abs(a.Cross(b)) * 0.5f;
float massTri = density * areaTri;
Area += areaTri;
Mass += massTri;
Inertia += massTri * (a.sqrMagnitude + b.sqrMagnitude + Vector2.Dot(a, b)) / 6f;
}
}
private static bool SphereSAT(FCollPoly p1, FCollSphere p2, out CollisionContact manifold)
{
FSATAxis[] axisPoly = GetAxis_sat(ref p1.Vertices);
FSATAxis circleAxis = GetCircleAxis_sat(p2.Center, ref p1.Vertices);
FSATAxis collisionAxis = axisPoly[0];
float penetration = 100000f;
manifold = null;
for (int i = 0; i < axisPoly.Length; i++)
{
Vector2 proj1 = ProjOntoAxis_sat(axisPoly[i], ref p1.Vertices);
float circleCenter = Vector2.Dot(p2.Center, axisPoly[i].Axis);
Vector2 proj2 = new Vector2(circleCenter - p2.Radius, circleCenter + p2.Radius);
float overlap = 0f;
if (!GetOverlapFromProjection_sat(proj1, proj2, out axisPoly[i].BodyA, out overlap))
{
return(false);
}
else
{
if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
{
float mins = Mathf.Abs(proj1.x - proj2.x);
float maxs = Mathf.Abs(proj1.y - proj2.y);
if (mins < maxs)
{
overlap += mins;
}
else
{
overlap += maxs;
}
}
if (overlap < penetration)
{
collisionAxis = axisPoly[i];
penetration = overlap;
}
}
}
//Circle axis
{
Vector2 proj1 = ProjOntoAxis_sat(circleAxis, ref p1.Vertices);
float circleCenter = Vector2.Dot(p2.Center, circleAxis.Axis);
Vector2 proj2 = new Vector2(circleCenter - p2.Radius, circleCenter + p2.Radius);
float overlap = 0f;
if (!GetOverlapFromProjection_sat(proj1, proj2, out circleAxis.BodyA, out overlap))
{
return(false);
}
else
{
if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
{
float mins = Mathf.Abs(proj1.x - proj2.x);
float maxs = Mathf.Abs(proj1.y - proj2.y);
if (mins < maxs)
{
overlap += mins;
}
else
{
overlap += maxs;
}
}
if (overlap < penetration)
{
collisionAxis = circleAxis;
penetration = overlap;
}
}
}
//Debug.Log("Collision axis: " + collisionAxis.Axis);
//Debug.Log("Penetration: " + penetration);
manifold = new CollisionContact();
FEdge eRef;
if (!collisionAxis.BodyA)
{
collisionAxis.Axis *= -1.0f;
}
eRef = GetBestEdge_sat(ref p1.Vertices, collisionAxis.Axis);
eRef.Normal = MeshUtils.CalcNormal(eRef.AV, eRef.BV);
Vector2 vLeft = p1.Vertices[CollMeshUtils.LeftIndex(eRef.AI, p1.Vertices.Length)];
Vector2 vRight = p1.Vertices[CollMeshUtils.RightIndex(eRef.BI, p1.Vertices.Length)];
Vector2 normLeft = MeshUtils.CalcNormalRaw(vLeft, eRef.AV);
Vector2 normRight = MeshUtils.CalcNormalRaw(eRef.BV, vRight);
Vector2 leftRel = p2.Center - eRef.AV;
Vector2 rightRel = p2.Center - eRef.BV;
if (Vector2.Dot(normLeft, leftRel) > 0f && Vector2.Dot(eRef.Normal, leftRel) > 0f)
{
manifold.Penetration = penetration;
manifold.Normal = collisionAxis.Axis;
manifold.ContactPoints = new Vector2[] { p2.Center - leftRel.normalized * p2.Radius };
}
else if (Vector2.Dot(normRight, rightRel) > 0f && Vector2.Dot(eRef.Normal, rightRel) > 0f)
{
manifold.Penetration = penetration;
manifold.Normal = collisionAxis.Axis;
manifold.ContactPoints = new Vector2[] { p2.Center - rightRel.normalized * p2.Radius };
}
else
{
manifold.Penetration = penetration;
manifold.Normal = collisionAxis.Axis;
manifold.ContactPoints = new Vector2[] { p2.Center - eRef.Normal * p2.Radius };
}
manifold.BodyAInc = false;
manifold.EdgeNormalA = eRef.Normal;
manifold.EdgeNormalB = (p2.Center - manifold.ContactPoints[0]).normalized;
return(true);
}
