public static MinkowskiPoint Support(ref SupportContext ctx, cpVect n)
{
SupportPoint a = ctx.func1(ctx.shape1, cpVect.cpvneg(n));
SupportPoint b = ctx.func2(ctx.shape2, n);
return(MinkowskiPointNew(a, b));
}
private void AddNewTrianglesFromLines(List <Line> lines, SupportPoint newp)
{
foreach (var l in lines)
{
poly.Add(new Triangle(newp, l.a, l.b, true));
}
}
public bool Behind(SupportPoint p)
{
float point = Vector3.Dot(p.point, normal);
float tri = Vector3.Dot(a.point, normal);
return(point - CompareError > tri);
}
public static MinkowskiPoint MinkowskiPointNew(SupportPoint a, SupportPoint b)
{
var point = new MinkowskiPoint(a.p, b.p,
cpVect.cpvsub(b.p, a.p),
((a.id & 0xFF) << 8 | (b.id & 0xFF)));
return(point);
}
private List <Triangle> GetBehinds(SupportPoint p)
{
List <Triangle> tris = new List <Triangle>();
tris.AddRange(poly.Where((arg) => arg.Behind(p)));
return(tris);
}
public float GetDepth(out Vector3 normal, out Vector3 position)
{
int iterations = 0;
while (true)
{
iterations++;
if (iterations > GJK.iterationCap)
{
shouldFail = true;
}
if (shouldFail)
{
normal = new Vector3(float.NaN);
position = new Vector3(float.NaN);
return(-1);
}
poly.Sort((x, y) => (x.depth).CompareTo(y.depth));
Triangle triangle = poly[0];
SupportPoint newp = Support(triangle.normal);
if (triangle.Behind(newp))
{
CreateAndFillHole(newp);
}
else
{
//done
position = triangle.normal * triangle.depth;
float a = (position - triangle.a.point).Length();
float b = (position - triangle.b.point).Length();
float c = (position - triangle.c.point).Length();
float total = a + b + c;
a /= total;
b /= total;
c /= total;
position = (a * triangle.a.AverageSupport) + (b * triangle.b.AverageSupport) + (c * triangle.c.AverageSupport);
normal = -triangle.normal;
Vector3 AtoB = at.position - bt.position;
if (Vector3.Dot(normal, AtoB) < 0)
{
normal *= -1;
}
return(triangle.depth);
}
}
}
public Triangle(SupportPoint a, SupportPoint b, SupportPoint c, bool fixNormal) : this()
{
this.a = a;
this.b = b;
this.c = c;
this.normal = Vector3.Normalize(Vector3.Cross(a.point - b.point, a.point - c.point));
this.depth = DepthProper();//This is to figure out if the normal is pointing the right way
if (fixNormal && depth < 0)
{
normal *= -1;
this.depth = DepthProper();
}
}
public static void Support(ref float3 search, ref SupportData aData, ref SupportData bData, ref int uniqueId, out SupportPoint assignment)
{
float3 tempA = float3.zero, tempB = float3.zero;
float3 negativeSearch = -search;
switch (aData.collider.type)
{
case ColliderType.Box:
boxSupport(ref negativeSearch, ref aData, out tempA);
break;
case ColliderType.Sphere:
sphereSupport(ref negativeSearch, ref aData, out tempA);
break;
case ColliderType.Capsule:
capsuleSupport(ref negativeSearch, ref aData, out tempA);
break;
case ColliderType.Plane:
//boxSupport(ref search, ref aData, ref tempA);
break;
}
switch (bData.collider.type)
{
case ColliderType.Box:
boxSupport(ref search, ref bData, out tempB);
break;
case ColliderType.Sphere:
sphereSupport(ref search, ref bData, out tempB);
break;
case ColliderType.Capsule:
capsuleSupport(ref search, ref bData, out tempB);
break;
case ColliderType.Plane:
//SupportHelper.boxSupport(ref negativeSearch, ref bData, ref tempB);
break;
}
assignment = new SupportPoint(uniqueId++);
assignment.aSupport = tempA;
assignment.bSupport = tempB;
assignment.v = tempB - tempA;
}
private void CreateAndFillHole(SupportPoint newp)
{
List <Triangle> tris = GetBehinds(newp);
int removed = tris.Count;
List <Line> lines = new List <Line>();
foreach (var t in tris)
{
poly.Remove(t);
lines.AddRange(t.ToLines());
}
lines = LinesInTriangles(lines, poly);
int added = lines.Count;
AddNewTrianglesFromLines(lines, newp);
Console.WriteLine(added - removed);
if (added - removed > trianglesAddedAbort)
{
shouldFail = true;
}
}
public void UpdateDepthTest()
{
Console.WriteLine(messages[counter]);
vec3 r = pm.DepthDetector.GetPenetrationDepthStep(result, ref tris, ref removed, ref closest, ref newPoint, ref counter);
if (r != vec3.NaN)
{
Console.WriteLine("Returned: " + r + ", normal: " + closest.Normal);
//Console.WriteLine("EPA normal: " + pm.DepthDetector.GetPenetrationInfo(result).Item1.Normal) ;
TriMeshes.Clear();
removed.Clear();
closest = null;
counter = 0;
newPoint = new SupportPoint(vec3.NaN, vec3.NaN);
drawPoint = vec3.NaN;
return;
}
drawPoint = newPoint.Sup;
TriMeshes.ForEach(t => t.Dispose());
TriMeshes.Clear();
tris.ForEach(t => TriMeshes.Add(new LineMesh(t.Vec3Points, t == closest ? triColorsClosest : triColors, t == closest ? 15 : 5)));
tris.ForEach(t => TriMeshes.Add(new LineMesh(new vec3[] { t.Center, t.Center + t.Normal.NormalizedSafe }, normalColors, drawPoint == vec3.NaN || vec3.Dot(t.Normal, newPoint.Sup - t.Vec3Points[0]) < 0 ? 5 : 15)));
if (counter == 2)
{
drawPoint = tris.Find(t => t == closest).ClosestPoint();
}
if (drawPoint != vec3.NaN)
{
pointMesh.SetPoints(new vec3[] { drawPoint });
}
}
public static MinkowskiPoint MinkowskiPointNew(SupportPoint a, SupportPoint b)
{
var point = new MinkowskiPoint(a.p, b.p,
cpVect.cpvsub(b.p, a.p),
((a.id & 0xFF) << 8 | (b.id & 0xFF)));
return point;
}
protected bool Equals(SupportPoint other)
{
return(X.Equals(other.X) && Y.Equals(other.Y));
}
public bool StartsWith(SupportPoint p) => p.point == a.point;
public Line(SupportPoint a, SupportPoint b)
{
this.a = a;
this.b = b;
}
