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