Ejemplo n.º 1
0
        public static void Drive(Pose start, ReedsSheppActionSet actions, float unit)
        {
            Pose current = new Pose(start);

            foreach (ReedsSheppAction action in actions.Actions)
            {
                switch (action.Steer)
                {
                case Steer.Straight:
                    current = Straight(current, action.Gear, action.Length, unit);
                    break;

                case Steer.Left:
                    current = TurnLeft(current, action.Gear, action.Length, unit);
                    break;

                case Steer.Right:
                    current = TurnRight(current, action.Gear, action.Length, unit);
                    break;
                }

                if (Debug != null)
                {
                    current.DrawPose(Debug, 2, Color.LightBlue);
                }
            }
        }
Ejemplo n.º 2
0
 private static ReedsSheppActionSet timeflipTransform(ReedsSheppActionSet actions)
 {
     foreach (ReedsSheppAction a in actions.Actions)
     {
         a.Gear = a.Gear == Gear.Backward ? Gear.Forward : Gear.Backward;
     }
     return(actions);
 }
Ejemplo n.º 3
0
        private static ReedsSheppActionSet LfRfLbpath(float t, float u, float v)
        {
            ReedsSheppActionSet actions = new ReedsSheppActionSet();

            actions.AddAction(Steer.Left, Gear.Forward, t);
            actions.AddAction(Steer.Right, Gear.Forward, u);
            actions.AddAction(Steer.Left, Gear.Backward, v);
            return(actions);
        }
Ejemplo n.º 4
0
        private static ReedsSheppActionSet LfSfLfpi2Rbpath(float t, float u, float v)
        {
            ReedsSheppActionSet actions = new ReedsSheppActionSet();

            actions.AddAction(Steer.Left, Gear.Forward, t);
            actions.AddAction(Steer.Straight, Gear.Forward, u);
            actions.AddAction(Steer.Left, Gear.Forward, MathHelper.PiOver2);
            actions.AddAction(Steer.Right, Gear.Backward, v);
            return(actions);
        }
Ejemplo n.º 5
0
 private static ReedsSheppActionSet reflectTransform(ReedsSheppActionSet actions)
 {
     foreach (ReedsSheppAction a in actions.Actions)
     {
         if (a.Steer == Steer.Left)
         {
             a.Steer = Steer.Right;
         }
         else if (a.Steer == Steer.Right)
         {
             a.Steer = Steer.Left;
         }
     }
     return(actions);
 }
Ejemplo n.º 6
0
        public static ArrayList <Pose> Discretize(Pose start, ReedsSheppActionSet actions, float unit, float maxLength)
        {
            Pose             prev  = new Pose(start);
            ArrayList <Pose> poses = new ArrayList <Pose>();

            poses.Add(prev);

            foreach (ReedsSheppAction action in actions.Actions)
            {
                int n = (int)Math.Ceiling(action.Length * unit / maxLength);

                if (action.Steer != Steer.Straight)
                {
                    float pieceAngle = action.Length / n;

                    float phi    = pieceAngle / 2;
                    float sinPhi = (float)Math.Sin(phi);
                    float L      = 2 * sinPhi * unit;
                    float dx     = L * (float)Math.Cos(phi);
                    float dy     = L * sinPhi;

                    if (action.Steer == Steer.Right)
                    {
                        dy         = -dy;
                        pieceAngle = -pieceAngle;
                    }

                    if (action.Gear == Gear.Backward)
                    {
                        dx         = -dx;
                        pieceAngle = -pieceAngle;
                    }

                    for (int i = 0; i < n; i++)
                    {
                        Vector2 pos = new Vector2(dx, dy);
                        pos  = Vector2.Transform(pos, Matrix.CreateRotationZ(prev.Orientation));
                        prev = new Pose(pos + prev.Position, prev.Orientation + pieceAngle, action.Gear);
                        poses.Add(prev);
                    }
                }
                else
                {
                    float pieceLength = action.Length * unit / n;
                    float dx          = pieceLength * (float)Math.Cos(prev.Orientation);
                    float dy          = pieceLength * (float)Math.Sin(prev.Orientation);

                    if (action.Gear == Gear.Backward)
                    {
                        dx = -dx;
                        dy = -dy;
                    }

                    for (int i = 0; i < n; i++)
                    {
                        prev = new Pose(dx + prev.X, dy + prev.Y, prev.Orientation, action.Gear);
                        poses.Add(prev);
                    }
                }
            }

            return(poses);
        }
Ejemplo n.º 7
0
 private static ReedsSheppActionSet timeflipTransform(ReedsSheppActionSet actions)
 {
     foreach (ReedsSheppAction a in actions.Actions)
         a.Gear = a.Gear == Gear.Backward ? Gear.Forward : Gear.Backward;
     return actions;
 }
Ejemplo n.º 8
0
 private static ReedsSheppActionSet reflectTransform(ReedsSheppActionSet actions)
 {
     foreach (ReedsSheppAction a in actions.Actions)
         if (a.Steer == Steer.Left)
             a.Steer = Steer.Right;
         else if (a.Steer == Steer.Right)
             a.Steer = Steer.Left;
     return actions;
 }
Ejemplo n.º 9
0
 private static ReedsSheppActionSet LfSfRfpi2Lbpath(float t, float u, float v)
 {
     ReedsSheppActionSet actions = new ReedsSheppActionSet();
     actions.AddAction(Steer.Left, Gear.Forward, t);
     actions.AddAction(Steer.Straight, Gear.Forward, u);
     actions.AddAction(Steer.Right, Gear.Forward, MathHelper.PiOver2);
     actions.AddAction(Steer.Left, Gear.Backward, v);
     return actions;
 }
Ejemplo n.º 10
0
 private static ReedsSheppActionSet LfSfLfpath(float t, float u, float v)
 {
     ReedsSheppActionSet actions = new ReedsSheppActionSet();
     actions.AddAction(Steer.Left, Gear.Forward, t);
     actions.AddAction(Steer.Straight, Gear.Forward, u);
     actions.AddAction(Steer.Left, Gear.Forward, v);
     return actions;
 }
Ejemplo n.º 11
0
 private static ReedsSheppActionSet LfRufLubRbpath(float t, float u, float v)
 {
     ReedsSheppActionSet actions = new ReedsSheppActionSet();
     actions.AddAction(Steer.Left, Gear.Forward, t);
     actions.AddAction(Steer.Right, Gear.Forward, u);
     actions.AddAction(Steer.Left, Gear.Backward, u);
     actions.AddAction(Steer.Right, Gear.Backward, v);
     return actions;
 }
Ejemplo n.º 12
0
        public static ArrayList<Pose> Discretize(Pose start, ReedsSheppActionSet actions, float unit, float maxLength)
        {
            Pose prev = new Pose(start);
            ArrayList<Pose> poses = new ArrayList<Pose>();
            poses.Add(prev);

            foreach (ReedsSheppAction action in actions.Actions)
            {
                int n = (int)Math.Ceiling(action.Length * unit / maxLength);

                if (action.Steer != Steer.Straight)
                {
                    float pieceAngle = action.Length / n;

                    float phi = pieceAngle / 2;
                    float sinPhi = (float)Math.Sin(phi);
                    float L = 2 * sinPhi * unit;
                    float dx = L * (float)Math.Cos(phi);
                    float dy = L * sinPhi;

                    if (action.Steer == Steer.Right)
                    {
                        dy = -dy;
                        pieceAngle = -pieceAngle;
                    }

                    if (action.Gear == Gear.Backward)
                    {
                        dx = -dx;
                        pieceAngle = -pieceAngle;
                    }

                    for (int i = 0; i < n; i++)
                    {
                        Vector2 pos = new Vector2(dx, dy);
                        pos = Vector2.Transform(pos, Matrix.CreateRotationZ(prev.Orientation));
                        prev = new Pose(pos + prev.Position, prev.Orientation + pieceAngle, action.Gear);
                        poses.Add(prev);
                    }
                }
                else
                {
                    float pieceLength = action.Length * unit / n;
                    float dx = pieceLength * (float)Math.Cos(prev.Orientation);
                    float dy = pieceLength * (float)Math.Sin(prev.Orientation);

                    if (action.Gear == Gear.Backward)
                    {
                        dx = -dx;
                        dy = -dy;
                    }

                    for (int i = 0; i < n; i++)
                    {
                        prev = new Pose(dx + prev.X, dy + prev.Y, prev.Orientation, action.Gear);
                        poses.Add(prev);
                    }
                }
            }

            return poses;
        }
Ejemplo n.º 13
0
        public static void Drive(Pose start, ReedsSheppActionSet actions, float unit)
        {
            Pose current = new Pose(start);
            foreach (ReedsSheppAction action in actions.Actions)
            {
                switch (action.Steer)
                {
                    case Steer.Straight:
                        current = Straight(current, action.Gear, action.Length, unit);
                        break;

                    case Steer.Left:
                        current = TurnLeft(current, action.Gear, action.Length, unit);
                        break;

                    case Steer.Right:
                        current = TurnRight(current, action.Gear, action.Length, unit);
                        break;
                }

                if (Debug != null)
                    current.DrawPose(Debug, 2, Color.LightBlue);
            }
        }