В настоящее время я занимаюсь разработкой 2D физического движка с нуля. И все работало хорошо на сегодняшний день. Это объектно-ориентированный, очевидно, с векторами, матрицами и классами форм, написанными мной с нуля. Столкновение еще не организовано, но перед этим мне нужно убедиться, что оно работает.
Поэтому для образовательных целей я решил реализовать алгоритм GJK для обнаружения столкновений и EPA для расчета нормального контакта. Часть GJK работает отлично. А из симплекса (Треугольник, поскольку он 2D) я вычисляю ближайший край, и оттуда я следовал алгоритму. Но по какой-то причине, которую я не могу найти, только в некоторых случаях, программа вводит бесконечное число l oop, добавляя новые вершины в список вершин, но когда я их печатаю, они все одинаковые, и разница, которая должна быть около 0 в EPA, дает большие результаты. такие как 70,55. Надеюсь, вы сможете помочь. Заранее спасибо.
public class Collision
{
private Shape shape1;
private Shape shape2;
public List<Vector2> vertices;
private Vector2 direction;
private double TOLERENCE = 2;
public Vector2 contactNormal;
public double penetrationDepth;
private enum State
{
NO_INTERSECTION, FOUND_INTERSECTION, STILL_EVOLVING
}
private State result = State.STILL_EVOLVING;
public Collision(Shape shape1, Shape shape2)
{
this.shape1 = shape1;
this.shape2 = shape2;
vertices = new List<Vector2>();
}
public bool isColliding()
{
while (result == State.STILL_EVOLVING)
{
result = evolveSimplex();
}
if (result == State.FOUND_INTERSECTION)
findContactDetails();
return result == State.FOUND_INTERSECTION;
}
private State evolveSimplex()
{
if (vertices.Count == 0)
{
direction = shape1.position - shape2.position;
}
else if (vertices.Count == 1)
{
direction = -direction;
}
else if (vertices.Count == 2)
{
//ab is the vector from the 1st vertex to the second one
Vector2 ab = vertices[1] - vertices[0];
//a0 is the vector from the 1st vertex to the origin
Vector2 a0 = -vertices[0];
//Get the direction that is perpendicular to ab in the direction of the origin.
direction = ab.getPerpendicularUnitVectorTowards(a0);
}
else if (vertices.Count == 3)
{
//a,b and c are the three vertices
Vector2 a = vertices[0];
Vector2 b = vertices[1];
Vector2 c = vertices[2];
//lets find cb, ca and c0
Vector2 cb = b - c;
Vector2 ca = a - c;
Vector2 c0 = -c;
// let's find the perpendicular directions to cb and ca that are towards outside the simplex.
Vector2 cbPerp = cb.getPerpendicularUnitVectorTowards(c0);
Vector2 caPerp = ca.getPerpendicularUnitVectorTowards(c0);
if (cbPerp * c0 < 0)
{
vertices.Remove(a);
direction = cbPerp;
}
else if (caPerp * c0 < 0)
{
vertices.Remove(b);
direction = caPerp;
}
else
{
return State.FOUND_INTERSECTION;
}
}
return addSupport(direction) ? State.STILL_EVOLVING : State.NO_INTERSECTION;
}
private bool addSupport(Vector2 direction)
{
Vector2 support = shape1.getSupportPoint(direction) - shape2.getSupportPoint(-direction);
vertices.Add(support);
return direction * support > 0;
}
private void findContactDetails()
{
while (true)
{
Edge closestEdge = findClosestEdge(vertices);
Vector2 support = shape1.getSupportPoint(closestEdge.normal) - shape2.getSupportPoint(-closestEdge.normal);
double distance = support * closestEdge.normal;
Console.WriteLine("difference between the two = " + (Math.Abs(distance - closestEdge.distance)) + " and it's " + (Math.Abs(distance - closestEdge.distance) < TOLERENCE));
if (Math.Abs(distance - closestEdge.distance) < TOLERENCE)
{
Console.WriteLine("Contact details found");
contactNormal = closestEdge.normal;
penetrationDepth = distance;
break;
}
Console.WriteLine("That was not the closest edge. adding the support to the vertices at index " + closestEdge.index);
vertices.Insert(closestEdge.index, support);
Console.WriteLine("The number of vertices now = " + vertices.Count);
for (int i = 0; i < vertices.Count; i++)
{
Form1.dots.Add(vertices[i]);
}
}
}
private Edge findClosestEdge(List<Vector2> simplex)
{
Console.WriteLine("Finding closest edge");
Edge closest = new Edge();
closest.distance = Double.PositiveInfinity;
for (int i = 0; i < simplex.Count; i++)
{
Console.WriteLine("i = " + i);
int j = i + 1 == simplex.Count ? 0 : i + 1;
Vector2 a = simplex[i];
Vector2 b = simplex[j];
Vector2 edge = b - a;
Vector2 oa = a;
Vector2 normal = edge.getPerpendicularUnitVectorTowards(oa);
double d = Math.Abs(normal * oa);
Console.WriteLine("distance from origin to edge = " + d);
if (d < closest.distance)
{
closest.distance = d;
closest.normal = normal;
closest.index = j;
}
}
Console.WriteLine("returing closest edge " + closest.index + " distance = " + closest.distance);
return closest;
}
}
И вот класс vector2.
public class Vector2
{
public double x;
public double y;
public static Vector2 zeroVector2 = new Vector2(0,0);
public Vector2(double x, double y)
{
this.x = x;
this.y = y;
}
public Vector2 add(Vector2 vector)
{
return new Vector2(x + vector.x, y + vector.y);
}
public Vector2 subtract(Vector2 vector)
{
return new Vector2(x - vector.x, y - vector.y);
}
public double dot(Vector2 vector)
{
return x * vector.x + y * vector.y;
}
public Vector2 multiply(double scalar)
{
return new Vector2(x*scalar, y*scalar);
}
public double getMagnitude()
{
return Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
}
public void negate()
{
x = -x;
y = -y;
}
public Vector2 negated()
{
return new Vector2(-x, -y);
}
public bool isZeroVector()
{
if (x == 0 && y == 0)
return true;
else return false;
}
public Vector2 normalized()
{
return this * (1 / getMagnitude());
}
public void normalize()
{
x *= (1 / getMagnitude());
y *= (1 / getMagnitude());
}
public static Vector2 operator -(Vector2 vector)
{
return vector.negated();
}
public static Vector2 operator +(Vector2 v1, Vector2 v2)
{
return v1.add(v2);
}
public static Vector2 operator -(Vector2 v1, Vector2 v2)
{
return v1.subtract(v2);
}
public static double operator *(Vector2 v1, Vector2 v2)
{
return v1.dot(v2);
}
public static Vector2 operator *(Vector2 v, double scalar)
{
return v.multiply(scalar);
}
public static Vector2 operator *(double scalar, Vector2 v)
{
return v.multiply(scalar);
}
public static bool operator ==(Vector2 v1, Vector2 v2)
{
if (v1.x == v2.x && v1.y == v2.y)
return true;
return false;
}
public static bool operator !=(Vector2 v1, Vector2 v2)
{
if (v1.x == v2.x && v1.y == v2.y)
return false;
return true;
}
public override string ToString()
{
return "x = " + x + " , y = " + y;
}
public Vector3 to3DVector()
{
return new Vector3(x, y, 0);
}
public Matrix toMatrix()
{
return new Matrix(new double[2,1]{{x}, {y}});
}
public Vector2 rotate(double degrees)
{
return (Matrix.getRotationMatrix(degrees) * this.toMatrix()).toVector2();
}
public static Vector2 toVector2(Point point)
{
return new Vector2(point.X, point.Y);
}
public Point toPoint()
{
return new Point((int)x,(int)y);
}
public Vector2 getPerpendicularUnitVectorTowards(Vector2 direction)
{
Vector2 perp = new Vector2(-y, x) * (1 / getMagnitude());
Console.WriteLine(perp);
if (perp * direction < 0)
perp = -perp;
return perp;
}
}