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