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 }); }