public static bool PointInPoly(this IPoly poly, Vector3 point, Vector3 projection)
{
for (int i = 1; i < poly.Resolution - 1; i++)
{
Vector3 a = poly.GetPoint(i) - poly.GetPoint(0);
Vector3 b = poly.GetPoint(i + 1) - poly.GetPoint(0);
Vector3 c = projection;
Vector3 d = point - poly.GetPoint(0);
var m11 = b.y * c.z - b.z * c.y;
var m21 = b.x * c.z - b.z * c.x;
var m31 = b.x * c.y - b.y * c.x;
var m12 = a.y * c.z - a.z * c.y;
var m22 = a.x * c.z - a.z * c.x;
var m32 = a.x * c.y - a.y * c.x;
float detT = a.x * m11 - a.y * m21 + a.z * m31;
float detTx = d.x * m11 - d.y * m21 + d.z * m31;
float detTy = -d.x * m12 + d.y * m22 - d.z * m32;
float x = detTx / detT;
float y = detTy / detT;
//Debug.Log(a + ":" + b + ":" + c + ":" + d + ":" + x + ":" + y + ":" + (x + y));
if (x >= 0 && y >= 0 && x + y <= 1f)
{
return(true);
}
}
return(false);
}
public static (float[], int[]) GetBisectedThetaValues(IPoly polygon, int index)
{
Vector3 center = polygon.GetPoint(index);
Vector3 left = polygon.GetPointWrapped(index - 1);
Vector3 right = polygon.GetPointWrapped(index + 1);
Vector3 biSect = Vector3.Lerp((left - center).normalized, (right - center).normalized, 0.5f).normalized;
int[] indicies = new int[polygon.Resolution - 1];
float[] tValues = new float[polygon.Resolution - 1];
for (int i = 0; i < polygon.Resolution; i++)
{
if (i < index)
{
indicies[i] = i;
tValues[i] = Vector3.Dot(biSect, (polygon.GetPoint(i) - center).normalized);
}
else if (i == index)
{
continue;
}
else
{
indicies[i - 1] = i;
tValues[i - 1] = Vector3.Dot(biSect, (polygon.GetPoint(i) - center).normalized);
}
}
Array.Sort(tValues, indicies);
return(tValues, indicies);
}
public static IBlock Extrude(IPoly polygon, Vector3 direction, float floorDistance, float ceilingDistance, Func <IEnumerable <Vector3>, IPoly> polyConstructor = null, Func <IEnumerable <IPoly>, IBlock> blockConstructor = null)
{
if (polyConstructor == null)
{
polyConstructor = polygon.Clone;
}
IPoly floor = polygon.Clone(polygon.GetPoints().Select(x => x + direction * floorDistance));
IPoly ceiling = polygon.Clone(polygon.GetPoints().Select(x => x + direction * ceilingDistance));
List <IPoly> faces = new List <IPoly>();
for (int i = 0; i < polygon.Resolution; i++)
{
Vector3 lr = floor.GetPoint(i);
Vector3 ll = floor.GetPoint((i + 1) % polygon.Resolution);
Vector3 ul = ceiling.GetPoint((i + 1) % polygon.Resolution);
Vector3 ur = ceiling.GetPoint(i);
faces.Add(polyConstructor(new Vector3[] { lr, ll, ul, ur }));
}
faces.Add(polyConstructor(ceiling.GetPoints()));
faces.Add(polyConstructor(floor.GetPoints().Reverse()));
if (blockConstructor == null)
{
blockConstructor = (x) => new Block(x);
}
return(blockConstructor(faces));
}
public static IPoly RemoveDuplicateVerticies(IPoly poly)
{
List <Vector3> output = new List <Vector3>();
for (int i = 0; i < poly.Resolution; i++)
{
if (output.Count == 0)
{
output.Add(poly.GetPoint(i));
}
else
{
Vector3 point = poly.GetPoint(i);
Vector3 last = output[output.Count - 1];
if (!Math3d.Compare(point, last))
{
output.Add(point);
}
}
}
if (Math3d.Compare(output[0], output[output.Count - 1]))
{
output.RemoveAt(0);
}
return(poly.Clone(output.ToArray()));
}
/// <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>
/// Calculate the normal for the polygon
/// Always calculates the normal, so may be slow.
/// </summary>
/// <param name="poly"></param>
/// <returns></returns>
public static Vector3 CalcNormal(this IPoly poly)
{
if (poly.Resolution < 3)
{
throw new System.InvalidOperationException("Polygon must have at least 3 points");
}
return(Vector3.Cross(poly.GetPoint(1) - poly.GetPoint(0), poly.GetPoint(2) - poly.GetPoint(0)).normalized);
}
/// <summary>
/// A default method for drawing the polygon
/// </summary>
/// <param name="poly"></param>
/// <param name="color"></param>
/// <param name="time"></param>
public static void DrawPoly(this IPoly poly, Color color, float time)
{
int r = poly.Resolution;
for (int i = 0; i < r; i++)
{
int j = (i + 1) % r;
Debug.DrawLine(poly.GetPoint(i), poly.GetPoint(j), color, time);
}
}
/// <summary>
/// Get a point, accounting for edges wrapping around the polygon
/// </summary>
/// <param name="poly"></param>
/// <param name="index"></param>
/// <returns></returns>
public static Vector3 GetPointWrapped(this IPoly poly, int index)
{
if (index < 0)
{
return(poly.GetPoint(-(index % poly.Resolution)));
}
else
{
return(poly.GetPoint(index % poly.Resolution));
}
}
/// <summary>
/// Calculate the perimeter of the polygon
/// Always calculates the perimeter, so may be slow.
/// </summary>
/// <param name="poly"></param>
/// <returns></returns>
public static float CalcPerimeter(this IPoly poly)
{
float perim = 0f;
int resolution = poly.Resolution;
for (int i = 0; i < resolution; i++)
{
int j = (i + 1) % resolution;
perim += (poly.GetPoint(i) - poly.GetPoint(j)).magnitude;
}
return(perim);
}
/// <summary>
/// Get the area of the polygon.
/// Always calculates the area, so may be expensive.
/// </summary>
/// <param name="poly"></param>
/// <returns></returns>
public static float CalcArea(this IPoly poly)
{
float area = 0f;
int resolution = poly.Resolution;
Vector3 p0 = poly.GetPoint(0);
for (int i = 1; i < resolution - 1; i++)
{
Vector3 p1 = poly.GetPoint(i);
Vector3 p2 = poly.GetPoint(i + 1);
area += Vector3.Cross(p1 - p0, p2 - p0).magnitude / 2f;
}
return(area);
}
public static bool IsEdgeIntersectingPolygon(IPoly polygon, Vector3 point1, Vector3 point2)
{
for (int i = 0; i < polygon.Resolution; i++)
{
Vector3 a = polygon.GetPoint(i);
Vector3 b = polygon.GetPointWrapped(i + 1);
if ((point1 - a).sqrMagnitude < 0.0001f)
{
continue;
}
if ((point1 - b).sqrMagnitude < 0.0001f)
{
continue;
}
if ((point2 - a).sqrMagnitude < 0.0001f)
{
continue;
}
if ((point2 - b).sqrMagnitude < 0.0001f)
{
continue;
}
if (Math3d.AreLineSegmentsCrossing(a, b, point1, point2))
{
return(true);
}
}
return(false);
}
/// <summary>
/// Contract the polygon toward a specified point.
/// locks controlls which edges are not able to move to complete the operation
/// </summary>
/// <param name="poly"></param>
/// <param name="center"></param>
/// <param name="tValue"></param>
/// <param name="lockList"></param>
/// <returns></returns>
public static IPoly Contract(this IPoly poly, Vector3 center, float tValue, bool[] locks)
{
if (locks == null)
{
return(Contract(poly, center, tValue));
}
else
{
Vector3[] output = new Vector3[poly.Resolution];
for (int i = 0; i < poly.Resolution; i++)
{
Vector3 point = poly.GetPoint(i);
bool rightLock = locks[i];
int minusIndex = (i + poly.Resolution - 1) % poly.Resolution;
bool leftLock = locks[minusIndex];
if (!rightLock && !leftLock)
{
output[i] = Vector3.Lerp(point, center, tValue);
}
else if (leftLock && rightLock)
{
output[i] = point;
}
else if (leftLock && !rightLock)
{
output[i] = Vector3.Lerp(point, poly.GetEdge(minusIndex).Center(), tValue);
}
else if (!leftLock && rightLock)
{
output[i] = Vector3.Lerp(point, poly.GetEdge(i).Center(), tValue);
}
}
return(poly.Clone(output));
}
}
/// <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>
/// Contract the polygon toward a specified point.
/// </summary>
/// <param name="distance"></param>
/// <param name="center"></param>
/// <param name="lockList"></param>
/// <returns></returns>
public static IPoly Contract(this IPoly poly, Vector3 center, float tValue)
{
Vector3[] output = new Vector3[poly.Resolution];
for (int i = 0; i < poly.Resolution; i++)
{
output[i] = Vector3.Lerp(poly.GetPoint(i), center, tValue);
}
return(poly.Clone(output));
}
public NGon(IPoly poly)
{
int r = poly.Resolution;
points = new Vector3[r];
for (int i = 0; i < r; i++)
{
points[i] = poly.GetPoint(i);
}
}
/// <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>
/// Returns the average of all the points in the polygon.
/// This is a useful approximation for the center of the polygon.
/// </summary>
/// <param name="poly"></param>
/// <returns></returns>
public static Vector3 Average(this IPoly poly)
{
Vector3 point = Vector3.zero;
int l = poly.Resolution;
for (int i = 0; i < l; i++)
{
point += poly.GetPoint(i);
}
return((1f / l) * point);
}
private static bool TryIntersectPolygon(IPoly polygon, float theta, int index, out IPoly a, out IPoly b)
{
a = b = null;
if (theta > Mathf.PI)
{
float[] tValues;
int[] indicies;
(tValues, indicies) = GetBisectedThetaValues(polygon, index);
for (int i = indicies.Length - 1; i >= 0; i--)
{
Vector3 v1 = polygon.GetPoint(index);
Vector3 v2 = polygon.GetPoint(indicies[i]);
if (!IsEdgeIntersectingPolygon(polygon, v1, v2))
{
(a, b) = Divide(polygon, index, i);
return(true);
}
}
}
return(false);
}
/// <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);
}
/// <summary>
/// Checks the collision status of a polygon.
/// </summary>
/// <param name="poly"></param>
/// <param name="p0"></param>
/// <param name="pn"></param>
/// <param name="inside"></param>
/// <param name="outside"></param>
/// <param name="threshold"></param>
public static void CheckPoly(IPoly poly, Vector3 p0, Vector3 pn, out bool inside, out bool outside, float threshold = 0.001f)
{
inside = false;
outside = false;
for (int i = 0; i < poly.Resolution; i++)
{
Vector3 point = poly.GetPoint(i);
float dis = Math3d.SignedDistancePlanePoint(pn, p0, point);
if (dis < -threshold)
{
inside = true;
}
if (dis > threshold)
{
outside = true;
}
if (inside && outside)
{
return;
}
}
}
public Vector3 GetPoint(int index)
{
return(poly.GetPoint(index));
}
/// <summary>
/// Calculates the surface normal normal of the polygon using the cross product
/// </summary>
/// <param name="poly"></param>
/// <param name="index"></param>
/// <returns></returns>
public static Vector3 CalcSurfaceNormal(this IPoly poly, int index)
{
return(-Vector3.Cross(poly.GetNormal(), poly.GetPoint((index + 1) % poly.Resolution) - poly.GetPoint(index)));
}
/// <summary>
/// Divide a polygon using the specified plane.
/// </summary>
/// <param name="poly">
/// The polygon to divide.
/// </param>
/// <param name="p0">
/// Plane Origin.
/// </param>
/// <param name="pn">
/// Plane Normal.
/// </param>
/// <param name="constructor">
/// The Polygon constructor can be specified for more flexability.
/// Defaults to new NGon.
/// </param>
/// <param name="threshold">
/// The percision threshold for comparing points to the plane.
/// </param>
/// <returns>
/// outputs new object[]{IPoly insidePolygon, IPoly outsidePolygon, Vector3 newlyCreatedPoint};
/// newly created point is useful for face concstruction for Block Divison, and texture maping.
/// </returns>
public static object[] Divide(
IPoly poly,
Vector3 p0,
Vector3 pn,
Func <IPoly, IEnumerable <Vector3>, IPoly> constructor,
float threshold = 0.001f)
{
//The following algorithm is used to divide faces
//Create a list for the inside, and outside points
//for each point, append it to the appropriate list
//if the point intersects the boundry, append it to the itnersection list
//construct new faces from the list
//the intersection point exiting the plane is returned, for use by other algorithms
//initialization
List <Vector3> insidePoints = new List <Vector3>();
List <Vector3> outsidePoints = new List <Vector3>();
bool hasOutputPont = false;
Vector3 outputPoint = default(Vector3);
int l = poly.Resolution;
int[] signs = new int[l];
//itterate through poitns and calculate their signs
for (int i = 0; i < l; i++)
{
float f = Math3d.SignedDistancePlanePoint(pn, p0, poly.GetPoint(i));
if (f < -threshold)
{
signs[i] = -1;
}
else if (f > threshold)
{
signs[i] = 1;
}
else
{
signs[i] = 0;
}
}
//itterate through points, and append them to the appropriate list
for (int i = 0; i < l; i++)
{
Vector3 l0 = poly.GetPoint(i);
//inside point
if (signs[i] == -1)
{
insidePoints.Add(l0);
}
//intersecting point
else if (signs[i] == 0)
{
insidePoints.Add(l0);
outsidePoints.Add(l0);
if (signs[(i + 1) % l] == 1)
{
outputPoint = l0;
hasOutputPont = true;
}
}
//outside point
else
{
outsidePoints.Add(l0);
}
//detect intersections
if (signs[i] * signs[(i + 1) % l] == -1)
{
Vector3 ld = poly.GetPoint((i + 1) % l) - l0;
Vector3 intersection = Math3d.LinePlaneIntersection(l0, ld, p0, pn);
insidePoints.Add(intersection);
outsidePoints.Add(intersection);
if (signs[i] == -1)
{
outputPoint = intersection;
hasOutputPont = true;
}
}
}
//construct output
object point = null;
if (hasOutputPont)
{
point = outputPoint;
}
return(new object[] {
insidePoints.Count >= 3 ? constructor(poly, insidePoints) : null,
outsidePoints.Count >= 3 ? constructor(poly, outsidePoints) : null,
point
});
}
