/// <summary>
/// Expands a polygon by the specified distance.
/// </summary>
/// <param name="poly"></param>
/// <param name="distance"></param>
/// <param name="constructor"></param>
/// <returns></returns>
public static IPoly Expand(this IPoly poly, float distance, Func <IEnumerable <Vector3>, IPoly> constructor = null)
{
if (constructor == null)
{
constructor = poly.Clone;
}
Vector3[] points = new Vector3[poly.Resolution];
Vector3[] normals = new Vector3[poly.Resolution];
Vector3[] direction = new Vector3[poly.Resolution];
for (int i = 0; i < poly.Resolution; i++)
{
points[i] = poly.GetPoint(i);
normals[i] = poly.GetSurfaceNormal(i).normalized;
direction[i] = poly.GetPoint((i + 1) % poly.Resolution) - poly.GetPoint(i);
}
Vector3[] outputPoints = new Vector3[poly.Resolution];
for (int i = 0; i < poly.Resolution; i++)
{
Vector3 p0 = points[i] + normals[i] * distance;
int iMinus = (i - 1 + poly.Resolution) % poly.Resolution;
Vector3 l0 = points[iMinus] + normals[iMinus] * distance;
outputPoints[i] = Math3d.LinePlaneIntersection(l0, direction[iMinus], p0, normals[i]);
}
return(constructor(outputPoints));
}
/// <summary>
/// Find the optimal split for a polygon
/// </summary>
/// <param name="poly"></param>
/// <returns></returns>
private (Vector3 point, Vector3 normal, List <BSPPoly <T> > left, List <BSPPoly <T> > right) FindOptimalSplit(BSPPoly <T>[] poly)
{
//init
Vector3 point = Vector3.zero, normal = Vector3.zero;
List <BSPPoly <T> > left = null, right = null;
//optimal diff
int diff = int.MaxValue;
//get the working polygon
IPoly working = poly[0].working;
for (int i = 0; i < working.Resolution; i++)
{
//get the current point and normal
Vector3 localPoint = working.GetPoint(i),
localNormal = working.GetSurfaceNormal(i);
//get output
(List <BSPPoly <T> > localInside, List <BSPPoly <T> > localOutput) = GetSplit(poly, localPoint, localNormal);
//calc the diff, and compare
int localDiff = Math.Abs(localInside.Count - localOutput.Count);
if (localDiff < diff)
{
left = localInside;
right = localOutput;
point = localPoint;
normal = localNormal;
diff = localDiff;
}
}
return(point, normal, left, right);
}
/// <summary>
/// Get the next bsp node for the polygons
/// </summary>
/// <param name="polygons"></param>
/// <param name="optimize"></param>
/// <returns></returns>
private BSPNode <T> GetNode(BSPPoly <T>[] polygons, bool optimize)
{
if (polygons.Length == 0)
{
return(null);
}
if (polygons.Length == 1)
{
return(new BSPNode <T>(polygons[0], null, null, Vector3.zero, Vector3.zero));
}
if (optimize)
{
polygons = FindOptimalPoly(polygons);
Vector3 point, normal;
List <BSPPoly <T> > left, right;
(point, normal, left, right) = FindOptimalSplit(polygons);
return(new BSPNode <T>(polygons[0], GetNode(left.ToArray(), optimize), GetNode(right.ToArray(), optimize), point, normal));
}
else
{
IPoly working = polygons[0].working;
Vector3 point = working.GetPoint(0),
normal = working.GetSurfaceNormal(0);
(List <BSPPoly <T> > left, List <BSPPoly <T> > right) = GetSplit(polygons, point, normal);
return(new BSPNode <T>(polygons[0], GetNode(left.ToArray(), optimize), GetNode(right.ToArray(), optimize), point, normal));
}
}
/// <summary>
/// Calculates the collision status of two polygons.
/// Returns colliding is the polygons are intersecting, not colliding if they are not, AEnclosedInB is A is inside B, and BEnclosedInA for ...
/// This method also returns the offending index
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="offendingIndex"></param>
/// <param name="threshold"></param>
/// <returns></returns>
public static CollisionType CalcCollision2D(IPoly a, IPoly b, out int offendingIndex, float threshold = 0.001f)
{
offendingIndex = -1;
//check for a
bool enclosed = true;
for (int i = 0; i < a.Resolution; i++)
{
Vector3 point = a.GetPoint(i);
Vector3 surfaceNormal = a.GetSurfaceNormal(i);
bool inside, outside;
CheckPoly(b, point, surfaceNormal, out inside, out outside, threshold);
if (outside)
{
enclosed = false;
}
if (!inside)
{
return(CollisionType.NotColliding);
}
if (inside && outside)
{
offendingIndex = i;
}
}
if (enclosed)
{
return(CollisionType.BEnclosedInA);
}
//check for b
enclosed = true;
for (int i = 0; i < b.Resolution; i++)
{
Vector3 point = b.GetPoint(i);
Vector3 surfaceNormal = b.GetSurfaceNormal(i);
bool inside, outside;
CheckPoly(a, point, surfaceNormal, out inside, out outside, threshold);
if (outside)
{
enclosed = false;
}
if (!inside)
{
return(CollisionType.NotColliding);
}
}
if (enclosed)
{
return(CollisionType.AEnclosedInB);
}
return(CollisionType.Colliding);
}
/// <summary>
/// Preforms the set subtract operation with multiple pos and a single neg.
/// </summary>
/// <param name="pos"></param>
/// <param name="neg"></param>
/// <param name="constructor"></param>
/// <returns></returns>
public static List <IPoly> SetSubtract(IEnumerable <IPoly> pos, IPoly neg, Func <IPoly, IEnumerable <Vector3>, IPoly> constructor)
{
//set subtract algorithm
//this version works for only 1 negative poly, and is the basis for multiple polys
//append all positive polys to a working stack
//pop polys from the working stack and compare them to the neg poly
//if they intersect, split them, and push them on the working stack
//if they are enclosed, remove them from the working list
//if they aren't instersecting append them to the output stack
//continue until working stack is empty
//Initialization
List <IPoly> outputList = new List <IPoly>();
Stack <IPoly> workingStack = new Stack <IPoly>(pos);
//working stack loop
while (workingStack.Count > 0)
{
//initialization
IPoly current = workingStack.Pop();
int offendingIndex;
//detect collision status
var status = CSGPhysics.CalcCollision2D(neg, current, out offendingIndex);
//enclosed, just throw away polygon
if (status == CSGPhysics.CollisionType.BEnclosedInA)
{
continue;
}
//if colliding, divide the polygon and push them on the working stack
else if (status != CSGPhysics.CollisionType.NotColliding)
{
IPoly inside, outside;
Divide(current, out inside, out outside, neg.GetPoint(offendingIndex), neg.GetSurfaceNormal(offendingIndex), constructor);
workingStack.Push(inside);
workingStack.Push(outside);
}
//if not colliding, append to output list
else
{
outputList.Add(current);
}
}
return(outputList);
}
