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);
}
}
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));
}
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 total = segs[segs.Length - 1].GetValue("time");
double rtime = total - path.FacingAngleStartDelay - path.FacingAngleEndDelay;
TrapezoidalProfile profile = CreateRotationProfile(robot, path.StartFacingAngle, path.EndFacingAngle, rtime, rotvel);
if (profile == null)
{
throw new DriveModifier.VelocitySplitException();
}
PathSegment[] fl = new PathSegment[segs.Length];
PathSegment[] fr = new PathSegment[segs.Length];
PathSegment[] bl = new PathSegment[segs.Length];
PathSegment[] br = new PathSegment[segs.Length];
result["fl"] = fl;
result["fr"] = fr;
result["bl"] = bl;
result["br"] = br;
for (int i = 0; i < segs.Length; i++)
{
//
// The time in the path
//
double time = segs[i].GetValue("time");
//
// The current facing angle based on the rotational trapezoidal profile
//
double current = XeroUtils.BoundDegrees(path.StartFacingAngle + profile.GetDistance(time - path.FacingAngleStartDelay));
//
// Get the required velocity to perform the rotation. This is the
// linear ground based velocity for the wheel that must be applied to
// each wheel in direction perpendicular to the vector from the center of
// the robot to the center of the wheel.
//
double rv = RotationalToGround(robot, profile.GetVelocity(time - path.FacingAngleStartDelay));
double ra = RotationalToGround(robot, profile.GetAcceleration(time - path.FacingAngleStartDelay));
//
// Get the velocity vector and acceleration vector relative to each of the
// wheels to rotate the robot based on the desired rotational velocity.
//
XeroVector rotflvel = GetWheelPerpendicularVector(robot, Wheel.FL, rv);
XeroVector rotfrvel = GetWheelPerpendicularVector(robot, Wheel.FR, rv);
XeroVector rotblvel = GetWheelPerpendicularVector(robot, Wheel.BL, rv);
XeroVector rotbrvel = GetWheelPerpendicularVector(robot, Wheel.BR, rv);
XeroVector rotflacc = GetWheelPerpendicularVector(robot, Wheel.FL, ra);
XeroVector rotfracc = GetWheelPerpendicularVector(robot, Wheel.FR, ra);
XeroVector rotblacc = GetWheelPerpendicularVector(robot, Wheel.BL, ra);
XeroVector rotbracc = GetWheelPerpendicularVector(robot, Wheel.BR, ra);
//
// Get the translational velocity vector to follow the path
//
XeroVector pathvel = XeroVector.FromDegreesMagnitude(segs[i].GetValue("heading"), segs[i].GetValue("velocity")).RotateDegrees(-current);
XeroVector pathacc = XeroVector.FromDegreesMagnitude(segs[i].GetValue("heading"), segs[i].GetValue("acceleration")).RotateDegrees(-current);
//
// For each wheel, the velocity vector is the sum of the rotational vector and the translational vector
//
XeroVector flv = rotflvel + pathvel;
XeroVector frv = rotfrvel + pathvel;
XeroVector blv = rotblvel + pathvel;
XeroVector brv = rotbrvel + pathvel;
XeroVector fla = rotflacc + pathvel;
XeroVector fra = rotfracc + pathvel;
XeroVector bla = rotblacc + pathvel;
XeroVector bra = rotbracc + pathvel;
//
// Now calculate the wheel positions based on the path position and the
// facing angle
//
XeroVector flpos = new XeroVector(robot.Length / 2.0, robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
XeroVector frpos = new XeroVector(robot.Length / 2.0, -robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
XeroVector blpos = new XeroVector(-robot.Length / 2.0, robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
XeroVector brpos = new XeroVector(-robot.Length / 2.0, -robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
PathSegment newseg = new PathSegment(segs[i]);
newseg.SetValue("x", flpos.X);
newseg.SetValue("y", flpos.Y);
newseg.SetValue("velocity", flv.Magnitude());
newseg.SetValue("heading", flv.AngleDegrees());
newseg.SetValue("acceleration", fla.Magnitude());
fl[i] = newseg;
newseg = new PathSegment(segs[i]);
newseg.SetValue("x", frpos.X);
newseg.SetValue("y", frpos.Y);
newseg.SetValue("velocity", frv.Magnitude());
newseg.SetValue("heading", frv.AngleDegrees());
newseg.SetValue("acceleration", fra.Magnitude());
fr[i] = newseg;
newseg = new PathSegment(segs[i]);
newseg.SetValue("x", blpos.X);
newseg.SetValue("y", blpos.Y);
newseg.SetValue("velocity", blv.Magnitude());
newseg.SetValue("heading", blv.AngleDegrees());
newseg.SetValue("acceleration", bla.Magnitude());
bl[i] = newseg;
newseg = new PathSegment(segs[i]);
newseg.SetValue("x", brpos.X);
newseg.SetValue("y", brpos.Y);
newseg.SetValue("velocity", brv.Magnitude());
newseg.SetValue("heading", brv.AngleDegrees());
newseg.SetValue("acceleration", bra.Magnitude());
br[i] = newseg;
}
return(result);
}
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);
}
