// Generates four dubins
static public DubinCSC FindDubins(DirectionalCircle orgLeft,
DirectionalCircle orgRight,
DirectionalCircle dstLeft,
DirectionalCircle dstRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DubinCSC[] dubins)
{
dubins = new DubinCSC[4];
// RSR
///////
DubinCSC shortestDubins = dubins[0] = DubinCSC.RSRCourse(orgRight, dstRight, ptStart, ptDest, turnRadius);
// LSL
////////
dubins[1] = DubinCSC.LSLCourse(orgLeft, dstLeft, ptStart, ptDest, turnRadius);
shortestDubins = DubinCSC.TakeShortest(shortestDubins, dubins[1]);
// RSL
////////
dubins[2] = DubinCSC.RSLCourse(orgRight, dstLeft, ptStart, ptDest, turnRadius);
shortestDubins = DubinCSC.TakeShortest(shortestDubins, dubins[2]);
// LSR
///////
dubins[3] = DubinCSC.LSRCourse(orgLeft, dstRight, ptStart, ptDest, turnRadius);
shortestDubins = DubinCSC.TakeShortest(shortestDubins, dubins[3]);
return(shortestDubins);
}
public Arc(DirectionalCircle in_circle, float strtTheta, Vector2 ptEnd, float radius)
{
this.circle = in_circle;
this.circle.startTheta = strtTheta;
deltaAngle = this.circle.Angle(strtTheta, ptEnd);
this.endTheta = strtTheta + deltaAngle;
this.length = Mathf.Abs(deltaAngle * radius);
}
static public DubinCSC LSRCourse(
DirectionalCircle orgLeft,
DirectionalCircle dstRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
float x1 = orgLeft.center.x;
float y1 = orgLeft.center.y;
float x2 = dstRight.center.x;
float y2 = dstRight.center.y;
Vector2 v = dstRight.center - orgLeft.center;
float dSqr = v.sqrMagnitude;
if (dSqr < turnRadius)
{
// Circles overlap
return(NullDubin);
}
float d = Mathf.Sqrt(dSqr);
v /= d;
// For Sign1 = -1
float c = (turnRadius + turnRadius) / d;
if (c * c > 1.0f)
{
return(NullDubin);
}
float h = Mathf.Sqrt(1.0f - c * c);
// Sign1 = -1, Sign2 = -1
float nx = (v.x * c + h * v.y) * turnRadius;
float ny = (v.y * c - h * v.x) * turnRadius;
DubinCSC course = new DubinCSC();
course.line = (new Line2D()
{
start = new Vector2(x1 + nx, y1 + ny),
end = new Vector2(x2 - nx, y2 - ny)
});
course.sLength = d;
course.startArc = new Arc(orgLeft, orgLeft.startTheta, course.line.start, turnRadius);
course.endArc = new Arc(dstRight, course.line.end, ptDest, turnRadius);
course.totalLength = course.sLength + course.startArc.length + course.endArc.length;
return(course);
}
// This just cheats and calls the above function, used because it has the same function signature as
// the directional one
static public DubinCSC FindDegenerateDubin(DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DirectionalCircle dstLeft,
DirectionalCircle dstRight)
{
return(FindDegenerateDubins(orgLeft, orgRight, ptStart, ptDest, turnRadius));
}
static public DubinCSC RSRCourse(
DirectionalCircle orgRight,
DirectionalCircle dstRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
// Compute tangent line
float x1 = orgRight.center.x;
float y1 = orgRight.center.y;
float x2 = dstRight.center.x;
float y2 = dstRight.center.y;
Vector2 v = dstRight.center - orgRight.center;
float dSqr = v.sqrMagnitude;
if (dSqr < turnRadius)
{
// Circles overlap
return(NullDubin);
}
float d = Mathf.Sqrt(dSqr);
v /= d; // note to self, i believe this is overkill, and just there for circles of different radius, normal doesn't need to be computed.
// and i could get away with a simple x1,y1 + -vy,vx
// And I might be able to get away with checking each individial arc/line/arc distance separately
// Sign1 = 1, Sign2 = 1
float nx = /*v.x * c*/ -v.y;
float ny = /*v.y * c*/ +v.x;
nx *= turnRadius;
ny *= turnRadius;
DubinCSC course = new DubinCSC();
course.line = new Line2D()
{
start = new Vector2(x1 + nx, y1 + ny),
end = new Vector2(x2 + nx, y2 + ny)
};
course.sLength = d;
course.startArc = new Arc(orgRight, orgRight.startTheta, course.line.start, turnRadius);
course.endArc = new Arc(dstRight, course.line.end, ptDest, turnRadius);
course.totalLength = course.sLength + course.startArc.length + course.endArc.length;
return(course);
}
static public DubinCSC LSLCourse(
DirectionalCircle orgLeft,
DirectionalCircle dstLeft,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
float x1 = orgLeft.center.x;
float y1 = orgLeft.center.y;
float x2 = dstLeft.center.x;
float y2 = dstLeft.center.y;
Vector2 v = dstLeft.center - orgLeft.center;
float dSqr = v.sqrMagnitude;
if (dSqr < turnRadius)
{
// Circles overlap
return(NullDubin);
}
float d = Mathf.Sqrt(dSqr);
v /= d;
float nx = /*v.x * c*/ -v.y;
float ny = /*v.y * c*/ +v.x;
// Sign1 = 1, Sign2 = -1
nx = -nx * turnRadius;
ny = -ny * turnRadius;
DubinCSC course = new DubinCSC();
course.line = (new Line2D()
{
start = new Vector2(x1 + nx, y1 + ny),
end = new Vector2(x2 + nx, y2 + ny)
});
course.sLength = d;
course.startArc = new Arc(orgLeft, orgLeft.startTheta, course.line.start, turnRadius);
course.endArc = new Arc(dstLeft, course.line.end, ptDest, turnRadius);
course.totalLength = course.sLength + course.startArc.length + course.endArc.length;
return(course);
}
static public DubinCSC FindDubin(DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DirectionalCircle dstLeft,
DirectionalCircle dstRight)
{
// RSR
///////
DubinCSC shortestDubins = DubinCSC.RSRCourse(orgRight, dstRight, ptStart, ptDest, turnRadius);
// LSL
////////
shortestDubins = DubinCSC.TakeShortest(shortestDubins, DubinCSC.LSLCourse(orgLeft, dstLeft, ptStart, ptDest, turnRadius));
// RSL
////////
shortestDubins = DubinCSC.TakeShortest(shortestDubins, DubinCSC.RSLCourse(orgRight, dstLeft, ptStart, ptDest, turnRadius));
// LSR
///////
return(DubinCSC.TakeShortest(shortestDubins, DubinCSC.LSRCourse(orgLeft, dstRight, ptStart, ptDest, turnRadius)));
}
static public DubinCSC FindDegenerateDubins(
DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
Vector2 vLeft = orgLeft.center - ptDest;
Vector2 vRight = orgRight.center - ptDest;
DubinCSC dubinLS = new DubinCSC();
DubinCSC dubinRS = new DubinCSC();
float rSq = turnRadius * turnRadius;
if (rSq < vLeft.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vLeft.magnitude);
float b = Mathf.Atan2(vLeft.y, vLeft.x);
float t = b + a;
Vector2 Q = new Vector2(orgLeft.center.x + turnRadius * -Mathf.Sin(t),
orgLeft.center.y + turnRadius * Mathf.Cos(t));
// TODO: I think I'm doing one too many Atan2s here....
dubinLS.startArc = new Arc(orgLeft, orgLeft.startTheta, Q, turnRadius);
dubinLS.line.start = Q;
dubinLS.line.end = ptDest;
dubinLS.sLength = (dubinLS.line.end - dubinLS.line.start).magnitude;
dubinLS.endArc.startTheta = 0.0f;
dubinLS.endArc.endTheta = 0.0f;
dubinLS.endArc.length = 0.0f;
dubinLS.totalLength = dubinLS.startArc.length + dubinLS.sLength;
}
else
{
dubinLS.totalLength = NullDubin.totalLength;
}
if (rSq < vRight.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vRight.magnitude);
float b = Mathf.Atan2(vRight.y, vRight.x);
float t = b - a;
Vector2 Q = new Vector2(orgRight.center.x + turnRadius * Mathf.Sin(t),
orgRight.center.y + turnRadius * -Mathf.Cos(t));
dubinRS.startArc = new Arc(orgRight, orgRight.startTheta, Q, turnRadius);
dubinRS.line.start = Q;
dubinRS.line.end = ptDest;
dubinRS.sLength = (dubinRS.line.end - dubinRS.line.start).magnitude;
dubinRS.endArc.startTheta = 0.0f;
dubinRS.endArc.endTheta = 0.0f;
dubinRS.endArc.length = 0.0f;
dubinRS.totalLength = dubinRS.startArc.length + dubinRS.sLength;
}
else
{
dubinRS.totalLength = NullDubin.totalLength;
}
return(DubinCSC.TakeShortest(dubinLS, dubinRS));
}
// note: This form doesn't care about ending direction, just tries to get to the point
// Generates two dubins
static public DubinCSC FindDegenerateDubins(
DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DubinCSC[] dubins)
{
Vector2 vLeft = orgLeft.center - ptDest; // lines from center of circle to destination
Vector2 vRight = orgRight.center - ptDest;
DubinCSC dubinLS = NullDubin;
DubinCSC dubinRS = NullDubin;
float rSq = turnRadius * turnRadius; // this could probably be pulled out to a member or argument
if (rSq < vLeft.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vLeft.magnitude); // vLeft.Mag = h
float b = Mathf.Atan2(vLeft.y, vLeft.x);
float t = b + a;
Vector2 Q = new Vector2(orgLeft.center.x + turnRadius * -Mathf.Sin(t),
orgLeft.center.y + turnRadius * Mathf.Cos(t));
dubinLS = new DubinCSC();
dubinLS.startArc = new Arc(orgLeft, orgLeft.startTheta, Q, turnRadius);
dubinLS.line.start = Q;
dubinLS.line.end = ptDest;
dubinLS.sLength = (dubinLS.line.end - dubinLS.line.start).magnitude;
dubinLS.endArc.startTheta = 0.0f;
dubinLS.endArc.endTheta = 0.0f;
dubinLS.endArc.length = 0.0f;
dubinLS.totalLength = dubinLS.startArc.length + dubinLS.sLength;
}
if (rSq < vRight.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vRight.magnitude);
float b = Mathf.Atan2(vRight.y, vRight.x);
float t = b - a;
Vector2 Q = new Vector2(orgRight.center.x + turnRadius * Mathf.Sin(t),
orgRight.center.y + turnRadius * -Mathf.Cos(t));
dubinRS = new DubinCSC();
dubinRS.startArc = new Arc(orgRight, orgRight.startTheta, Q, turnRadius);
dubinRS.line.start = Q;
dubinRS.line.end = ptDest;
dubinRS.sLength = (dubinRS.line.end - dubinRS.line.start).magnitude;
dubinRS.endArc.startTheta = 0.0f;
dubinRS.endArc.endTheta = 0.0f;
dubinRS.endArc.length = 0.0f;
dubinRS.totalLength = dubinRS.startArc.length + dubinRS.sLength;
}
DubinCSC shortDubin = DubinCSC.TakeShortest(dubinLS, dubinRS);
dubins[0] = dubinLS;
dubins[1] = dubinRS;
return(shortDubin);
}
public Arc(DirectionalCircle in_circle, Vector2 ptEnd, float radius)
: this(in_circle, in_circle.startTheta, ptEnd, radius)
{
}
public Arc(DirectionalCircle in_circle, Vector2 ptStart, Vector2 ptEnd, float radius)
: this(in_circle, Mathf.Atan2(ptStart.y - in_circle.center.y, ptStart.x - in_circle.center.x), ptEnd, radius)
{
}
