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 DrawSpline(Graphics g, RobotPath path, int index) { float diam = 2.0f; using (Brush b = new SolidBrush(Color.LightCoral)) { double step = 1.0 / 1000.0; for (double t = 0.0; t < 1.0; t += step) { double x, y, heading; RectangleF rf; double px, py; PointF pt, w; path.Evaluate(index, t, out x, out y, out heading); heading = XeroUtils.DegreesToRadians(heading); double ca = Math.Cos(heading); double sa = Math.Sin(heading); px = x - m_file.Robot.Width * sa / 2.0; py = y + m_file.Robot.Width * ca / 2.0; pt = new PointF((float)px, (float)py); w = WorldToWindow(pt); rf = new RectangleF(w, new SizeF(0.0f, 0.0f)); rf.Offset(diam / 2.0f, diam / 2.0f); rf.Inflate(diam, diam); g.FillEllipse(b, rf); px = x + m_file.Robot.Width * sa / 2.0; py = y - m_file.Robot.Width * ca / 2.0; pt = new PointF((float)px, (float)py); w = WorldToWindow(pt); rf = new RectangleF(w, new SizeF(0.0f, 0.0f)); rf.Offset(diam / 2.0f, diam / 2.0f); rf.Inflate(diam, diam); g.FillEllipse(b, rf); } } }
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 void GenerateSpline(WayPoint p0, WayPoint p1, out Spline xsp, out Spline ysp) { double p0h = XeroUtils.DegreesToRadians(p0.Heading); double p1h = XeroUtils.DegreesToRadians(p1.Heading); double scale = 1.2 * p0.Distance(p1); double x0 = p0.X; double x1 = p1.X; double dx0 = Math.Cos(p0h) * scale; double dx1 = Math.Cos(p1h) * scale; double ddx0 = 0.0; double ddx1 = 0.0; double y0 = p0.Y; double y1 = p1.Y; double dy0 = Math.Sin(p0h) * scale; double dy1 = Math.Sin(p1h) * scale; double ddy0 = 0.0; double ddy1 = 0.0; double ax = -6 * x0 - 3 * dx0 - 0.5 * ddx0 + 0.5 * ddx1 - 3 * dx1 + 6 * x1; double bx = 15 * x0 + 8 * dx0 + 1.5 * ddx0 - ddx1 + 7 * dx1 - 15 * x1; double cx = -10 * x0 - 6 * dx0 - 1.5 * ddx0 + 0.5 * ddx1 - 4 * dx1 + 10 * x1; double dx = 0.5 * ddx0; double ex = dx0; double fx = x0; double ay = -6 * y0 - 3 * dy0 - 0.5 * ddy0 + 0.5 * ddy1 - 3 * dy1 + 6 * y1; double by = 15 * y0 + 8 * dy0 + 1.5 * ddy0 - ddy1 + 7 * dy1 - 15 * y1; double cy = -10 * y0 - 6 * dy0 - 1.5 * ddy0 + 0.5 * ddy1 - 4 * dy1 + 10 * y1; double dy = 0.5 * ddy0; double ey = dy0; double fy = y0; double dist = p0.Distance(p1); xsp = new Spline(dist, new double[] { fx, ex, dx, cx, bx, ax }); ysp = new Spline(dist, new double[] { fy, ey, dy, cy, by, ay }); }
private void DrawOneWaypoint(Graphics g, WayPoint pt, Pen selpen, Pen normpen, Brush selbrush, Brush normbrush) { PointF[] triangle = new PointF[] { new PointF(m_tw, 0.0f), new PointF(-m_tw / 2.0f, m_tw / 2.0f), new PointF(-m_tw / 2.0f, -m_tw / 2.0f) }; Matrix mm; mm = new Matrix(); mm.Translate((float)pt.X, (float)pt.Y); mm.Rotate((float)pt.Heading); mm.TransformPoints(triangle); float angle = (float)XeroUtils.DegreesToRadians(pt.Heading); // // Draw the point // PointF p = WorldToWindow(new PointF((float)pt.X, (float)pt.Y)); WorldToWindowMatrix.TransformPoints(triangle); g.FillPolygon(normbrush, triangle); if (m_selected == pt) { g.DrawPolygon(selpen, triangle); } else { g.DrawPolygon(normpen, triangle); } // // Draw the selection box // if (m_selected == pt) { PointF[] selbox = new PointF[] { new PointF((float)m_file.Robot.Length / 2.0f, (float)m_file.Robot.Width / 2.0f), new PointF(-(float)m_file.Robot.Length / 2.0f, (float)m_file.Robot.Width / 2.0f), new PointF(-(float)m_file.Robot.Length / 2.0f, -(float)m_file.Robot.Width / 2.0f), new PointF((float)m_file.Robot.Length / 2.0f, -(float)m_file.Robot.Width / 2.0f), }; PointF[] rotateline = new PointF[] { new PointF(0f, (float)m_file.Robot.Width / 2.0f), new PointF(0f, 3.0f * (float)m_file.Robot.Width / 4.0f), }; PointF[] circle = new PointF[] { new PointF(0f, 3.0f * (float)m_file.Robot.Width / 4.0f), new PointF(0f, (float)m_file.Robot.Width / 4.0f), }; mm.TransformPoints(selbox); WorldToWindowMatrix.TransformPoints(selbox); g.DrawPolygon(selpen, selbox); mm.TransformPoints(rotateline); WorldToWindowMatrix.TransformPoints(rotateline); g.DrawLine(selpen, rotateline[0], rotateline[1]); mm.TransformPoints(circle); WorldToWindowMatrix.TransformPoints(circle); RectangleF rectf = new RectangleF(circle[0], new SizeF(0, 0)); rectf.Inflate(5.0f, 5.0f); g.FillEllipse(selbrush, rectf); } }
public override Dictionary <string, PathSegment[]> ModifyPath(RobotParams robot, RobotPath path, PathSegment[] segs, double rotvel) { Dictionary <string, PathSegment[]> result = new Dictionary <string, PathSegment[]>(); if (segs == null) { return(null); } double lvel, lacc, ljerk, lpos = 0.0; double rvel, racc, rjerk, rpos = 0.0; double plx = 0.0, ply = 0.0; double prx = 0.0, pry = 0.0; double plvel = 0.0, prvel = 0.0; double placc = 0.0, pracc = 0.0; PathSegment[] lsegs = new PathSegment[segs.Length]; PathSegment[] rsegs = new PathSegment[segs.Length]; result["left"] = lsegs; result["right"] = rsegs; for (int i = 0; i < segs.Length; i++) { double time = segs[i].GetValue("time"); double heading = XeroUtils.DegreesToRadians(segs[i].GetValue("heading")); double ca = Math.Cos(heading); double sa = Math.Sin(heading); double px = segs[i].GetValue("x"); double py = segs[i].GetValue("y"); double lx = px - robot.Width * sa / 2.0; double ly = py + robot.Width * ca / 2.0; double rx = px + robot.Width * sa / 2.0; double ry = py - robot.Width * ca / 2.0; if (i == 0) { lvel = 0.0; lacc = 0.0; lpos = 0.0; ljerk = 0.0; rvel = 0.0; racc = 0.0; rpos = 0.0; rjerk = 0.0; } else { double dt = segs[i].GetValue("time") - segs[i - 1].GetValue("time"); double ldist = Math.Sqrt((lx - plx) * (lx - plx) + (ly - ply) * (ly - ply)); double rdist = Math.Sqrt((rx - prx) * (rx - prx) + (ry - pry) * (ry - pry)); lvel = ldist / dt; rvel = rdist / dt; lacc = (lvel - plvel) / dt; racc = (rvel - prvel) / dt; ljerk = (lacc - placc) / dt; rjerk = (racc - pracc) / dt; lpos += ldist; rpos += rdist; } PathSegment left = new PathSegment(); left.SetValue("time", time); left.SetValue("x", lx); left.SetValue("y", ly); left.SetValue("heading", segs[i].GetValue("heading")); left.SetValue("position", lpos); left.SetValue("velocity", lvel); left.SetValue("acceleration", lacc); left.SetValue("jerk", ljerk); lsegs[i] = left; PathSegment right = new PathSegment(); right.SetValue("time", segs[i].GetValue("time")); right.SetValue("x", rx); right.SetValue("y", ry); right.SetValue("heading", segs[i].GetValue("heading")); right.SetValue("position", rpos); right.SetValue("velocity", rvel); right.SetValue("acceleration", racc); right.SetValue("jerk", rjerk); rsegs[i] = right; plx = lx; ply = ly; prx = rx; pry = ry; plvel = lvel; prvel = rvel; placc = lacc; pracc = racc; } return(result); }