/**
* @brief Returns the first {@link FPCollider2D} within a rectangular area. Returns null if there is none.
*
* @param pointA Top-left corner of the rectangle.
* @param radius Bottom-right corner of the rectangle.
* @param layerMask Unity's layer mask to filter objects.
**/
public static object _OverlapArea(FPVector2 pointA, FPVector2 pointB, Physics2D.BodySpecialSensor sensorType, int layerMask = UnityEngine.Physics.DefaultRaycastLayers)
{
FPVector2 center;
center.x = (pointA.x + pointB.x) * FP.Half;
center.y = (pointA.y + pointB.y) * FP.Half;
Physics2D.Vertices vertices = new Physics2D.Vertices(4);
vertices.Add(new FPVector2(pointA.x, pointA.y) - center);
vertices.Add(new FPVector2(pointB.x, pointA.y) - center);
vertices.Add(new FPVector2(pointB.x, pointB.y) - center);
vertices.Add(new FPVector2(pointA.x, pointB.y) - center);
return(OverlapGeneric(new Physics2D.PolygonShape(vertices, 1), center, sensorType, layerMask));
}
/**
* @brief Create the internal shape used to represent a PolygonCollider.
**/
public override Spax.Physics2D.Shape[] CreateShapes()
{
if (_points == null || _points.Length == 0)
{
return(null);
}
FPVector2 lossy2D = new FPVector2(lossyScale.x, lossyScale.y);
Spax.Physics2D.Vertices v = new Physics2D.Vertices();
for (int index = 0, length = _points.Length; index < length; index++)
{
v.Add(FPVector2.Scale(_points[index], lossy2D));
}
List <Spax.Physics2D.Vertices> convexShapeVs = Spax.Physics2D.BayazitDecomposer.ConvexPartition(v);
Spax.Physics2D.Shape[] result = new Physics2D.Shape[convexShapeVs.Count];
for (int index = 0, length = result.Length; index < length; index++)
{
result[index] = new Spax.Physics2D.PolygonShape(convexShapeVs[index], 1);
}
return(result);
}
/// <summary>
/// Merges all parallel edges in the list of vertices
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="tolerance">The tolerance.</param>
public static Vertices MergeParallelEdges(Vertices vertices, FP tolerance)
{
//From Eric Jordan's convex decomposition library
if (vertices.Count <= 3)
{
return(vertices); //Can't do anything useful here to a triangle
}
bool[] mergeMe = new bool[vertices.Count];
int newNVertices = vertices.Count;
//Gather points to process
for (int i = 0; i < vertices.Count; ++i)
{
int lower = (i == 0) ? (vertices.Count - 1) : (i - 1);
int middle = i;
int upper = (i == vertices.Count - 1) ? (0) : (i + 1);
FP dx0 = vertices[middle].x - vertices[lower].x;
FP dy0 = vertices[middle].y - vertices[lower].y;
FP dx1 = vertices[upper].y - vertices[middle].x;
FP dy1 = vertices[upper].y - vertices[middle].y;
FP norm0 = FP.Sqrt(dx0 * dx0 + dy0 * dy0);
FP norm1 = FP.Sqrt(dx1 * dx1 + dy1 * dy1);
if (!(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3)
{
//Merge identical points
mergeMe[i] = true;
--newNVertices;
}
dx0 /= norm0;
dy0 /= norm0;
dx1 /= norm1;
dy1 /= norm1;
FP cross = dx0 * dy1 - dx1 * dy0;
FP dot = dx0 * dx1 + dy0 * dy1;
if (FP.Abs(cross) < tolerance && dot > 0 && newNVertices > 3)
{
mergeMe[i] = true;
--newNVertices;
}
else
{
mergeMe[i] = false;
}
}
if (newNVertices == vertices.Count || newNVertices == 0)
{
return(vertices);
}
int currIndex = 0;
//Copy the vertices to a new list and clear the old
Vertices newVertices = new Vertices(newNVertices);
for (int i = 0; i < vertices.Count; ++i)
{
if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices)
{
continue;
}
Debug.Assert(currIndex < newNVertices);
newVertices.Add(vertices[i]);
++currIndex;
}
return(newVertices);
}
/// <summary>
/// Returns the convex hull from the given vertices..
/// </summary>
public static Vertices GetConvexHull(Vertices vertices)
{
if (vertices.Count <= 3)
{
return(vertices);
}
Vertices pointSet = new Vertices(vertices);
//Sort by X-axis
pointSet.Sort(_pointComparer);
FPVector2[] h = new FPVector2[pointSet.Count];
Vertices res;
int top = -1; // indices for bottom and top of the stack
int i; // array scan index
// Get the indices of points with min x-coord and min|max y-coord
const int minmin = 0;
FP xmin = pointSet[0].x;
for (i = 1; i < pointSet.Count; i++)
{
if (pointSet[i].x != xmin)
{
break;
}
}
// degenerate case: all x-coords == xmin
int minmax = i - 1;
if (minmax == pointSet.Count - 1)
{
h[++top] = pointSet[minmin];
if (pointSet[minmax].y != pointSet[minmin].y) // a nontrivial segment
{
h[++top] = pointSet[minmax];
}
h[++top] = pointSet[minmin]; // add polygon endpoint
res = new Vertices(top + 1);
for (int j = 0; j < top + 1; j++)
{
res.Add(h[j]);
}
return(res);
}
top = -1;
// Get the indices of points with max x-coord and min|max y-coord
int maxmax = pointSet.Count - 1;
FP xmax = pointSet[pointSet.Count - 1].x;
for (i = pointSet.Count - 2; i >= 0; i--)
{
if (pointSet[i].x != xmax)
{
break;
}
}
int maxmin = i + 1;
// Compute the lower hull on the stack H
h[++top] = pointSet[minmin]; // push minmin point onto stack
i = minmax;
while (++i <= maxmin)
{
// the lower line joins P[minmin] with P[maxmin]
if (MathUtils.Area(pointSet[minmin], pointSet[maxmin], pointSet[i]) >= 0 && i < maxmin)
{
continue; // ignore P[i] above or on the lower line
}
while (top > 0) // there are at least 2 points on the stack
{
// test if P[i] is left of the line at the stack top
if (MathUtils.Area(h[top - 1], h[top], pointSet[i]) > 0)
{
break; // P[i] is a new hull vertex
}
top--; // pop top point off stack
}
h[++top] = pointSet[i]; // push P[i] onto stack
}
// Next, compute the upper hull on the stack H above the bottom hull
if (maxmax != maxmin) // if distinct xmax points
{
h[++top] = pointSet[maxmax]; // push maxmax point onto stack
}
int bot = top;
i = maxmin;
while (--i >= minmax)
{
// the upper line joins P[maxmax] with P[minmax]
if (MathUtils.Area(pointSet[maxmax], pointSet[minmax], pointSet[i]) >= 0 && i > minmax)
{
continue; // ignore P[i] below or on the upper line
}
while (top > bot) // at least 2 points on the upper stack
{
// test if P[i] is left of the line at the stack top
if (MathUtils.Area(h[top - 1], h[top], pointSet[i]) > 0)
{
break; // P[i] is a new hull vertex
}
top--; // pop top point off stack
}
h[++top] = pointSet[i]; // push P[i] onto stack
}
if (minmax != minmin)
{
h[++top] = pointSet[minmin]; // push joining endpoint onto stack
}
res = new Vertices(top + 1);
for (int j = 0; j < top + 1; j++)
{
res.Add(h[j]);
}
return(res);
}
/// <summary>
/// Calculates the polygon(s) from the result simplical chain.
/// </summary>
/// <remarks>Used by method <c>Execute()</c>.</remarks>
private static PolyClipError BuildPolygonsFromChain(List <Edge> simplicies, out List <Vertices> result)
{
result = new List <Vertices>();
PolyClipError errVal = PolyClipError.None;
while (simplicies.Count > 0)
{
Vertices output = new Vertices();
output.Add(simplicies[0].EdgeStart);
output.Add(simplicies[0].EdgeEnd);
simplicies.RemoveAt(0);
bool closed = false;
int index = 0;
int count = simplicies.Count; // Needed to catch infinite loops
while (!closed && simplicies.Count > 0)
{
if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeStart))
{
if (VectorEqual(simplicies[index].EdgeEnd, output[0]))
{
closed = true;
}
else
{
output.Add(simplicies[index].EdgeEnd);
}
simplicies.RemoveAt(index);
--index;
}
else if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeEnd))
{
if (VectorEqual(simplicies[index].EdgeStart, output[0]))
{
closed = true;
}
else
{
output.Add(simplicies[index].EdgeStart);
}
simplicies.RemoveAt(index);
--index;
}
if (!closed)
{
if (++index == simplicies.Count)
{
if (count == simplicies.Count)
{
result = new List <Vertices>();
Debug.WriteLine("Undefined error while building result polygon(s).");
return(PolyClipError.BrokenResult);
}
index = 0;
count = simplicies.Count;
}
}
}
if (output.Count < 3)
{
errVal = PolyClipError.DegeneratedOutput;
Debug.WriteLine("Degenerated output polygon produced (vertices < 3).");
}
result.Add(output);
}
return(errVal);
}
/// <summary>
/// Decompose the polygon into triangles.
///
/// Properties:
/// - Only works on counter clockwise polygons
///
/// </summary>
/// <param name="vertices">The list of points describing the polygon</param>
public static List <Vertices> ConvexPartition(Vertices vertices)
{
Debug.Assert(vertices.Count > 3);
Debug.Assert(vertices.IsCounterClockWise());
int[] polygon = new int[vertices.Count];
for (int v = 0; v < vertices.Count; v++)
{
polygon[v] = v;
}
int nv = vertices.Count;
// Remove nv-2 Vertices, creating 1 triangle every time
int count = 2 * nv; /* error detection */
List <Vertices> result = new List <Vertices>();
for (int v = nv - 1; nv > 2;)
{
// If we loop, it is probably a non-simple polygon
if (0 >= (count--))
{
// Triangulate: ERROR - probable bad polygon!
return(new List <Vertices>());
}
// Three consecutive vertices in current polygon, <u,v,w>
int u = v;
if (nv <= u)
{
u = 0; // Previous
}
v = u + 1;
if (nv <= v)
{
v = 0; // New v
}
int w = v + 1;
if (nv <= w)
{
w = 0; // Next
}
_tmpA = vertices[polygon[u]];
_tmpB = vertices[polygon[v]];
_tmpC = vertices[polygon[w]];
if (Snip(vertices, u, v, w, nv, polygon))
{
int s, t;
// Output Triangle
Vertices triangle = new Vertices(3);
triangle.Add(_tmpA);
triangle.Add(_tmpB);
triangle.Add(_tmpC);
result.Add(triangle);
// Remove v from remaining polygon
for (s = v, t = v + 1; t < nv; s++, t++)
{
polygon[s] = polygon[t];
}
nv--;
// Reset error detection counter
count = 2 * nv;
}
}
return(result);
}
/// <summary>
/// Activate the explosion at the specified position.
/// </summary>
/// <param name="pos">The position where the explosion happens </param>
/// <param name="radius">The explosion radius </param>
/// <param name="maxForce">The explosion force at the explosion point (then is inversely proportional to the square of the distance)</param>
/// <returns>A list of bodies and the amount of force that was applied to them.</returns>
public Dictionary <Fixture, FPVector2> Activate(FPVector2 pos, FP radius, FP maxForce)
{
AABB aabb;
aabb.LowerBound = pos + new FPVector2(-radius, -radius);
aabb.UpperBound = pos + new FPVector2(radius, radius);
Fixture[] shapes = new Fixture[MaxShapes];
// More than 5 shapes in an explosion could be possible, but still strange.
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
// Query the world for overlapping shapes.
World.QueryAABB(
fixture =>
{
if (fixture.TestPoint(ref pos))
{
if (IgnoreWhenInsideShape)
{
exit = true;
return(false);
}
containedShapes[containedShapeCount++] = fixture;
}
else
{
shapes[shapeCount++] = fixture;
}
// Continue the query.
return(true);
}, ref aabb);
if (exit)
{
return(new Dictionary <Fixture, FPVector2>());
}
Dictionary <Fixture, FPVector2> exploded = new Dictionary <Fixture, FPVector2>(shapeCount + containedShapeCount);
// Per shape max/min angles for now.
FP[] vals = new FP[shapeCount * 2];
int valIndex = 0;
for (int i = 0; i < shapeCount; ++i)
{
PolygonShape ps;
CircleShape cs = shapes[i].Shape as CircleShape;
if (cs != null)
{
// We create a "diamond" approximation of the circle
Vertices v = new Vertices();
FPVector2 vec = FPVector2.zero + new FPVector2(cs.Radius, 0);
v.Add(vec);
vec = FPVector2.zero + new FPVector2(0, cs.Radius);
v.Add(vec);
vec = FPVector2.zero + new FPVector2(-cs.Radius, cs.Radius);
v.Add(vec);
vec = FPVector2.zero + new FPVector2(0, -cs.Radius);
v.Add(vec);
ps = new PolygonShape(v, 0);
}
else
{
ps = shapes[i].Shape as PolygonShape;
}
if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
{
FPVector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos;
FP angleToCentroid = FP.Atan2(toCentroid.y, toCentroid.x);
FP min = FP.MaxValue;
FP max = FP.MinValue;
FP minAbsolute = 0.0f;
FP maxAbsolute = 0.0f;
for (int j = 0; j < ps.Vertices.Count; ++j)
{
FPVector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
FP newAngle = FP.Atan2(toVertex.y, toVertex.x);
FP diff = (newAngle - angleToCentroid);
diff = (diff - FP.Pi) % (2 * FP.Pi);
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
if (diff < 0.0f)
{
diff += 2 * FP.Pi; // correction for not handling negs
}
diff -= FP.Pi;
if (FP.Abs(diff) > FP.Pi)
{
continue; // Something's wrong, point not in shape but exists angle diff > 180
}
if (diff > max)
{
max = diff;
maxAbsolute = newAngle;
}
if (diff < min)
{
min = diff;
minAbsolute = newAngle;
}
}
vals[valIndex] = minAbsolute;
++valIndex;
vals[valIndex] = maxAbsolute;
++valIndex;
}
}
Array.Sort(vals, 0, valIndex, _rdc);
_data.Clear();
bool rayMissed = true;
for (int i = 0; i < valIndex; ++i)
{
Fixture fixture = null;
FP midpt;
int iplus = (i == valIndex - 1 ? 0 : i + 1);
if (vals[i] == vals[iplus])
{
continue;
}
if (i == valIndex - 1)
{
// the single edgecase
midpt = (vals[0] + FP.PiTimes2 + vals[i]);
}
else
{
midpt = (vals[i + 1] + vals[i]);
}
midpt = midpt / 2;
FPVector2 p1 = pos;
FPVector2 p2 = radius * new FPVector2(FP.Cos(midpt), FP.Sin(midpt)) + pos;
// RaycastOne
bool hitClosest = false;
World.RayCast((f, p, n, fr) =>
{
Body body = f.Body;
if (!IsActiveOn(body))
{
return(0);
}
hitClosest = true;
fixture = f;
return(fr);
}, p1, p2);
//draws radius points
if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic))
{
if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed))
{
int laPos = _data.Count - 1;
ShapeData la = _data[laPos];
la.Max = vals[iplus];
_data[laPos] = la;
}
else
{
// make new
ShapeData d;
d.Body = fixture.Body;
d.Min = vals[i];
d.Max = vals[iplus];
_data.Add(d);
}
if ((_data.Count > 1) &&
(i == valIndex - 1) &&
(_data.Last().Body == _data.First().Body) &&
(_data.Last().Max == _data.First().Min))
{
ShapeData fi = _data[0];
fi.Min = _data.Last().Min;
_data.RemoveAt(_data.Count - 1);
_data[0] = fi;
while (_data.First().Min >= _data.First().Max)
{
fi.Min -= FP.PiTimes2;
_data[0] = fi;
}
}
int lastPos = _data.Count - 1;
ShapeData last = _data[lastPos];
while ((_data.Count > 0) &&
(_data.Last().Min >= _data.Last().Max)) // just making sure min<max
{
last.Min = _data.Last().Min - FP.PiTimes2;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count; ++i)
{
if (!IsActiveOn(_data[i].Body))
{
continue;
}
FP arclen = _data[i].Max - _data[i].Min;
FP first = FPMath.Min(MaxEdgeOffset, EdgeRatio * arclen);
int insertedRays = FP.Ceiling((((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle)).AsInt();
if (insertedRays < 0)
{
insertedRays = 0;
}
FP offset = (arclen - first * 2.0f) / ((FP)MinRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on FPs.
//Added FPEquals with a large epsilon.
for (FP j = _data[i].Min + first;
j < _data[i].Max || MathUtils.FPEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
FPVector2 p1 = pos;
FPVector2 p2 = pos + radius * new FPVector2(FP.Cos(j), FP.Sin(j));
FPVector2 hitpoint = FPVector2.zero;
FP minlambda = FP.MaxValue;
List <Fixture> fl = _data[i].Body.FixtureList;
for (int x = 0; x < fl.Count; x++)
{
Fixture f = fl[x];
RayCastInput ri;
ri.Point1 = p1;
ri.Point2 = p2;
ri.MaxFraction = 50f;
RayCastOutput ro;
if (f.RayCast(out ro, ref ri, 0))
{
if (minlambda > ro.Fraction)
{
minlambda = ro.Fraction;
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
}
}
// the force that is to be applied for this particular ray.
// offset is angular coverage. lambda*length of segment is distance.
FP impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / FP.Pi * (1.0f - Spax.FPMath.Min(FP.One, minlambda));
// We Apply the impulse!!!
FPVector2 vectImp = FPVector2.Dot(impulse * new FPVector2(FP.Cos(j), FP.Sin(j)), -ro.Normal) * new FPVector2(FP.Cos(j), FP.Sin(j));
_data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
// We gather the fixtures for returning them
if (exploded.ContainsKey(f))
{
exploded[f] += vectImp;
}
else
{
exploded.Add(f, vectImp);
}
if (minlambda > 1.0f)
{
hitpoint = p2;
}
}
}
}
// We check contained shapes
for (int i = 0; i < containedShapeCount; ++i)
{
Fixture fix = containedShapes[i];
if (!IsActiveOn(fix.Body))
{
continue;
}
FP impulse = MinRays * maxForce * 180.0f / FP.Pi;
FPVector2 hitPoint;
CircleShape circShape = fix.Shape as CircleShape;
if (circShape != null)
{
hitPoint = fix.Body.GetWorldPoint(circShape.Position);
}
else
{
PolygonShape shape = fix.Shape as PolygonShape;
hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid);
}
FPVector2 vectImp = impulse * (hitPoint - pos);
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
if (!exploded.ContainsKey(fix))
{
exploded.Add(fix, vectImp);
}
}
return(exploded);
}
/// <summary>
/// Combine a list of triangles into a list of convex polygons.
///
/// Note: This only works on triangles.
/// </summary>
///<param name="triangles">The triangles.</param>
///<param name="maxPolys">The maximun number of polygons to return.</param>
///<param name="tolerance">The tolerance</param>
public static List <Vertices> PolygonizeTriangles(List <Vertices> triangles, int maxPolys = int.MaxValue, float tolerance = 0.001f)
{
if (triangles.Count <= 0)
{
return(triangles);
}
List <Vertices> polys = new List <Vertices>();
bool[] covered = new bool[triangles.Count];
for (int i = 0; i < triangles.Count; ++i)
{
covered[i] = false;
//Check here for degenerate triangles
Vertices triangle = triangles[i];
FPVector2 a = triangle[0];
FPVector2 b = triangle[1];
FPVector2 c = triangle[2];
if ((a.x == b.x && a.y == b.y) || (b.x == c.x && b.y == c.y) || (a.x == c.x && a.y == c.y))
{
covered[i] = true;
}
}
int polyIndex = 0;
bool notDone = true;
while (notDone)
{
int currTri = -1;
for (int i = 0; i < triangles.Count; ++i)
{
if (covered[i])
{
continue;
}
currTri = i;
break;
}
if (currTri == -1)
{
notDone = false;
}
else
{
Vertices poly = new Vertices(3);
for (int i = 0; i < 3; i++)
{
poly.Add(triangles[currTri][i]);
}
covered[currTri] = true;
int index = 0;
for (int i = 0; i < 2 * triangles.Count; ++i, ++index)
{
while (index >= triangles.Count)
{
index -= triangles.Count;
}
if (covered[index])
{
continue;
}
Vertices newP = AddTriangle(triangles[index], poly);
if (newP == null)
{
continue; // is this right
}
if (newP.Count > Settings.MaxPolygonVertices)
{
continue;
}
if (newP.IsConvex())
{
//Or should it be IsUsable? Maybe re-write IsConvex to apply the angle threshold from Box2d
poly = new Vertices(newP);
covered[index] = true;
}
}
//We have a maximum of polygons that we need to keep under.
if (polyIndex < maxPolys)
{
SimplifyTools.MergeParallelEdges(poly, tolerance);
//If identical points are present, a triangle gets
//borked by the MergeParallelEdges function, hence
//the vertex number check
if (poly.Count >= 3)
{
polys.Add(new Vertices(poly));
}
else
{
Debug.WriteLine("Skipping corrupt poly.");
}
}
if (poly.Count >= 3)
{
polyIndex++; //Must be outside (polyIndex < polysLength) test
}
}
}
//TODO: Add sanity check
//Remove empty vertice collections
for (int i = polys.Count - 1; i >= 0; i--)
{
if (polys[i].Count == 0)
{
polys.RemoveAt(i);
}
}
return(polys);
}
private static Vertices AddTriangle(Vertices t, Vertices vertices)
{
// First, find vertices that connect
int firstP = -1;
int firstT = -1;
int secondP = -1;
int secondT = -1;
for (int i = 0; i < vertices.Count; i++)
{
if (t[0].x == vertices[i].x && t[0].y == vertices[i].y)
{
if (firstP == -1)
{
firstP = i;
firstT = 0;
}
else
{
secondP = i;
secondT = 0;
}
}
else if (t[1].x == vertices[i].x && t[1].y == vertices[i].y)
{
if (firstP == -1)
{
firstP = i;
firstT = 1;
}
else
{
secondP = i;
secondT = 1;
}
}
else if (t[2].x == vertices[i].x && t[2].y == vertices[i].y)
{
if (firstP == -1)
{
firstP = i;
firstT = 2;
}
else
{
secondP = i;
secondT = 2;
}
}
}
// Fix ordering if first should be last vertex of poly
if (firstP == 0 && secondP == vertices.Count - 1)
{
firstP = vertices.Count - 1;
secondP = 0;
}
// Didn't find it
if (secondP == -1)
{
return(null);
}
// Find tip index on triangle
int tipT = 0;
if (tipT == firstT || tipT == secondT)
{
tipT = 1;
}
if (tipT == firstT || tipT == secondT)
{
tipT = 2;
}
Vertices result = new Vertices(vertices.Count + 1);
for (int i = 0; i < vertices.Count; i++)
{
result.Add(vertices[i]);
if (i == firstP)
{
result.Add(t[tipT]);
}
}
return(result);
}