public void Evaluate(int index, double t, out double x, out double y, out double heading)
{
if (m_splines[index].Item2 == null)
{
Spline s = m_splines[index].Item1;
double tx = t * s.KnotDistance;
double ty = s.Evaluate(tx);
double ca = Math.Cos(XeroUtils.DegreesToRadians(s.AngleOffset));
double sa = Math.Sin(XeroUtils.DegreesToRadians(s.AngleOffset));
x = tx * ca - ty * sa + s.XOffset;
y = tx * sa + ty * ca + s.YOffset;
heading = XeroUtils.RadiansToDegrees(XeroUtils.BoundRadians(Math.Atan(s.Derivative(tx)) + XeroUtils.DegreesToRadians(s.AngleOffset)));;
}
else
{
//
// For path generators that generate independent splines for the X and Y coordinate
//
x = m_splines[index].Item1.Evaluate(t);
y = m_splines[index].Item2.Evaluate(t);
double dx = m_splines[index].Item1.Derivative(t);
double dy = m_splines[index].Item2.Derivative(t);
heading = XeroUtils.RadiansToDegrees(Math.Atan2(dy, dx));
}
}
private void GenerateSpline(WayPoint p0, WayPoint p1, out Spline xsp, out Spline ysp)
{
double dist = p0.Distance(p1);
double aoffset = Math.Atan2(p1.Y - p0.Y, p1.X - p0.X);
double a0_delta = Math.Tan(XeroUtils.BoundRadians(XeroUtils.DegreesToRadians(p0.Heading) - aoffset));
double a1_delta = Math.Tan(XeroUtils.BoundRadians(XeroUtils.DegreesToRadians(p1.Heading) - aoffset));
double[] coeffs = new double[6];
coeffs[5] = -(3 * (a0_delta + a1_delta)) / (dist * dist * dist * dist);
coeffs[4] = (8 * a0_delta + 7 * a1_delta) / (dist * dist * dist);
coeffs[3] = -(6 * a0_delta + 4 * a1_delta) / (dist * dist);
coeffs[2] = 0.0;
coeffs[1] = a0_delta;
coeffs[0] = 0.0;
xsp = new Spline(dist, coeffs);
xsp.XOffset = p0.X;
xsp.YOffset = p0.Y;
xsp.AngleOffset = XeroUtils.RadiansToDegrees(aoffset);
ysp = null;
}
private double FindAngle(PointF first, PointF second)
{
double angle = Math.Atan2(second.Y - first.Y, second.X - first.X);
return(XeroUtils.BoundDegrees(XeroUtils.RadiansToDegrees(angle) - 90.0));
}