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