private static EPAData EPA(ref GJKState state)
{
Vector3 origin = Vector3.zero - state.CurrentSimplex.PeekBack();
Vector3 ta, tb;
while (true)
{
Edge e = FindClosestEdge(origin, ref state);
Vector3 supportP = SupportFunction(e.EdgeNormal, state.GetPolytopeA, state.GetPolytopeB, out ta, out tb);
float d = Vector3.Dot(supportP, e.EdgeNormal);
if (d - e.Distance < 0.0001f)
{
EPAData data = new EPAData();
data.Normal = e.EdgeNormal;
data.Depth = d;
data.Contact = (ta + tb) / 2f;
return(data);
}
else
{
state.CurrentSimplex.Insert(e.PointA, supportP);
}
}
}
private static Edge FindClosestEdge(Vector3 origin, ref GJKState state)
{
List <Edge> edges = new List <Edge>();
for (int i = 0; i < state.CurrentSimplex.GetSize(); i++)
{
Vector3 a = state.CurrentSimplex.PeekAt(i);
Vector3 b;
if (i + 1 != state.CurrentSimplex.GetSize())
{
b = state.CurrentSimplex.PeekAt(i + 1);
}
else
{
b = state.CurrentSimplex.PeekAt(0);
}
edges.Add(new Edge(a, b, origin, i));
}
edges.Sort();
return(edges[0]);
}
public static void Collided(ref GJKState state)
{
Polytope polytopeA = state.GetPolytopeA;
Polytope polytopeB = state.GetPolytopeB;
// More than 20 it
if (state.Iteration > 20)
{
state.FinishRun = true;
state.IsCollided = false;
}
else
{
// First GJK run
if (state.CurrentSimplex.GetSize() == 0)
{
// Add the initial point
Vector3 ta, tb;
state.CurrentSimplex.Push(SupportFunction(polytopeB.GetCentre() - polytopeA.GetCentre(), polytopeA, polytopeB, out ta, out tb));
// Get a search direction from first point to origin
if (state.LastDirection == Vector3.zero)
{
Vector3 directionAToO = new Vector3(0f, 0f, 0f) - state.CurrentSimplex.PeekBack();
state.LastDirection = directionAToO;
}
}
// Second GJK run
else if (state.CurrentSimplex.GetSize() == 1)
{
// Add next point
Vector3 ta, tb;
state.CurrentSimplex.Push(SupportFunction(state.LastDirection, polytopeA, polytopeB, out ta, out tb));
// Check if this point passes origin
if (state.CurrentSimplex.PeekBack().IsInOppositeDirection(state.LastDirection))
{
// This object is definably not colliding as the second point in the direction of
// origin is not even passing it
state.FinishRun = true;
state.IsCollided = false;
}
}
else // Begin it
{
ProcessSimplex(ref state);
}
}
state.Iteration++;
}
void GJK()
{
a.meshGJK = new GJKMesh()
{
transform = a.transform, mesh = a.meshReference
};
b.meshGJK = new GJKMesh()
{
transform = b.transform, mesh = b.meshReference
};
state = new GJKState();
state.isColliding = GJKAlgorithm.Intersects(a.meshGJK, a.meshGJK.transform, b.meshGJK, b.meshGJK.transform, state);
}
// Update is called once per frame
private void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
if (_state == null)
{
_state = new GJKState(ref PolytopeA, ref PolytopeB);
_state.LastDirection = initDirection;
}
_minkowskisumPoints.Clear();
_minkowskisumPoints = MinkowskiSum.CalcMinkowskiSum(PolytopeA, PolytopeB);
_gjkSupportPoints.Clear();
if (!_state.FinishRun)
{
GJK.Collided(ref _state);
for (int i = 0; i < _state.CurrentSimplex.GetSize(); i++)
{
_gjkSupportPoints.Add(_state.CurrentSimplex.PeekAt(i));
}
}
else
{
// Physics res
float c = 0.5f;
PhysicsSim compA = _state.GetPolytopeA.GetComponent <PhysicsSim>();
PhysicsSim compB = _state.GetPolytopeB.GetComponent <PhysicsSim>();
compA.velocity = UIController.initVelocutyA;
compB.velocity = UIController.initVelocutyB;
float massTotal = compA.mass + compB.mass;
// Contacts
Vector3 lpA = _state.CurrentSimplex.PopBack();
Vector3 lpB = _state.CurrentSimplex.PopBack();
Vector3 cpA = _state.epaData.Contact;
Vector3 cpB = cpA;
// Inert
float invVal = 1f / 0.16f;
Matrix4x4 rotInert = Matrix4x4.identity;
rotInert.SetRow(0, new Vector4(invVal, 0f, 0f, 0f));
rotInert.SetRow(1, new Vector4(0f, invVal, 0f, 0f));
rotInert.SetRow(2, new Vector4(0f, 0f, 0f, invVal));
float j =
(-(1f + c) * Vector3.Dot(compA.velocity - compB.velocity, _state.epaData.Normal)) /
((1f / compA.mass + 1f / compB.mass) +
Vector3.Dot(
Vector3.Cross(rotInert * Vector3.Cross(cpA, _state.epaData.Normal), cpA) +
Vector3.Cross(rotInert * Vector3.Cross(cpB, _state.epaData.Normal), cpB),
_state.epaData.Normal
));
Vector4 wa = rotInert *
(
Vector3.Cross(
cpA,
j * _state.epaData.Normal
)
);
compA.AngularVelocity.x += wa.x;
compA.AngularVelocity.y += wa.y;
compA.AngularVelocity.z += wa.z;
Vector4 wb = rotInert *
(
Vector3.Cross(
cpB,
j * _state.epaData.Normal
)
);
compB.AngularVelocity.x -= wb.x;
compB.AngularVelocity.y -= wb.y;
compB.AngularVelocity.z -= wb.z;
compA.velocity = compA.velocity + ((j / compA.mass) * _state.epaData.Normal);
compB.velocity = compA.velocity - ((j / compB.mass) * _state.epaData.Normal);
_state = null;
}
}
}
public static void RenewState()
{
_state = null;
}
private static void ProcessSimplex(ref GJKState state)
{
// Line
if (state.CurrentSimplex.GetSize() == 2)
{
Vector3 pointA = state.CurrentSimplex.PeekFront();
Vector3 pointB = state.CurrentSimplex.PeekBack();
// Get line direction
// Since A to B was last used
Vector3 directionBToA = pointA - pointB;
// Get B to origin
Vector3 directionBToO = new Vector3(0f, 0f, 0f) - pointB;
// Check if they B to A is in same direction as B to origin
// So it usually means the origin is in between
if (directionBToA.IsInSameDirection(directionBToO))
{
// Triple cross product
state.LastDirection = Vector3.Cross(Vector3.Cross(directionBToA, directionBToO), directionBToA);
// Add third point
Vector3 ta, tb;
state.CurrentSimplex.Push(SupportFunction(state.LastDirection, state.GetPolytopeA, state.GetPolytopeB, out ta, out tb));
}
else // Second point is shit
{
// Drop oldest point and try again with direction B to origin
state.LastDirection = directionBToO;
state.CurrentSimplex.PopFront();
}
}
// Triangle
else if (state.CurrentSimplex.GetSize() == 3)
{
// Clear lines
state.MiscDebugLines.Clear();
// Get the triangle's normal
Vector3 pointA = state.CurrentSimplex.PeekAt(0); // Oldest
Vector3 pointB = state.CurrentSimplex.PeekAt(1);
Vector3 pointC = state.CurrentSimplex.PeekAt(2); // Newest
Vector3 trigNormal = Vector3.Cross(pointA - pointC, pointB - pointC);
// Get C to origin
Vector3 directionCToO = new Vector3(0f, 0f, 0f) - pointC;
// Get new direction
Vector3 newDirection;
// Check if we got the right direction for normal
if (trigNormal.IsInOppositeDirection(directionCToO))
{
// Negate normal
//newDirection = Vector3.Cross(Vector3.Cross(-trigNormal, directionCToO), -trigNormal);
newDirection = -trigNormal;
}
else
{
//newDirection = Vector3.Cross(Vector3.Cross(trigNormal, directionCToO), trigNormal);
newDirection = trigNormal;
}
state.LastDirection = newDirection.normalized;
// Add 4th point
Vector3 ta, tb;
state.CurrentSimplex.Push(SupportFunction(state.LastDirection, state.GetPolytopeA, state.GetPolytopeB, out ta, out tb));
}
// Tetrahedron
else
{
Vector3 origin = Vector3.zero;
Vector3 point0 = state.CurrentSimplex.PeekAt(0);
Vector3 point1 = state.CurrentSimplex.PeekAt(1);
Vector3 point2 = state.CurrentSimplex.PeekAt(2);
Vector3 point3 = state.CurrentSimplex.PeekAt(3);
Matrix4x4 d0 = new Matrix4x4();
Matrix4x4 d1 = new Matrix4x4();
Matrix4x4 d2 = new Matrix4x4();
Matrix4x4 d3 = new Matrix4x4();
Matrix4x4 d4 = new Matrix4x4();
d0.SetRow(0, new Vector4(point0.x, point0.y, point0.z, 1f));
d0.SetRow(1, new Vector4(point1.x, point1.y, point1.z, 1f));
d0.SetRow(2, new Vector4(point2.x, point2.y, point2.z, 1f));
d0.SetRow(3, new Vector4(point3.x, point3.y, point3.z, 1f));
d1.SetRow(0, new Vector4(origin.x, origin.y, origin.z, 1f));
d1.SetRow(1, new Vector4(point1.x, point1.y, point1.z, 1f));
d1.SetRow(2, new Vector4(point2.x, point2.y, point2.z, 1f));
d1.SetRow(3, new Vector4(point3.x, point3.y, point3.z, 1f));
d2.SetRow(0, new Vector4(point0.x, point0.y, point0.z, 1f));
d2.SetRow(1, new Vector4(origin.x, origin.y, origin.z, 1f));
d2.SetRow(2, new Vector4(point2.x, point2.y, point2.z, 1f));
d2.SetRow(3, new Vector4(point3.x, point3.y, point3.z, 1f));
d3.SetRow(0, new Vector4(point0.x, point0.y, point0.z, 1f));
d3.SetRow(1, new Vector4(point1.x, point1.y, point1.z, 1f));
d3.SetRow(2, new Vector4(origin.x, origin.y, origin.z, 1f));
d3.SetRow(3, new Vector4(point3.x, point3.y, point3.z, 1f));
d4.SetRow(0, new Vector4(point0.x, point0.y, point0.z, 1f));
d4.SetRow(1, new Vector4(point1.x, point1.y, point1.z, 1f));
d4.SetRow(2, new Vector4(point2.x, point2.y, point2.z, 1f));
d4.SetRow(3, new Vector4(origin.x, origin.y, origin.z, 1f));
// Determinants
float det0 = d0.determinant;
float det1 = d1.determinant;
float det2 = d2.determinant;
float det3 = d3.determinant;
float det4 = d4.determinant;
// Degenerate simplex :(
if (det0 == 0f)
{
state.CurrentSimplex.PopBack();
}
// Check if all five det have same sign
else if (Mathf.Sign(det0) == Mathf.Sign(det1) &&
Mathf.Sign(det1) == Mathf.Sign(det2) &&
Mathf.Sign(det2) == Mathf.Sign(det3) &&
Mathf.Sign(det3) == Mathf.Sign(det4))
{
state.FinishRun = true;
Debug.Log("Collided.");
state.epaData = EPA(ref state);
}
// Drop point 0
else if (Mathf.Sign(det0) != Mathf.Sign(det1))
{
state.CurrentSimplex.RemoveAt(0);
}
// Drop point 1
else if (Mathf.Sign(det0) != Mathf.Sign(det2))
{
state.CurrentSimplex.RemoveAt(1);
}
// Drop point 2
else if (Mathf.Sign(det0) != Mathf.Sign(det3))
{
state.CurrentSimplex.RemoveAt(2);
}
// Drop point 3
else if (Mathf.Sign(det0) != Mathf.Sign(det4))
{
state.CurrentSimplex.RemoveAt(3);
}
}
}
//public void SetDirectionX(string s) {
// float x;
// float.TryParse(s, out x);
// direction = new Vector3(x, direction.y, direction.z);
//}
//public void SetDirectionY(string s) {
// float y;
// float.TryParse(s, out y);
// direction = new Vector3(direction.x, y, direction.z);
//}
//public void SetDirectionZ(string s) {
// float z;
// float.TryParse(s, out z);
// direction = new Vector3(direction.x, direction.y, z);
//}
public static bool Intersects(IConvexRegion regionOne, Transform oneTrans, IConvexRegion regionTwo, Transform twoTrans, GJKState state) {
// Get an initial point on the Minkowski difference.
Vector3 s = Support(regionOne, regionTwo, twoTrans.position - oneTrans.position, state, out sa);
// Create our initial simplex.
Simplex simplex = new Simplex();
simplex.Add(s);
// TODO: Choose an initial direction.
direction = -s;
state.simplices.Add(simplex.Clone());
// Choose a maximim number of iterations to avoid an
// infinite loop during a non-convergent search.
int maxIterations = 32;
for (int i = 0; i < maxIterations; i++) {
// Get our next simplex point toward the origin.
Vector3 a = Support(regionOne, regionTwo, direction, state, out sa, out sb);
// If we move toward the origin and didn't pass it
// then we never will and there's no intersection.
if (a.IsInOppositeDirection(direction)) {
return false;
}
// otherwise we add the new point to the simplex and process it.
simplex.Add(a);
state.simplices.Add(simplex.Clone());
// Here we either find a collision or we find the closest feature of
// the simplex to the origin, make that the new simplex and update the direction
// to move toward the origin from that feature.
if (ProcessSimplex(simplex, ref direction)) {
float tolerance = 0.000001f; // Or another such small number
while (true) {
Edge e = simplex.FindClosestEdge();
Vector3 p = Support(regionOne, regionTwo, e.normal, state, out sa, out sb);
float proj = Vector3.Dot(p, e.normal);
if (proj - e.distance < tolerance) {
}
}
else {
}
}
return true;
}