private void PointInRelationToPlane(MyVector2 a, MyVector2 b, MyVector2 testPoint)
{
MyVector2 planeDir = b - a;
MyVector2 planeNormal = MyVector2.Normalize(new MyVector2(planeDir.y, -planeDir.x));
MyVector2 planePos = a + planeDir * 0.5f;
//Positive if infront, negative if behind
float distanceToPlane = _Geometry.DistanceFromPointToPlane(planeNormal, planePos, testPoint);
Debug.Log("Distance: " + distanceToPlane);
//Display
//Plane
Gizmos.color = Color.blue;
Gizmos.DrawLine(a.ToVector3(), b.ToVector3());
//Plane normal
Gizmos.DrawLine(planePos.ToVector3(), planePos.ToVector3() + planeNormal.ToVector3() * 0.1f);
//Point
Gizmos.color = Color.white;
Gizmos.DrawWireSphere(testPoint.ToVector3(), 0.1f);
}
//Help method to calculate the normal from two points
private static MyVector2 GetNormal(MyVector2 a, MyVector2 b)
{
MyVector2 lineDir = b - a;
//Flip x with y and set one to negative to get the normal
MyVector2 normal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
return(normal);
}
//Help method to calculate the average normal of two line segments
private static MyVector2 GetAverageNormal(MyVector2 a, MyVector2 b, MyVector2 c)
{
MyVector2 normal_1 = GetNormal(a, b);
MyVector2 normal_2 = GetNormal(b, c);
MyVector2 averageNormal = (normal_1 + normal_2) * 0.5f;
averageNormal = MyVector2.Normalize(averageNormal);
return(averageNormal);
}
//
// Arrow
//
public static HashSet <Triangle2> Arrow(MyVector2 p1, MyVector2 p2, float lineWidth, float arrowSize)
{
HashSet <Triangle2> arrowTriangles = new HashSet <Triangle2>();
//An arrow consists of two parts: the pointy part and the rectangular part
//First we have to see if we can fit the parts
MyVector2 lineDir = p2 - p1;
float lineLength = MyVector2.Magnitude(lineDir);
if (lineLength < arrowSize)
{
Debug.Log("Cant make arrow because line is too short");
return(null);
}
//Make the arrow tip
MyVector2 lineDirNormalized = MyVector2.Normalize(lineDir);
MyVector2 arrowBottom = p2 - lineDirNormalized * arrowSize;
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDirNormalized.y, -lineDirNormalized.x));
MyVector2 arrowBottom_R = arrowBottom + lineNormal * arrowSize * 0.5f;
MyVector2 arrowBottom_L = arrowBottom - lineNormal * arrowSize * 0.5f;
Triangle2 arrowTipTriangle = new Triangle2(p2, arrowBottom_R, arrowBottom_L);
arrowTriangles.Add(arrowTipTriangle);
//Make the arrow rectangle
float halfWidth = lineWidth * 0.5f;
MyVector2 p1_T = p1 + lineNormal * halfWidth;
MyVector2 p1_B = p1 - lineNormal * halfWidth;
MyVector2 p2_T = arrowBottom + lineNormal * halfWidth;
MyVector2 p2_B = arrowBottom - lineNormal * halfWidth;
HashSet <Triangle2> rectangle = LineSegment(p1_T, p1_B, p2_T, p2_B);
foreach (Triangle2 t in rectangle)
{
arrowTriangles.Add(t);
}
return(arrowTriangles);
}
//Help method to calculate the intersection point between two planes offset in normal direction by a width
private static MyVector2 GetIntersectionPoint(MyVector2 a, MyVector2 b, MyVector2 c, float halfWidth, bool isTopPoint)
{
//Direction of the lines going to and from point b
MyVector2 beforeDir = MyVector2.Normalize(b - a);
MyVector2 afterDir = MyVector2.Normalize(c - b);
MyVector2 beforeNormal = GetNormal(a, b);
MyVector2 afterNormal = GetNormal(b, c);
//Compare the normals!
//normalDirFactor is used to determine if we want to top point (same direction as normal)
float normalDirFactor = isTopPoint ? 1f : -1f;
//If they are the same it means we have a straight line and thus we cant do plane-plane intersection
//if (beforeNormal.Equals(afterNormal))
//When comparing the normals, we cant use the regular small value because then
//the line width goes to infinity when doing plane-plane intersection
float dot = MyVector2.Dot(beforeNormal, afterNormal);
//Dot is 1 if the point in the same dir and -1 if the point in the opposite dir
float one = 1f - 0.01f;
if (dot > one || dot < -one)
{
MyVector2 averageNormal = MyVector2.Normalize((afterNormal + beforeNormal) * 0.5f);
MyVector2 intersectionPoint = b + averageNormal * halfWidth * normalDirFactor;
return(intersectionPoint);
}
else
{
//Now we can calculate where the plane starts
MyVector2 beforePlanePos = b + beforeNormal * halfWidth * normalDirFactor;
MyVector2 afterPlanePos = b + afterNormal * halfWidth * normalDirFactor;
Plane2 planeBefore = new Plane2(beforePlanePos, beforeNormal);
Plane2 planeAfter = new Plane2(afterPlanePos, afterNormal);
//Calculate the intersection point
//We know they are intersecting, so we don't need to test that
MyVector2 intersectionPoint = _Intersections.GetPlanePlaneIntersectionPoint(planeBefore, planeAfter);
return(intersectionPoint);
}
}
//
// Line segment
//
public static HashSet <Triangle2> LineSegment(MyVector2 p1, MyVector2 p2, float width)
{
MyVector2 lineDir = p2 - p1;
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
//Bake in the width in the normal
lineNormal *= width * 0.5f;
MyVector2 p1_T = p1 + lineNormal;
MyVector2 p2_T = p2 + lineNormal;
MyVector2 p1_B = p1 - lineNormal;
MyVector2 p2_B = p2 - lineNormal;
HashSet <Triangle2> lineTriangles = LineSegment(p1_T, p1_B, p2_T, p2_B);
return(lineTriangles);
}
public double ErrorScore(List <MyVector2> points)
{
foreach (MyVector2 p in points)
{
mean += p;
}
mean /= points.Count;
double W = points.Count;
double[,] C = new double[2, 2];
foreach (MyVector2 point in points)
{
C[0, 0] += (point.x - mean.x) * (point.x - mean.x);
C[0, 1] += (point.x - mean.x) * (point.y - mean.y);
C[1, 0] += (point.y - mean.y) * (point.x - mean.x);
C[1, 1] += (point.y - mean.y) * (point.y - mean.y);
}
C[0, 0] /= W;
C[0, 1] /= W;
C[1, 0] /= W;
C[1, 1] /= W;
Matrix2d CM = new Matrix2d(C);
SVD svd = new SVD(C);
//svd.w - eigen value, start from 1
//svd.u - eigen vector, start from 1
int max = 1, min = 2;
if (svd.w[max] < svd.w[min])
{
int temp = max;
max = min;
min = temp;
}
double major = 2 * Math.Sqrt(svd.w[max]);
MyVector2 majoraxis = new MyVector2(svd.u[1, max], svd.u[2, max]);
majoraxis = majoraxis.Normalize();
double minor = 2 * Math.Sqrt(svd.w[min]);
MyVector2 minoraxis = new MyVector2(svd.u[1, min], svd.u[2, min]);
minoraxis = minoraxis.Normalize();
majorendp = mean + majoraxis * major;
majorstartp = mean - majoraxis * major;
minorendp = mean + minoraxis * minor;
minorstartp = mean - minoraxis * minor;
double error = Math.Abs(W - 4 * Math.PI * Math.Sqrt(CM.Det()));
error /= W;
Console.WriteLine("Like a ellipse error: {0}", error); //10^-2 may be a good threshold
return(error);
}
//
// Connected line segments
//
//isConnected means if the end points are connected to form a loop
public static HashSet <Triangle2> ConnectedLineSegments(List <MyVector2> points, float width, bool isConnected)
{
if (points != null && points.Count < 2)
{
Debug.Log("Cant form a line with fewer than two points");
return(null);
}
//Generate the triangles
HashSet <Triangle2> lineTriangles = new HashSet <Triangle2>();
//If the lines are connected we need to do plane-plane intersection to find the
//coordinate where the lines meet at each point, or the line segments will
//not get the same size
//(There might be a better way to do it than with plane-plane intersection)
List <MyVector2> topCoordinate = new List <MyVector2>();
List <MyVector2> bottomCoordinate = new List <MyVector2>();
float halfWidth = width * 0.5f;
for (int i = 0; i < points.Count; i++)
{
MyVector2 p = points[i];
//First point = special case if the lines are not connected
if (i == 0 && !isConnected)
{
MyVector2 lineDir = points[1] - points[0];
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
topCoordinate.Add(p + lineNormal * halfWidth);
bottomCoordinate.Add(p - lineNormal * halfWidth);
}
//Last point = special case if the lines are not connected
else if (i == points.Count - 1 && !isConnected)
{
MyVector2 lineDir = p - points[points.Count - 2];
MyVector2 lineNormal = MyVector2.Normalize(new MyVector2(lineDir.y, -lineDir.x));
topCoordinate.Add(p + lineNormal * halfWidth);
bottomCoordinate.Add(p - lineNormal * halfWidth);
}
else
{
//Now we need to find the intersection points between the top line and the bottom line
MyVector2 p_before = points[MathUtility.ClampListIndex(i - 1, points.Count)];
MyVector2 p_after = points[MathUtility.ClampListIndex(i + 1, points.Count)];
MyVector2 pTop = GetIntersectionPoint(p_before, p, p_after, halfWidth, isTopPoint: true);
MyVector2 pBottom = GetIntersectionPoint(p_before, p, p_after, halfWidth, isTopPoint: false);
topCoordinate.Add(pTop);
bottomCoordinate.Add(pBottom);
}
}
//Debug.Log();
for (int i = 0; i < points.Count; i++)
{
//Skip the first point if it is not connected to the last point
if (i == 0 && !isConnected)
{
continue;
}
int i_minus_one = MathUtility.ClampListIndex(i - 1, points.Count);
MyVector2 p1_T = topCoordinate[i_minus_one];
MyVector2 p1_B = bottomCoordinate[i_minus_one];
MyVector2 p2_T = topCoordinate[i];
MyVector2 p2_B = bottomCoordinate[i];
HashSet <Triangle2> triangles = LineSegment(p1_T, p1_B, p2_T, p2_B);
foreach (Triangle2 t in triangles)
{
lineTriangles.Add(t);
}
}
//Debug.Log(lineTriangles.Count);
return(lineTriangles);
}