public Vector2 calculateCollisions(Vector2 velocity, Vector2 finalPos, Player player)
{
Vector2[] bBox = broadPhase(player.Position, finalPos, new Vector2(player.Height, player.Width));
Vector2[] finalPosVerts = new Vector2[4];
for ()
{
CollisionPoly playerPoly = new CollisionPoly();
}
int currentTile;
for (int i = (int)Math.Floor(bBox[0].X); i < (int)Math.Ceiling(bBox[1].X); i++)
{
for (int j = (int)Math.Floor(bBox[0].Y); j < (int)Math.Ceiling(bBox[1].Y); j++)
{
currentTile = collisionArray[i, j];
if (currentTile == 0)
{
return(velocity);
}
else if (currentTile == 1)
{
if (isCollision(playerPoly, new CollisionPoly(new Vector2[] { new Vector2(i * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, j * tileSize), new Vector2(i * tileSize, (j + 1) * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize) })))
{
}
}
}
}
}
public static Boolean isCollision(CollisionPoly poly1, CollisionPoly poly2)
{
Vector2 direction = new Vector2();
List <Vector2> simplex = new List <Vector2>();
while (true)
{
switch (simplex.Count)
{
case 0:
//direction in the direction of separation
direction = Vector2.Subtract(poly1.center(), poly2.center());
break;
case 1:
//direction opposite direction of separation
direction = Vector2.Multiply(direction, -1);
break;
case 2:
//direction orthogonal to the line segment generated by first two points and also
direction = tripleCrossProd(Vector2.Subtract(simplex[1], simplex[0]), Vector2.Multiply(simplex[0], -1), Vector2.Subtract(simplex[1], simplex[0]));
break;
case 3:
Vector2 side1 = Vector2.Subtract(simplex[1], simplex[2]);
Vector2 side2 = Vector2.Subtract(simplex[0], simplex[2]);
//vector orthogonal to the 2nd line segment generated (vertex 2 -> vertex 1)
Vector2 orthogonal1 = tripleCrossProd(side2, side1, side1);
//vector orthogonal to third line segment generated (vertex 3 -> vertex 1)
Vector2 orthogonal2 = tripleCrossProd(side1, side2, side2);
if (Vector2.Dot(orthogonal1, Vector2.Multiply(simplex[2], -1)) > 0)
{
direction = orthogonal1;
simplex.RemoveAt(0);
}
else if (Vector2.Dot(orthogonal2, Vector2.Multiply(simplex[2], -1)) > 0)
{
direction = orthogonal2;
simplex.RemoveAt(1);
}
else
{
return(true);
}
break;
}
Vector2 nextVert = Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1)));
if (Vector2.Dot(direction, nextVert) > 0)
{
simplex.Add(nextVert);
}
else
{
return(false);
}
}
}
public float[,] calculateTimes(int initialX, int initialY, int finalX, int finalY, CollisionPoly initialPos, Vector2 velocity, int unitConversion)
{
int currentTile;
float[,] collisionTimes = new float[finalY - initialY, finalX - initialX];
CollisionPoly tilePoly;
for (int i = initialY; i < finalY; i++)
{
for (int j = initialX; j < finalX; j++)
{
//System.Diagnostics.Debug.WriteLine(i + " , " + j);
currentTile = collisionArray[i, j];
switch (currentTile)
{
case 0:
tilePoly = null;
break;
case 1:
tilePoly = new CollisionPoly(new Vector2[] { new Vector2(j * tileSize, i * tileSize), new Vector2((j + 1) * tileSize, i * tileSize), new Vector2((j + 1) * tileSize, (i + 1) * tileSize), new Vector2(j * tileSize, (i + 1) * tileSize) });
break;
case 2:
tilePoly = new CollisionPoly(new Vector2[] { new Vector2((j + 1) * tileSize, i * tileSize), new Vector2((j + 1) * tileSize, (i + 1) * tileSize), new Vector2(j * tileSize, (i + 1) * tileSize) });
break;
case 3:
tilePoly = new CollisionPoly(new Vector2[] { new Vector2(j * tileSize, i * tileSize), new Vector2((j + 1) * tileSize, (i + 1) * tileSize), new Vector2(j * tileSize, (i + 1) * tileSize) });
break;
default:
tilePoly = new CollisionPoly(new Vector2[4]);
break;
}
if (tilePoly != null)
{
collisionTimes[i - initialY, j - initialX] = calculateIntersection(initialPos, tilePoly, velocity).Length() / (velocity.Length() * unitConversion);
}
else
{
collisionTimes[i - initialY, j - initialX] = float.MaxValue;
}
}
}
return(collisionTimes);
}
public static Vector2 calculateIntersection(CollisionPoly poly1, CollisionPoly poly2, Vector2?velocity)
{
//Constructs basic simplex
Vector2 direction = Vector2.Subtract(poly1.center(), poly2.center());
List <Vector2> simplex = new List <Vector2>();
simplex.Add(Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1))));
direction = Vector2.Multiply(direction, -1);
simplex.Add(Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1))));
direction = tripleCrossProd(Vector2.Subtract(simplex[1], simplex[0]), Vector2.Multiply(simplex[0], -1), Vector2.Subtract(simplex[1], simplex[0]));
simplex.Add(Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1))));
//Calculates distance inside/outside object
if (velocity.HasValue)
{
while (true)
{
Edge closestEdge = closestEdgeAlongVector(simplex, (Vector2)velocity);
Vector2 support = Vector2.Subtract(poly1.supportFunction(closestEdge.normal), poly2.supportFunction(Vector2.Negate(closestEdge.normal)));
if (Math.Abs(Vector2.Dot(support, closestEdge.normal) - closestEdge.dist) <= 0.0001)
{
return(closestEdge.toOrigin);
}
else
{
simplex.Insert(closestEdge.index, support);
}
}
}
else
{
while (true)
{
Edge closestEdge = closestEdgeToOrigin(simplex);
Vector2 support = Vector2.Subtract(poly1.supportFunction(closestEdge.normal), poly2.supportFunction(Vector2.Negate(closestEdge.normal)));
if (Math.Abs(Vector2.Dot(support, closestEdge.normal) - closestEdge.dist) <= 0.00001)
{
return(Vector2.Multiply(closestEdge.normal, Vector2.Dot(support, closestEdge.normal)));
}
else
{
simplex.Insert(closestEdge.index, support);
}
}
}
}
public Vector2 calculateCollisions(Vector2 velocity, Vector2 initialPos, Player player, float elapsedTime, int unitConversion)
{
while (true)
{
Vector2[] bBox = broadPhase(initialPos, player.Position, new Vector2(player.Width, player.Height));
CollisionPoly tilePoly = new CollisionPoly(new Vector2[4]);
CollisionPoly playerPoly = new CollisionPoly(player.Verts);
CollisionPoly initialPosVerts = new CollisionPoly(new Vector2[] {
initialPos,
new Vector2(initialPos.X + player.Width, initialPos.Y),
new Vector2(initialPos.X + player.Width, initialPos.Y + player.Height),
new Vector2(initialPos.X, initialPos.Y + player.Height)
});
float[,] collisionTimes = calculateTimes((int)Math.Floor(bBox[0].X), (int)Math.Floor(bBox[0].Y), (int)Math.Ceiling(bBox[1].X), (int)Math.Ceiling(bBox[1].Y), initialPosVerts, velocity, unitConversion);
//System.Diagnostics.Debug.WriteLine("(" + bBox[0].X + "," + bBox[0].Y + ") ; (" + bBox[1].X + "," + bBox[1].Y + ")");
//calculates index/indicies with the lowest collisionTime
List <int[]> lowestIndices = new List <int[]>();
float currentLowest = float.MaxValue;
for (int i = 0; i < collisionTimes.GetLength(0); i++)
{
for (int j = 0; j < collisionTimes.GetLength(1); j++)
{
if (collisionTimes[i, j] < currentLowest)
{
currentLowest = collisionTimes[i, j];
}
}
}
for (int i = 0; i < collisionTimes.GetLength(0); i++)
{
for (int j = 0; j < collisionTimes.GetLength(1); j++)
{
//fake epsilon lol
if (Math.Abs(collisionTimes[i, j] - currentLowest) < 0.00000000001)
{
lowestIndices.Add(new int[] { i + (int)Math.Floor(bBox[0].Y), j + (int)Math.Floor(bBox[0].X) });
}
}
}
//System.Diagnostics.Debug.WriteLine(currentLowest);
if (currentLowest < (elapsedTime / 1000))
{
int tile;
//System.Diagnostics.Debug.Write(currentLowest);
Vector2 collisionPos = Vector2.Subtract(initialPos, Vector2.Multiply(velocity, currentLowest));
//System.Diagnostics.Debug.WriteLine(collisionPos);
foreach (int[] index in lowestIndices)
{
int i = index[0];
int j = index[1];
tile = collisionArray[i, j];
System.Diagnostics.Debug.WriteLine(tile);
switch (tile)
{
case 1:
tilePoly = new CollisionPoly(new Vector2[] { new Vector2(i * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, (j) * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize), new Vector2((i) * tileSize, (j + 1) * tileSize) });
break;
case 2:
tilePoly = new CollisionPoly(new Vector2[] { new Vector2((i + 1) * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize), new Vector2(i * tileSize, (j + 1) * tileSize) });
break;
case 3:
tilePoly = new CollisionPoly(new Vector2[] { new Vector2(i * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize), new Vector2(i * tileSize, (j + 1) * tileSize) });
break;
default:
tilePoly = new CollisionPoly(new Vector2[4]);
break;
}
playerPoly = new CollisionPoly(player.Verts);
//System.Diagnostics.Debug.WriteLine(player.Position);
System.Diagnostics.Debug.Write(calculateIntersection(playerPoly, tilePoly, null));
System.Diagnostics.Debug.Write("calculating tile " + i + ", " + j + ")");
player.Position = Vector2.Subtract(player.Position, calculateIntersection(playerPoly, tilePoly, null));
//System.Diagnostics.Debug.WriteLine(player.Position);
velocity = Vector2.Divide(Vector2.Subtract(player.Position, collisionPos), elapsedTime / (1000) * unitConversion);
//System.Diagnostics.Debug.WriteLine(velocity);
//player.Position = Vector2.Add(player.Position, unitConversion * velocity * (elapsedTime - currentLowest) / (1000));
//System.Diagnostics.Debug.Write(player.Position);
initialPos = player.Position;
}
//System.Diagnostics.Debug.Write(isCollision(playerPoly, tilePoly));
}
else
{
//System.Diagnostics.Debug.WriteLine(Vector2.Divide(Vector2.Subtract(player.Position, initialPos), elapsedTime / (1000)).X + "," + Vector2.Divide(Vector2.Subtract(player.Position, initialPos), elapsedTime / (1000)).Y);
return(velocity);
}
}
/*
* //int i = (int)Math.Floor(bBox[0].X);
* //int j = (int)Math.Floor(bBox[0].Y);
* Vector2[] bBox = broadPhase(initialPos, player.Position, new Vector2(player.Width, player.Height));
* Vector2 finalPos = Vector2.Add(initialPos,velocity);
* Vector2[] finalPosVerts = Enumerable.ToArray<Vector2>(from vert in player.Verts select Vector2.Add(vert, velocity));
* CollisionPoly playerPoly = new CollisionPoly(finalPosVerts);
* int currentTile;
* int i = (int)Math.Floor(bBox[0].X);
* int j = (int)Math.Floor(bBox[0].Y);
*
* while (i < (int)Math.Ceiling(bBox[1].X))
* {
*
* while (j < (int)Math.Ceiling(bBox[1].Y))
* {
* currentTile = collisionArray[j, i];
*
* switch (currentTile)
* {
* case 0:
* break;
* case 1:
* CollisionPoly collisionBox = new CollisionPoly(new Vector2[] { new Vector2(i * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize), new Vector2(i * tileSize, (j + 1) * tileSize) });
* if (isCollision(playerPoly, collisionBox))
* {
* float collisionTime = calculateIntersection(playerPoly, collisionBox, velocity).Length() / velocity.Length();
* //player.Position = Vector2.Add(player.Position, Vector2.Multiply(velocity, collisionTime));
*
* System.Diagnostics.Debug.Write(calculateIntersection(playerPoly, collisionBox, null));
* System.Diagnostics.Debug.Write("calculating tile " + i + ", " + j + ")");
* finalPos = Vector2.Subtract(finalPos, calculateIntersection(playerPoly, collisionBox, null));
* finalPosVerts = Enumerable.ToArray<Vector2>(from vert in finalPosVerts select Vector2.Subtract(finalPos, calculateIntersection(playerPoly, collisionBox, null)));
* playerPoly.vert = finalPosVerts;
* velocity = Vector2.Subtract(finalPos, player.Position);
* bBox = broadPhase(player.Position, finalPos, new Vector2(player.Height, player.Width));
* playerPoly = new CollisionPoly(player.Verts);
* i = (int)Math.Floor(bBox[0].X);
* j = (int)Math.Floor(bBox[0].Y);
*
* }
* break;
* case 2:
* CollisionPoly collisionTriangleL = new CollisionPoly(new Vector2[] { new Vector2((i + 1) * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize), new Vector2(i * tileSize, (j + 1) * tileSize) });
* if (isCollision(playerPoly, collisionTriangleL))
* {
* float collisionTime = calculateIntersection(playerPoly, collisionTriangleL, velocity).Length() / velocity.Length();
* player.Position = Vector2.Subtract(player.Position, Vector2.Multiply(velocity, collisionTime));
*
* //System.Diagnostics.Debug.Write(calculateIntersection(playerPoly, collisionBox, null));
* finalPos = Vector2.Subtract(finalPos, calculateIntersection(playerPoly, collisionTriangleL, null));
* finalPosVerts = Enumerable.ToArray<Vector2>(from vert in finalPosVerts select Vector2.Subtract(finalPos, calculateIntersection(playerPoly, collisionTriangleL, null)));
* playerPoly.vert = finalPosVerts;
* velocity = Vector2.Subtract(finalPos, player.Position);
* bBox = broadPhase(player.Position, finalPos, new Vector2(player.Height, player.Width));
* playerPoly = new CollisionPoly(player.Verts);
* i = (int)Math.Floor(bBox[0].X);
* j = (int)Math.Floor(bBox[0].Y);
* }
* break;
* case 3:
* CollisionPoly collisionTriangleR = new CollisionPoly(new Vector2[] { new Vector2(i * tileSize, j * tileSize), new Vector2((i + 1) * tileSize, (j + 1) * tileSize), new Vector2(i * tileSize, (j + 1) * tileSize) });
* if (isCollision(playerPoly, collisionTriangleR))
* {
* float collisionTime = calculateIntersection(playerPoly, collisionTriangleR, velocity).Length() / velocity.Length();
* player.Position = Vector2.Subtract(player.Position, Vector2.Multiply(velocity, collisionTime));
*
* //System.Diagnostics.Debug.Write(calculateIntersection(playerPoly, collisionBox, null));
* finalPos = Vector2.Subtract(finalPos, calculateIntersection(playerPoly, collisionTriangleR, null));
* finalPosVerts = Enumerable.ToArray<Vector2>(from vert in finalPosVerts select Vector2.Subtract(finalPos, calculateIntersection(playerPoly, collisionTriangleR, null)));
* playerPoly.vert = finalPosVerts;
* velocity = Vector2.Subtract(finalPos, player.Position);
* bBox = broadPhase(player.Position, finalPos, new Vector2(player.Height, player.Width));
* playerPoly = new CollisionPoly(player.Verts);
* i = (int)Math.Floor(bBox[0].X);
* j = (int)Math.Floor(bBox[0].Y);
* }
* break;
* }
* j++;
* }
* i++;
* j = (int)Math.Floor(bBox[0].Y);
* }
* player.Position = finalPos;
* return velocity;
*/
}