/***************************************************/
private static int CollapseToPolylineCount(this Arc curve, double angleTolerance, int maxSegmentCount = 100)
{
double angle = curve.Angle();
double factor = Math.Min(Math.PI * 0.25, Math.Max(angle * 0.5 / maxSegmentCount, angleTolerance));
return(System.Convert.ToInt32(Math.Ceiling(angle * 0.5 / factor)));
}
/***************************************************/
public static Vector Normal(this Arc arc)
{
if (arc.Angle() > 0)
{
return(arc.CoordinateSystem.Z);
}
else
{
return(arc.CoordinateSystem.Z.Reverse());
}
}
public static Vector Normal(this Arc curve, double tolerance = Tolerance.Distance)
{
if (curve.Angle() > 0)
{
return(curve.CoordinateSystem.Z);
}
else
{
return(curve.CoordinateSystem.Z.Reverse());
}
}
/***************************************************/
public static double Area(this PolyCurve curve)
{
if (curve.Curves.Count == 1 && curve.Curves[0] is Circle)
{
return((curve.Curves[0] as Circle).Area());
}
if (!curve.IsClosed())
{
return(0);
}
Plane p = curve.FitPlane();
if (p == null)
{
return(0.0); // points are collinear
}
Point sPt = curve.StartPoint();
double area = 0;
foreach (ICurve c in curve.SubParts())
{
if (c is NurbsCurve)
{
throw new NotImplementedException("Area of NurbsCurve is not imlemented yet so the area of this PolyCurve cannot be calculated");
}
Point ePt = c.IEndPoint();
Vector prod = CrossProduct(sPt - p.Origin, ePt - p.Origin);
area += prod * p.Normal * 0.5;
if (c is Arc)
{
Arc arc = c as Arc;
double radius = arc.Radius;
double angle = arc.Angle();
double arcArea = (angle - Math.Sin(angle)) * radius * radius * 0.5;
if (arc.CoordinateSystem.Z.DotProduct(p.Normal) > 0)
{
area += arcArea;
}
else
{
area -= arcArea;
}
}
sPt = ePt.Clone();
}
return(Math.Abs(area));
}
/***************************************************/
private static double IntegrateRegion(Arc arc, int powX, double tol = Tolerance.Distance)
{
Point centre = arc.CoordinateSystem.Origin;
double r = arc.Radius;
Point start = arc.StartPoint();
Point end = arc.EndPoint();
double a = Vector.XAxis.Angle(start - centre, Plane.XY);
double k = Math.Abs(arc.Angle());
if ((start - centre).CrossProduct(arc.StartDir()).Z < 0)
{
k *= -1;
}
switch (powX)
{
case 0:
return(
centre.X * r * (-Math.Sin(a) + Math.Sin(a + k)) +
(r * r * (2 * k - Math.Sin(2 * a) + Math.Sin(2 * (a + k)))
) / 4);
/********************/
case 1:
return((r * (
(Math.Sin(3 * (k + a)) + 9 * Math.Sin(k + a) -
Math.Sin(3 * a) - 9 * Math.Sin(a)) * r * r +
6 * (Math.Sin(2 * (k + a)) + 2 * k - Math.Sin(2 * a)) * centre.X * r +
12 * (Math.Sin(k + a) - Math.Sin(a)) * centre.X * centre.X
)) / 24);
/********************/
case 2:
return((r * (
(Math.Sin(4 * (k + a)) + 8 * Math.Sin(2 * (k + a)) + 12 * k - Math.Sin(4 * a) -
8 * Math.Sin(2 * a)) * r * r * r -
32 * (Math.Sin(k + a) * (Math.Sin(k + a) * Math.Sin(k + a) - 3) +
(Math.Cos(a) * Math.Cos(a) + 2) * Math.Sin(a)) * centre.X * r * r +
24 * (Math.Sin(2 * (k + a)) + 2 * k - Math.Sin(2 * a)) * centre.X * centre.X * r +
32 * (Math.Sin(k + a) - Math.Sin(a)) * centre.X * centre.X * centre.X
)) / 96);
/********************/
default:
return(IntegrateRegion(arc.CollapseToPolyline(0.01), powX, tol)); //TODO is this good value??
}
}
/***************************************************/
public static List <Point> LineIntersections(this Arc arc, Line line, bool useInfiniteLine = false, double tolerance = Tolerance.Distance)
{
Line l = line.DeepClone();
l.Infinite = useInfiniteLine ? true : l.Infinite;
List <Point> iPts = new List <Point>();
Point midPoint = arc.PointAtParameter(0.5);
Point center = arc.Centre();
//Check if curves are coplanar
if (Math.Abs(arc.CoordinateSystem.Z.DotProduct(l.Direction())) > Tolerance.Angle)
{
//Curves not coplanar
Point pt = l.PlaneIntersection((Plane)arc.CoordinateSystem);
if (pt != null && Math.Abs(pt.Distance(center) - arc.Radius) <= tolerance)
{
iPts.Add(pt);
}
}
else
{
//Curves coplanar
Circle c = new Circle {
Centre = center, Normal = arc.CoordinateSystem.Z, Radius = arc.Radius
};
iPts = c.LineIntersections(l);
}
List <Point> output = new List <Point>();
double halfAngle = arc.Angle() / 2;
double tolAngle = tolerance / arc.Radius;
double sqrd = 2 * Math.Pow(arc.Radius, 2) * (1 - Math.Cos(Math.Abs(halfAngle + tolAngle))); // Cosine rule
{
foreach (Point pt in iPts)
{
if ((l.Infinite || pt.Distance(l) <= tolerance) && midPoint.SquareDistance(pt) <= sqrd)
{
output.Add(pt);
}
}
}
return(output);
}
/***************************************************/
/**** Public Methods - Curves ****/
/***************************************************/
public static Point PointAtParameter(this Arc curve, double t)
{
if (t < 0)
{
t = 0;
}
if (t > 1)
{
t = 1;
}
double alfa = curve.Angle() * t + curve.StartAngle;
Vector localX = curve.CoordinateSystem.X;
return(curve.CoordinateSystem.Origin + localX.Rotate(alfa, curve.FitPlane().Normal) * curve.Radius);
}
/***************************************************/
/**** public Methods - Vectors ****/
/***************************************************/
public static List <Point> SortAlongCurve(this List <Point> points, Arc arc, double distanceTolerance = Tolerance.Distance, double angleTolerance = Tolerance.Angle)
{
if (arc.Angle() <= angleTolerance)
{
return(points.Select(p => p.Clone()).ToList());
}
List <Tuple <Point, double> > cData = points.Select(p => new Tuple <Point, double>(p.Clone(), arc.ParameterAtPoint(arc.ClosestPoint(p)))).ToList();
cData.Sort(delegate(Tuple <Point, double> d1, Tuple <Point, double> d2)
{
return(d1.Item2.CompareTo(d2.Item2));
});
return(cData.Select(d => d.Item1).ToList());
}
/***************************************************/
public static string ToSVGString(this Arc arc)
{
int largeArcFlag = System.Convert.ToInt32((arc.Angle() > Math.PI));
int sweepFlag = System.Convert.ToInt32(!arc.IsClockwise(Vector.ZAxis));
Point start = arc.StartPoint();
Point end = arc.EndPoint();
string arcString = "<path d=\"M" + start.X.ToString()
+ "," + start.Y.ToString()
+ " A" + arc.Radius() + "," + arc.Radius()
+ " 0"
+ " " + largeArcFlag
+ "," + sweepFlag
+ " " + end.X.ToString()
+ "," + end.Y.ToString() + "\"/>";
return(arcString);
}
/***************************************************/
/**** Public Methods - Curves ****/
/***************************************************/
public static double ParameterAtPoint(this Arc curve, Point point, double tolerance = Tolerance.Distance)
{
if (curve.ClosestPoint(point).SquareDistance(point) > tolerance * tolerance)
{
return(-1);
}
Point centre = curve.CoordinateSystem.Origin;
Vector normal = curve.CoordinateSystem.Z;
Vector v1 = curve.CoordinateSystem.X;
Vector v2 = point - centre;
double angle = v1.SignedAngle(v2, normal) - curve.StartAngle;
angle = Math.Abs(angle) < Tolerance.Angle ? 0 : angle; //Really small negative angles gives wrong result. This solves that problem.
return(((angle + 2 * Math.PI) % (2 * Math.PI)) / curve.Angle());
}
/***************************************************/
public static string ToSVGString(this Arc arc)
{
if (arc == null)
{
BH.Engine.Reflection.Compute.RecordError("Cannot convert a null arc to SVG string.");
return("");
}
int largeArcFlag = System.Convert.ToInt32((arc.Angle() > Math.PI));
int sweepFlag = System.Convert.ToInt32(!arc.IsClockwise(Vector.ZAxis));
Point start = arc.StartPoint();
Point end = arc.EndPoint();
string arcString = "<path d=\"M" + start.X.ToString()
+ "," + start.Y.ToString()
+ " A" + arc.Radius + "," + arc.Radius
+ " 0"
+ " " + largeArcFlag
+ "," + sweepFlag
+ " " + end.X.ToString()
+ "," + end.Y.ToString() + "\"/>";
return(arcString);
}
public static double Area(this PolyCurve curve, double tolerance = Tolerance.Distance)
{
if (curve == null)
{
BH.Engine.Reflection.Compute.RecordError("Cannot query area as the geometry is null.");
return(double.NaN);
}
if (curve.Curves.Count == 1 && curve.Curves[0] is Circle)
{
return((curve.Curves[0] as Circle).Area(tolerance));
}
if (!curve.IsClosed(tolerance))
{
Reflection.Compute.RecordWarning("Cannot calculate area for an open curve.");
return(0);
}
Plane p = curve.FitPlane(tolerance);
if (p == null)
{
return(0.0); // points are collinear
}
Point sPt = curve.StartPoint();
double area = 0;
foreach (ICurve c in curve.SubParts())
{
if (c is NurbsCurve)
{
Reflection.Compute.RecordError("Area for NurbsuCurve is not implemented.");
return(double.NaN);
}
Point ePt = c.IEndPoint();
Vector prod = CrossProduct(sPt - p.Origin, ePt - p.Origin);
area += prod * p.Normal * 0.5;
if (c is Arc)
{
Arc arc = c as Arc;
double radius = arc.Radius;
double angle = arc.Angle();
double arcArea = (angle - Math.Sin(angle)) * radius * radius * 0.5;
if (arc.CoordinateSystem.Z.DotProduct(p.Normal) > 0)
{
area += arcArea;
}
else
{
area -= arcArea;
}
}
sPt = ePt.DeepClone();
}
return(Math.Abs(area));
}
public static Arc RoundCoordinates(this Arc arc, int decimalPlaces = 6)
{
// do the rounding
Point start = arc.StartPoint().RoundCoordinates(decimalPlaces);
Point end = arc.EndPoint().RoundCoordinates(decimalPlaces);
double angle = arc.Angle();
double dist = start.Distance(end);
if (dist == 0)
{
// translate the origin as one of the points were, and set both angles to that ones angle
return(new Arc()
{
CoordinateSystem = arc.CoordinateSystem.Translate(start - arc.StartPoint()),
Radius = arc.Radius,
StartAngle = arc.StartAngle,
EndAngle = arc.StartAngle,
});
}
// recalculate the radius based on not changing the total angle
// Consider a equal legged triangle with endpoints at the arc's endpoints
// we know the "top" angle and the "base" length and are solving for the last two sides length
double radius = Math.Sqrt(
Math.Pow(dist / (2 * Math.Tan(angle / 2)), 2) + // "Height"
Math.Pow(dist / 2, 2) // "half the base"
);
// Align the normal to the new endpoints
Vector normal = arc.CoordinateSystem.Z.CrossProduct(end - start).CrossProduct(start - end).Normalise();
Circle startCircle = new Circle()
{
Normal = normal, Centre = start, Radius = radius
};
Circle endCircle = new Circle()
{
Normal = normal, Centre = end, Radius = radius
};
List <Point> intersections = startCircle.CurveIntersections(endCircle).OrderBy(x => x.SquareDistance(arc.CoordinateSystem.Origin)).ToList();
Point newOrigin = null;
// 180degrees arc where the points got rounded away from eachother
if (intersections.Count == 0)
{
newOrigin = (start + end) / 2;
radius = newOrigin.Distance(start);
}
else
{
// Ensure that the new centre is at the same side of the start/end points
Vector unitNormal = normal.Normalise();
unitNormal *= angle > Math.PI ? -1 : 1;
foreach (Point pt in intersections)
{
Vector temp = (start - pt).CrossProduct(end - pt).Normalise();
if ((temp + unitNormal).SquareLength() >= 1)
{
newOrigin = pt;
break;
}
}
}
Vector newX = (start - newOrigin).Normalise();
oM.Geometry.CoordinateSystem.Cartesian coordClone = Create.CartesianCoordinateSystem(newOrigin, newX, Query.CrossProduct(normal, newX));
double endAngle = (start - newOrigin).Angle(end - newOrigin);
endAngle = angle > Math.PI ? 2 * Math.PI - endAngle : endAngle;
Arc result = new Arc()
{
CoordinateSystem = coordClone,
Radius = radius,
StartAngle = 0,
EndAngle = endAngle,
};
return(result);
}
/***************************************************/
/**** Public Methods - Curves ****/
/***************************************************/
public static bool IsClosed(this Arc arc, double tolerance = Tolerance.Distance)
{
return((arc.Angle() - Math.PI * 2) * arc.Radius > -tolerance);
}
/***************************************************/
/**** Public Methods - Curves ****/
/***************************************************/
public static double Length(this Arc curve)
{
return(curve.Angle() * curve.Radius());
}
/***************************************************/
public static bool IsClockwise(this Arc arc, Vector axis, double tolerance = Tolerance.Distance)
{
Vector normal = arc.CoordinateSystem.Z;
return((normal.DotProduct(axis) < 0) != (arc.Angle() > Math.PI));
}
// Obstructions, must check line and arc intersection
override protected bool Obstructed(
IEnumerable <Polygon> polys, Vector3 startPos)
{
Vector3 newPoint = this.PredictPosition(startPos);
Vector2 sp = new Vector2(startPos.x, startPos.z);
Vector2 np = new Vector2(newPoint.x, newPoint.z);
// Length of the car
float L = KinematicCarState.L;
if (omega == 0.0f) // Check straight line intersection
{
Vector2 transVec = (np - sp).normalized * L;
Vector2 midPoint = (sp + np) / 2;
// TODO midPoint not necessary if you fix arc
Edge e = new Edge(sp, np + transVec); // TODO check correctness
foreach (Polygon p in polys)
{
if (p.Intersects(e) || p.IsInside(midPoint))
{
return(true);
}
}
}
else // Check arc intersection
// Center of turning circle
{
Vector3 center = startPos + centerOff;
Vector2 cp = new Vector2(center.x, center.z);
Vector2 cenToS = sp - cp;
Vector2 cenToN = np - cp;
// Angles of the arc
float a1, a2;
if (omega < 0)
{
a1 = Arc.Angle(Vector2.right, cenToS);
a2 = Arc.Angle(Vector2.right, cenToN);
}
else
{
a1 = Arc.Angle(Vector2.right, cenToN);
a2 = Arc.Angle(Vector2.right, cenToS);
}
/*Vector2 normal = Quaternion.Euler(0, 90, 0) * (sp - np);
* float na = (a1 + a2) / 2;
* Vector2 aaa2 = cp + normal * r;
* Vector2 aaa3 = cp - normal * r;
* float aaaa2 = Arc.Angle(Vector2.right, aaa2);
* float aaaa3 = Arc.Angle(Vector2.right, aaa3);
* Vector2 ffff = aaa2;
* if (a1 < a2) {
* if (aaaa2 > a1 && aaaa2 < a2) {
* ffff = aaa2;
* }
* if (aaaa3 > a1 && aaaa3 < a2) {
* ffff = aaa3;
* }
* } else {
* if (aaaa2 > a1 && aaaa2 < a2 && aaaa2 < 360 && aaaa2 > 0) {
* ffff = aaa2;
* }
* if (aaaa3 > a1 && aaaa3 < a2 && aaaa3 < 360 && aaaa3 > 0) {
* ffff = aaa3;
* }
* }*/
// Checking the front of the vehicle
Vector3 transVec = velocity.normalized * L;
Vector3 cenToFront = -centerOff + transVec;
float frontR = cenToFront.magnitude;
float diffAngle = Tangents.RotationAngle(-centerOff, cenToFront);
// Check if arc intersects with any of the polygons
Arc arc = new Arc(cp, r, a1, a2);
// TODO check correctness, if it is - or + diffAngle
Arc frontArc = new Arc(cp, frontR, a1 - diffAngle, a2 - diffAngle);
foreach (Polygon p in polys)
{
if (p.Intersects(arc) || p.Intersects(frontArc))
{
return(true);
}
}
}
return(false);
}
/***************************************************/
/**** Public Methods - Curves ****/
/***************************************************/
public static double Area(this Arc curve)
{
return(curve.IsClosed() ? curve.Angle() * Math.Pow(curve.Radius(), 2) : 0);
}
