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