示例#1
0
    // 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);
    }
示例#2
0
    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)));
    }