private bool CheckCollision(PrismCollision collision)
{
Debug.Log("beginning of check collision");
// Task 1. Determine whether there is an actual collision using the GJK
var prismA = collision.a;
var prismB = collision.b;
// For each collision, create a simplex to calculate Minkowski sum
//Simplex simplex = new Simplex();
// Oho Yeah it's working now!
Simplex gjk_simp = GJK_collision(prismA, prismB, new Simplex());
// Task 2. If there is, compute the penetration depth vector using EPA algorithm
// EPA calculate penetration depth:
// if (gjk_simp != null)
// {
// collision.penetrationDepthVectorAB = EPA_pd(prismA, prismB, gjk_simp);
// Debug.Log("collision");
// return true;
// }
// else
// {
// Debug.Log("no collision");
// return false;
// }
collision.penetrationDepthVectorAB = Vector3.zero;
return(true);
}
private bool CheckCollision(PrismCollision collision)
{
var prismA = collision.a;
var prismB = collision.b;
var centerA = prismA.transform.position;
var centerB = prismB.transform.position;
//Subtracts centers of both prisms to find the initial direction of the vector to perform GJK on.
Vector3 d = centerB - centerA;
// If GJK returns points we have a simplex, otherwise we have an empty list of vectors.
Simplex GJKVector = GJK(prismA, prismB, d);
if (GJKVector != null)
{
print("GJK Not null, starting EPA " + prismA.name + " " + prismB.name);
collision.penetrationDepthVectorAB = EPA(GJKVector, prismA, prismB);
return(true);
}
else
{
return(false);
}
}
private bool CheckCollision(PrismCollision collision)
{
var prismA = collision.a;
var prismB = collision.b;
collision.penetrationDepthVectorAB = Vector3.zero;
return(true);
}
private Vector3 getSupportVector(PrismCollision collision, Vector3 d)
{
var prismA = collision.a;
var prismB = collision.b;
Vector3 aPoint = getSupportPoint(prismA.points, d);
Vector3 bPoint = getSupportPoint(prismB.points, -d);
//print(aPoint);
//print(bPoint);
//return the minkowski point
return(aPoint - bPoint);
}
private IEnumerable <PrismCollision> PotentialCollisions()
{
List <belonger> meep = new List <belonger>();
for (int i = 0; i < prisms.Count; i++)
{
belonger minx = new belonger();
belonger maxx = new belonger();
minx.xval = prisms[i].points.Min(point => point.x);
maxx.xval = prisms[i].points.Max(point => point.x);
maxx.max = true;
minx.max = false;
Debug.Log("these are our generated x vals" + minx.xval + ' ' + maxx.xval);
minx.p = maxx.p = prisms[i];
meep.Add(minx);
meep.Add(maxx);
}
foreach (belonger b in meep)
{
Debug.Log("this is supposed to be the list" + b.xval + "this is if its max" + b.max);
}
meep.Sort(delegate(belonger x, belonger y) { return(x.xval.CompareTo(y.xval)); });
foreach (belonger b in meep)
{
Debug.Log("this is supposed to be the sorted list entry" + b.xval + "this is if its max" + b.max);
}
Debug.Log("This is supposed to be the ordered list" + meep);
List <Prism> activelist = new List <Prism>();
foreach (belonger b in meep)
{
if (b.max == false)
{
foreach (Prism p in activelist)
{
var checkPrisms = new PrismCollision();
checkPrisms.a = p;
checkPrisms.b = b.p;
yield return(checkPrisms);
}
activelist.Add(b.p);
}
else
{
activelist.Remove(b.p);
}
}
yield break;
}
private IEnumerable <PrismCollision> QuadtreeRecursion(QuadTree quadtree)
{
if (quadtree.subtree != null)
{
Debug.Log("20");
foreach (PrismCollision check in QuadtreeRecursion(quadtree.subtree[0]))
{
yield return(check);
}
foreach (PrismCollision check in QuadtreeRecursion(quadtree.subtree[1]))
{
yield return(check);
}
foreach (PrismCollision check in QuadtreeRecursion(quadtree.subtree[2]))
{
yield return(check);
}
foreach (PrismCollision check in QuadtreeRecursion(quadtree.subtree[3]))
{
yield return(check);
}
}
if (quadtree.father != null)
{
for (int i = 0; i < quadtree.objects.Count; i++)
{
foreach (PrismCollision check in fatherRecursion(quadtree.father, quadtree.objects[i]))
{
yield return(check);
}
}
}
for (int i = 0; i < quadtree.objects.Count - 1; i++)
{
if (quadtree.objects[i].check == false)
{
for (int j = i + 1; j < quadtree.objects.Count; j++)
{
if (quadtree.objects[i].prismObject.name != quadtree.objects[j].prismObject.name)
{
Debug.Log("30");
var check = new PrismCollision();
check.a = quadtree.objects[i];
check.b = quadtree.objects[j];
quadtree.objects[i].check = true;
yield return(check);
}
}
}
}
}
private void ResolveCollision(PrismCollision collision)
{
var prismObjA = collision.a.prismObject;
var prismObjB = collision.b.prismObject;
var pushA = -collision.penetrationDepthVectorAB / 2;
var pushB = collision.penetrationDepthVectorAB / 2;
prismObjA.transform.position += pushA;
prismObjB.transform.position += pushB;
Debug.DrawLine(prismObjA.transform.position, prismObjA.transform.position + collision.penetrationDepthVectorAB, Color.cyan, UPDATE_RATE);
}
private IEnumerable <PrismCollision> PotentialCollisions()
{
ktree = new KdTree <float, int>(2, new FloatMath());
for (int i = 0; i < prisms.Count; i++)
{
var prism = prisms[i];
foreach (var point in prism.points)
{
ktree.Add(new[] { point.x, point.z }, i);
}
}
print(prisms.Count);
for (int i = 0; i < prisms.Count; i++)
{
if (prisms[i].points.Length == 0)
{
continue;
}
var prismI = prisms[i];
var prismTransformI = prismObjects[i].transform;
var bboxI = BoundingBox(prismI.points);
float radius = Vector3.Distance(bboxI[0], bboxI[1]) / 2;
//print(radius);
float[] position = new float[] { prismTransformI.position.x, prismTransformI.position.z };
foreach (KdTree.KdTreeNode <float, int> node in ktree.RadialSearch(position, radius))
{
int j = node.Value;
if (j == i)
{
continue;
}
var prismJ = prisms[j];
var prismTransformJ = prismObjects[j].transform;
var bboxJ = BoundingBox(prismJ.points);
if (AreBoxesColliding(bboxI, bboxJ))
{
var checkPrisms = new PrismCollision();
checkPrisms.a = prisms[i];
checkPrisms.b = prisms[j];
print("in here");
yield return(checkPrisms);
}
}
}
yield break;
}
private IEnumerable <PrismCollision> PotentialCollisions()
{
for (int i = 0; i < prisms.Count; i++)
{
for (int j = i + 1; j < prisms.Count; j++)
{
var checkPrisms = new PrismCollision();
checkPrisms.a = prisms[i];
checkPrisms.b = prisms[j];
yield return(checkPrisms);
}
}
yield break;
}
private void ResolveCollision(PrismCollision collision)
{
print("RESOLVING");
var pushA = -collision.penetrationDepthVectorAB / 2;
var pushB = collision.penetrationDepthVectorAB / 2;
for (int i = 0; i < collision.a.pointCount; i++)
{
collision.a.points[i] += pushA;
}
for (int i = 0; i < collision.b.pointCount; i++)
{
collision.b.points[i] += pushB;
}
}
private void ResolveCollision(PrismCollision collision)
{
var prismObjA = collision.a.prismObject;
var prismObjB = collision.b.prismObject;
var pushA = -collision.penetrationDepthVectorAB / 2;
var pushB = collision.penetrationDepthVectorAB / 2;
for (int i = 0; i < collision.a.pointCount; i++)
{
collision.a.points[i] += pushA;
}
for (int i = 0; i < collision.b.pointCount; i++)
{
collision.b.points[i] += pushB;
}
Debug.DrawLine(prismObjA.transform.position, prismObjA.transform.position + collision.penetrationDepthVectorAB, Color.cyan, UPDATE_RATE);
}
private IEnumerable <PrismCollision> PotentialCollisions()
{
for (int i = 0; i < prisms.Count; i++)
{
Prism p = prisms[i];
var nearestNeighbor = prisms.FindClosest(p.transform.position);
Debug.Log(nearestNeighbor == p);
if (!nearestNeighbor.Equals(p))
{
var checkPrisms = new PrismCollision();
checkPrisms.a = p;
checkPrisms.b = nearestNeighbor;
Debug.DrawLine(p.transform.position, nearestNeighbor.transform.position, Color.red);
yield return(checkPrisms);
}
}
yield break;
}
private IEnumerable<PrismCollision> PotentialCollisions()
{
for (int i = 0 ; i < prisms.Count ; i++){
newobj.Clear();
//newobj.AddAll(prisms.Where((v, k) => k >= prisms[i]).ToList());
var x = prisms.FindClosest(prisms[i].transform.position);
print(x);
print(prisms[i]);
if(x.Equals(prisms[i])==false){
var setPrim = new PrismCollision{ a = prisms[i] , b = x};
Debug.DrawLine(prisms[i].transform.position, x.transform.position, Color.green);
yield return setPrim;
}
}
yield break;
}
private IEnumerable <PrismCollision> PotentialCollisions()
{
foreach (var prism in prisms)
{
Debug.Log("Checking prism #" + prism.name);
var nearestNeighbor = prisms.FindClosest(prism.transform.position); // Find closest object to given position
if (nearestNeighbor != prism)
{
var checkPrisms = new PrismCollision
{
a = prism,
b = nearestNeighbor
};
yield return(checkPrisms);
}
}
yield break;
}
private IEnumerable <PrismCollision> fatherRecursion(QuadTree quadFather, Prism obj)
{
if (quadFather.father != null)
{
foreach (PrismCollision check in fatherRecursion(quadFather.father, obj))
{
yield return(check);
}
}
for (int i = 0; i < quadFather.objects.Count; i++)
{
if (quadFather.objects[i].check == false)
{
Debug.Log("50");
var check = new PrismCollision();
check.a = quadFather.objects[i];
check.b = obj;
yield return(check);
}
}
}
private List <PrismCollision> collisionAlongX()
{
var outputListX = new List <PrismCollision>();
var _activeList = new List <Prism>();
_axisPointsX = new List <Tuple <float, char, Prism> >();
for (int i = 0; i < prisms.Count; i++)
{
_axisPointsX.Add(new Tuple <float, char, Prism>(prisms[i].bounds.minV.x, 's', prisms[i]));
_axisPointsX.Add(new Tuple <float, char, Prism>(prisms[i].bounds.maxV.x, 'e', prisms[i]));
//Debug.Log(prisms[i].bounds.minV.x +"--"+ prisms[i].bounds.maxV.x);
}
_axisPointsX.Sort((a, b) => a.Item1.CompareTo(b.Item1));
for (int i = 0; i < _axisPointsX.Count; i++)
{
if (_axisPointsX[i].Item2 == 's')
{
for (int j = 0; j < _activeList.Count; j++)
{
var checkPrisms = new PrismCollision(_axisPointsX[i].Item3, _activeList[j]);
// checkPrisms.a = _axisPointsX[i].Item3;
// checkPrisms.b = _activeList[j];
outputListX.Add(checkPrisms);
}
_activeList.Add(_axisPointsX[i].Item3);
}
else if (_axisPointsX[i].Item2 == 'e')
{
_activeList.Remove(_axisPointsX[i].Item3);
}
}
// Debug.Log(outputListX.Count);
return(outputListX);
}
private bool CheckCollision(PrismCollision collision)
{
#region Great Jolly Kappadia
var prismA = collision.a;
var prismB = collision.b;
//Generate minkowski difference
//List<Vector3> minkowskiDiff = new List<Vector3>();
Vector3 offset = new Vector3(0, 1, 0);
/*
* foreach (Vector3 vecA in prismA.points) {
* foreach (Vector3 vecB in prismB.points) {
* Vector3 nextVec = vecA - vecB;
* minkowskiDiff.Add(nextVec);
* //Debug.DrawLine(nextVec, nextVec + offset, Color.black, 5f);
*
* }
* }
* // */
List <Vector3> simplex = new List <Vector3>();
Vector3 initDir = new Vector3(1, 0, 0);
simplex.Add(MinkowskiSupport(prismA.points, prismB.points, initDir));
initDir = -initDir;
while (true)
{
simplex.Add(MinkowskiSupport(prismA.points, prismB.points, initDir));
//Optimization to make sure the last element added actually passed the origin
//If not the Minkowski sum doesn't contain the origin
if (Vector3.Dot(simplex.Last(), initDir) <= 0)
{
return(false);
}
else
{
var values = containsOrigin(simplex, initDir);
initDir = values.Item2;
if (values.Item1)
{
break;
}
}
}
foreach (Vector3 v in simplex)
{
//Debug.DrawLine(v, v + 2*offset, Color.blue, 5f);
}
//Debug.DrawLine(Vector3.zero, Vector3.zero + 2*offset, Color.white, 5f);
#endregion
#region Environmental Protection Agency
List <Edge> edges = new List <Edge> ();
for (int x = 0; x < simplex.Count; x++)
{
for (int y = x + 1; y < simplex.Count; y++)
{
Edge e = new Edge(simplex[x], simplex[y]);
if (!edges.Contains(e))
{
edges.Add(e);
}
}
}
//Doesn't work currently, so commented out
//This is suppoped to be part of the EPA algorithm
float improvement = 10, best = -1, bestdist = -1;
int curpos;
while (improvement > 0.5)
{
curpos = -1;
bestdist = -1;
for (int pos = 0; pos < edges.Count; pos++)
{
float result = edges[pos].originDistance();
if (bestdist < 0 || result < bestdist)
{
bestdist = result;
curpos = pos;
}
}
best = bestdist;
Vector3 newPoint = MinkowskiSupport(prismA.points, prismB.points, edges[curpos].normal());
simplex.Add(newPoint);
edges.Add(new Edge(edges[curpos].v1, newPoint));
edges.Add(new Edge(newPoint, edges[curpos].v2));
edges.Remove(edges[curpos]);
float newOD1 = edges[edges.Count - 2].originDistance();
float newOD2 = edges[edges.Count - 1].originDistance();
bestdist = min(newOD1, newOD2);
improvement = best - bestdist;
}
curpos = -1;
bestdist = -1;
for (int pos = 0; pos < edges.Count; pos++)
{
float result = edges[pos].originDistance();
if (bestdist < 0 || result < bestdist)
{
bestdist = result;
curpos = pos;
}
}
collision.penetrationDepthVectorAB = edges[curpos].normal() * (bestdist + 0.05f);
#endregion
return(true);
}
private IEnumerable <PrismCollision> PotentialCollisions()
{
/*
* for (int i = 0; i < prisms.Count; i++) {
* for (int j = i + 1; j < prisms.Count; j++) {
* var checkPrisms = new PrismCollision();
* checkPrisms.a = prisms[i];
* checkPrisms.b = prisms[j];
*
* yield return checkPrisms;
* }
* }
* // */
foreach (Prism p in prisms)
{
p.setBounds();
}
QuadTree tree = new QuadTree(Quadtree_Depth, new Vector2(0.0f, 0.0f), prismRegionRadiusXZ);
// */
/* Uses a hashset to keep track of collisions
* This way if A and B are overlapping on many quadrants, you'll
* notice but only do the intensive check a single time.
* // */
HashSet <int> collisionKeys = new HashSet <int>();
List <PrismCollision> colList = new List <PrismCollision>();
foreach (Prism p in prisms)
{
List <Prism[]> cols = tree.register(p);
if (cols.Count == 0)
{
//print("0 collisions");
continue;
}
//print(cols.Count);
foreach (Prism[] col in cols)
{
// very simply: order prisms from 0 to n-1, use the key n*bigger + smaller
int bigger = max(col[0].num, col[1].num), smaller = min(col[0].num, col[1].num);
int key = bigger * prismCount + smaller;
//print("Made it here");
// check if this key (unique for every combination of 2 prisms) is NOT already in the set
if (!collisionKeys.Contains(key))
{
var check = new PrismCollision();
check.a = col[0];
check.b = col[1];
//print("Collision between " + col[0].num + " and " + col[1].num);
collisionKeys.Add(key);
colList.Add(check);
}
}
}
// */
return(colList);
}
private bool CheckCollision(PrismCollision collision)
{
var prismA = collision.a;
var prismB = collision.b;
// prismA.check = true;
// prismB.check = true;
//
// Debug.Log("Points in prism A");
// foreach (var VARIABLE in prismA.points3D)
// {
// Debug.Log(VARIABLE);
// }
//
// Debug.Log("Points in prism B");
// foreach (var VARIABLE in prismB.points3D)
// {
// Debug.Log(VARIABLE);
// }
// List<Vector3> minkowski = new List<Vector3>();
// foreach(var p in prismA.points){
// foreach(var p2 in prismB.points){
// minkowski.Add( p - p2);
// }
// }
// draw the minkowski
// for (var i = 0; i < minkowski.Count(); i++)
// {
// Debug.DrawLine(minkowski[i], minkowski[i] + new Vector3(0.0f, 0f, 0.05f), Color.magenta, UPDATE_RATE);
// }
bool colliding = GJK(prismA, prismB);
// remove duplicates from pointList
HashSet <String> p_str = new HashSet <string>();
List <Vector3> toDelete = new List <Vector3>();
foreach (var p in pointList)
{
String s = p.ToString();
if (p_str.Contains(s))
{
toDelete.Add(p);
}
else
{
p_str.Add(s);
}
}
foreach (var p in toDelete)
{
pointList.Remove(p);
}
if (colliding)
{
collision.penetrationDepthVectorAB = EPA(prismA, prismB);
}
else
{
collision.penetrationDepthVectorAB = Vector3.zero;
}
return(colliding);
}
private bool CheckCollision(PrismCollision collision)
{
var prismA = collision.a;
var prismB = collision.b;
// simplex 3 vector3 list
List <Vector3> simplex = new List <Vector3>();
// starting direction
Vector3 direction = new Vector3(-1, 0, 0);
// support function to return furthest point in prism in given direction
Vector3 support(Prism a, Vector3 d)
{
float max = -float.MaxValue;
int iPt = -1;
for (int i = 0; i < a.points.Length; i++)
{
float dot = Vector3.Dot(d, a.points[i]);
if (dot > max)
{
max = dot;
iPt = i;
}
}
return(a.points[iPt]);
}
// function to return furthest point on minkowski difference convex hull given direction
Vector3 mink(Prism a, Prism b, Vector3 d)
{
Vector3 ptA = support(a, d);
Vector3 ptB = support(b, -d);
return(ptA - ptB);
}
// Add the first point to the simplex
simplex.Add(mink(prismA, prismB, direction));
// Reverse the direction so we get the furthest point in opposite direction
direction = -direction;
// Start looping through simplexes with minkowski difference points
while (true)
{
// get new point in the current direction
Vector3 newPt = mink(prismA, prismB, direction);
// detect whether or not point goes past the origin, dot product must be positive
if (Vector3.Dot(newPt, direction) < 0)
{
return(false);
}
// point is past origin so we can add it
simplex.Add(newPt);
// check if simplex contains the origin, update if not
if (contOrigin() == true)
{
//Starts EPA algo
while (true)
{
float distance = float.PositiveInfinity;
Vector3 normal = new Vector3(0, 0, 0);
int index = 0;
nearestEdge(simplex, ref distance, ref normal, ref index);
//get support point in minkDiff
Vector3 supp = mink(prismA, prismB, normal);
float d = Vector3.Dot(supp, normal);
//Check if simplex and minkDiff points converged
if (d - distance <= Mathf.Epsilon)
{
collision.penetrationDepthVectorAB = normal * distance;
return(true);
}
else
{
//update simplex to include minkDiff point
simplex.Insert(index, supp);
}
}
}
}
//Finds edge in simplex that is nearest to origin
void nearestEdge(List <Vector3> simp, ref float closeDist, ref Vector3 closeNorm, ref int closeIndex)
{
for (int i = 0; i < simp.Count; i++)
{
int j = i == simp.Count - 1 ? 0 : i + 1;
Vector3 edge = simp[j] - simp[i];
//origin to first vector (simplex[i])
Vector3 ao = simp[i];
Vector3 norm = Vector3.Cross(Vector3.Cross(edge, ao), edge);
norm = Vector3.Normalize(norm);
float dist = Mathf.Abs(Vector3.Dot(ao, norm));
if (dist < closeDist)
{
closeDist = dist;
closeNorm = norm;
closeIndex = j;
}
}
}
bool contOrigin()
{
// if simplex only has 2 points
if (simplex.Count() == 2)
{
// get the points
Vector3 ptA = simplex[1];
Vector3 ptB = simplex[0];
// get the segments
Vector3 aOrig = new Vector3(0, 0, 0) - ptA;
Vector3 aB = ptB - ptA;
// adjust direction to be perp
direction = new Vector3(-aB[2], 0, aB[0]);
// if dot product of direction and aOrig less than 0, adjust to point at origin
if (Vector3.Dot(direction, aOrig) < 0)
{
direction = -direction;
}
// keep editing simplex
return(false);
}
else if (simplex.Count() == 3)
{
Vector3 ptA = simplex[2];
Vector3 ptB = simplex[1];
Vector3 ptC = simplex[0];
// get the segments
Vector3 aOrig = new Vector3(0, 0, 0) - ptA;
Vector3 aB = ptB - ptA;
Vector3 aC = ptC - ptA;
// adjust direction to be perp to segment away c
direction = new Vector3(-aB[2], 0, aB[0]);
if (Vector3.Dot(direction, ptC) > 0)
{
direction = -direction;
}
// if perp vector from AB pointing toward origin we can remove c
if (Vector3.Dot(direction, aOrig) > 0)
{
simplex.Remove(ptC);
return(false);
}
// readjust direction AC from B
direction = new Vector3(-aC[2], 0, aC[0]);
if (Vector3.Dot(direction, ptB) > 0)
{
direction = -direction;
}
// if perp vector from AC pointing toward origin we can remove b
if (Vector3.Dot(direction, aOrig) > 0)
{
simplex.Remove(ptB);
return(false);
}
// otherwise the origin is contained inside of the trianglge
return(true);
}
else
{
return(false);
Debug.Log("Simplex size not equal to 2 or 3");
}
}
collision.penetrationDepthVectorAB = Vector3.zero;
return(true);
}
private bool CheckCollision(PrismCollision collision)
{
var prismA = collision.a;
var prismB = collision.b;
Vector3 a, b, c, ab, ac, ao, abPerp, acPerp;
bool ans = false;
List <Vector3> simplex = new List <Vector3>();
Vector3 direction = new Vector3(1, 1, 1);
simplex.Add(getSupportVector(collision, direction));
direction = -direction;
while (true)
{
simplex.Add(getSupportVector(collision, direction));
if (Vector3.Dot(simplex.Last(), direction) < 0)
{
//print(Vector3.Dot(simplex.Last(), direction));
ans = false;
break;
}
else
{
a = simplex.Last();
ao = -a;
if (simplex.Count == 3)
{
b = simplex[1];
c = simplex[0];
ab = b - a;
ac = c - a;
//AxBxC = B(C.dot(A)) – A(C.dot(B))
//tripleProduct(ac, ab, ab);
//tripleProduct(ab, ac, ac)
abPerp = ab * Vector3.Dot(ab, ac) - ac * Vector3.Dot(ab, ab);
acPerp = ac * Vector3.Dot(ac, ab) - ab * Vector3.Dot(ac, ac);
if (Vector3.Dot(abPerp, ao) > 0)
{
simplex.Remove(c);
direction = abPerp;
}
else
{
if (Vector3.Dot(acPerp, ao) > 0)
{
simplex.Remove(b);
direction = acPerp;
}
else
{
ans = true;
break;
}
}
}
else
{
b = simplex[1];
ab = b - a;
//abPerp = tripleProduct(ab, ao, ab);
abPerp = ao * Vector3.Dot(ab, ab) - ab * Vector3.Dot(ab, ao);
direction = abPerp;
}
}
}
Vector3 normal = Vector3.zero;
float depth = 0f;
float closestDist;
Vector3 closestNormal;
int closestIndex;
while (true)
{
closestDist = 1000000000000f;
closestNormal = Vector3.zero;
closestIndex = -1;
for (int i = 0; i < simplex.Count; i++)
{
int j = i + 1 == simplex.Count ? 0 : i + 1;
a = simplex[i];
b = simplex[j];
c = b - a;
ab = a * Vector3.Dot(c, c) - c * Vector3.Dot(c, a);
/*print(a);
* print(b);
* print(c);
* print(ab);*/
Vector3.Normalize(ab);
float d = Vector3.Dot(ab, a);
if (d < closestDist)
{
closestDist = d;
closestNormal = ab;
closestIndex = j;
}
}
//print(simplex.Count);
Vector3 p = getSupportVector(collision, closestNormal);
float ds = Vector3.Dot(p, closestNormal);
if (ds - closestDist < 0.5)
{
normal = closestNormal;
depth = ds;
break;
}
else
{
simplex.Insert(closestIndex, p);
}
}
print(depth);
collision.penetrationDepthVectorAB = normal * 2 * (1 / 1 - depth);
return(ans);
}
private bool CheckCollision(PrismCollision collision)
{
var prismA = collision.a;
var prismB = collision.b;
List <Vector3> points;
bool check = false;
//Minkowski Difference Computation
points = new List <Vector3>();
points.Clear();
foreach (var point1 in prismA.points)
{
foreach (var point2 in prismB.points)
{
var result = Vector3.zero;
result = point1 - point2;
points.Add(result);
}
}
//gjk
bool[] col = new bool[4];
for (int i = 1; i < col.Length; i++)
{
col[i] = false;
}
for (int i = 0; i < points.Count; i++)
{
if (points[i].x > 0 && points[i].z > 0)
{
col[0] = true;
}
if (points[i].x > 0 && points[i].z < 0)
{
col[1] = true;
}
if (points[i].x < 0 && points[i].z < 0)
{
col[2] = true;
}
if (points[i].x < 0 && points[i].z > 0)
{
col[3] = true;
}
if (points[i].x > 0 && points[i].z == 0)
{
col[0] = true;
col[1] = true;
}
if (points[i].x < 0 && points[i].z == 0)
{
col[2] = true;
col[3] = true;
}
if (points[i].x == 0 && points[i].z > 0)
{
col[0] = true;
col[3] = true;
}
if (points[i].x == 0 && points[i].z < 0)
{
col[1] = true;
col[2] = true;
}
}
int count = 0;
for (int i = 0; i < col.Length; i++)
{
if (col[i] == true)
{
count++;
}
}
if (count == 4)
{
check = true;
}
else
{
int far = 0;
for (int i = 0; i < points.Count - 2; i++)
{
if (points[far].sqrMagnitude < points[1].sqrMagnitude)
{
far = i;
}
}
for (int i = 0; i < points.Count - 1; i++)
{
if (check)
{
break;
}
for (int j = i + 1; j < points.Count; j++)
{
if (i != far && j != far)
{
if (whichSide(points[far], points[i], points[j]) == whichSide(points[far], points[i], new Vector3(0, 0, 0)) &&
whichSide(points[i], points[j], points[far]) == whichSide(points[i], points[j], new Vector3(0, 0, 0)) &&
whichSide(points[j], points[far], points[i]) == whichSide(points[j], points[far], new Vector3(0, 0, 0)))
{
check = true;
break;
}
}
}
}
}
if (!check)
{
return(false);
}
//epa
Vector3 speedV = new Vector3(0, 0, 0);
for (int i = 0; i < points.Count; i++)
{
for (int j = 0; j < points.Count; j++)
{
if (i != j)
{
bool edge = true;
for (int k = 0; k < points.Count; k++)
{
if (!(k == i || k == j))
{
if (whichSide(points[i], points[j], points[k]) != whichSide(points[i], points[j], new Vector3(0, 0, 0)))
{
edge = false;
}
}
}
if (edge && (((perpendicular(points[i], points[j]).sqrMagnitude < speedV.sqrMagnitude) || (speedV.x == 0 && speedV.y == 0 && speedV.z == 0))))
{
speedV = perpendicular(points[i], points[j]);
}
}
}
}
collision.penetrationDepthVectorAB = new Vector3(speed * speedV.x, 0, speed * speedV.z);
return(true);
}
private bool CheckCollision(PrismCollision collision){
var prismA = collision.a;
var prismB = collision.b;
List<Vector2> Simplex = new List<Vector2>();
int sizeOfList = Simplex.Count;
collision.penetrationDepthVectorAB = Vector3.zero;
return true;
Vector2 getFarthestPointInDirection(Vector2 d) {
int index = 0;
double maxDot = (int)Vector2.Dot(Simplex[index], d);
if (Simplex != null)
{
for (int i = 1; i < sizeOfList; i++)
{
int dot = (int)Vector2.Dot(Simplex[i], d);
if (dot > maxDot)
{
maxDot = dot;
index = i;
}
}
}
return Simplex[index];
}
Vector2 support(Prism a, Prism b, Vector2 d){
// get points on the edge of the shapes in opposite directions
Vector2 p1 = a.getFarthestPointInDirection(d);
Vector2 p2 = b.getFarthestPointInDirection(new Vector2(-d.x, -d.y));
// Minkowski Difference
Vector2 p3 = new Vector2((p1 - p2).x, (p1 - p2).y);
// p3 is now a point in Minkowski space on the edge of the Minkowski Difference
return p3;
}
bool containsOrigin(ref Vector2 d){
Vector2 a = Simplex.Last();
Vector2 ao = new Vector2(-a.x, -a.y);
if (Simplex.Count() == 3){
support(prismA, prismB, d);
if((a.x == 0) && (a.y == 0) {
return true;
}
}
return false;
}
bool IsCollide(Prism a, Prism b) {
Vector2 last = Simplex[sizeOfList - 1];
Vector2 dark = new Vector2(1, -1);
Simplex.Add(dark);
// negate d for the next point
Negate(dark);
// start looping
while (true)
{
// add a new point to the simplex because we haven't terminated yet
Simplex.Add(support(a, b, dark));
// make sure that the last point we added actually passed the origin
int dot6 = (int)Vector2.Dot(last, dark);
if (dot6 <= 0)
{
// if the point added last was not past the origin in the direction of d
// then the Minkowski Sum cannot possibly contain the origin since
// the last point added is on the edge of the Minkowski Difference
return false;
}
else
{
if (containsOrigin(ref dark))
{
// if it does then we know there is a collision
return true;
}
}
}
}
}
