Beispiel #1
0
        /// <summary>
        ///     CartesianToDiamond is the main method in DiamondToolbox class.
        ///     This method converts a Cartesian coordinate into a Diamond coordinate.
        /// </summary>
        /// <param name="cartPoint">Cartesian coordiant in DoublePoint type</param>
        /// <param name="radius">Maximum Cartesian value positive or negative on either axis in double type</param>
        /// <returns>Diamond coordinate in DiamondPoint type</returns>
        public static DiamondPoint CartesianToDiamond(DoublePoint cartPoint, double radius)
        {
            DoublePoint[] diamondPolygon =
            {
                new DoublePoint(0,       radius),
                new DoublePoint(radius,        0),
                new DoublePoint(0,       -radius),
                new DoublePoint(-radius, 0)
            };

            // If the Cartesian point is outside of the diamond ...
            if (!IsInsidePolygon(diamondPolygon, cartPoint))
            {
                // If the Cartesian point is on the Y axis ...
                if (cartPoint.X == 0)
                {
                    // If the Cartesian point if North of the Diamond ...
                    if (cartPoint.Y > 0)
                    {
                        // Bring it back to the North most point
                        cartPoint.Y = radius;
                    }
                    else // If the Cartesian point is South of the Diamond ...
                         // Bring it back to the South most point
                    {
                        cartPoint.Y = -radius;
                    }
                }
                // If the Cartesian point is on the X axis ...
                else if (cartPoint.Y == 0)
                {
                    // If the Cartesian point if East of the Diamond ...
                    if (cartPoint.X > 0)
                    {
                        // Bring it back to the East most point
                        cartPoint.X = radius;
                    }
                    else // If the Cartesian point is West of the Diamond ...
                         // Bring it back to the West most point
                    {
                        cartPoint.X = -radius;
                    }
                }
                else
                // If the Cartesian point is in the Northeast ...
                if (cartPoint.X > 0 && cartPoint.Y > 0)
                {
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint,                   // line from test point
                        new DoublePoint(0, 0),       //   to center
                        new DoublePoint(0, radius),  // Line from North point
                        new DoublePoint(radius, 0)); //   to East point
                }
                // If the Cartesian point is in the Southeast ...
                else if (cartPoint.X > 0 && cartPoint.Y < 0)
                {
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint,                   // line from test point
                        new DoublePoint(0, 0),       //   to center
                        new DoublePoint(0, -radius), // Line from South point
                        new DoublePoint(radius, 0)); //   to East point
                }
                // If the Cartesian point is in the Southwest ...
                else if (cartPoint.X < 0 && cartPoint.Y < 0)
                {
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint,                    // line from test point
                        new DoublePoint(0, 0),        //   to center
                        new DoublePoint(0, -radius),  // Line from South point
                        new DoublePoint(-radius, 0)); //   to West point
                }
                // If the Cartesian point is in the Northwest ...
                else
                {
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint,                    // line from test point
                        new DoublePoint(0, 0),        //   to center
                        new DoublePoint(0, radius),   // Line from North point
                        new DoublePoint(-radius, 0)); //   to West point
                }
            }
            // Now that we have migrated the Cartesian point to the diamond ...

            // Calculate the left axis diamond value of the Cartesian point

            // Create a ray from the Cartesian point at a 45 degree angle through
            //   the upper left side of the diamond
            DoublePoint leftOuterEnd =
                new DoublePoint(cartPoint.X - (2 * radius), cartPoint.Y + (2 * radius));
            // Get the point on the ray that intesects the upper left diamond edge
            DoublePoint leftIntersect = GetIntersection(
                cartPoint,                    // Ray from Cartesian point
                leftOuterEnd,                 //   beyond the diamond at 45 degrees Northwest
                new DoublePoint(0, radius),   // Line from North point
                new DoublePoint(-radius, 0)); //   to West point
            // Use the Cartesian Y values to find the diamond scale values
            double leftScale = (leftIntersect.Y - (radius / 2)) * 2;

            // Calculate the right axis diamond value of the Cartesian point

            // Create a ray from the Cartesian point at a 45 degree angle through
            //   the upper right side of the diamond
            DoublePoint rightOuterEnd =
                new DoublePoint(cartPoint.X + (2 * radius), cartPoint.Y + (2 * radius));
            // Get the point on the ray that intesects the upper right diamond edge
            DoublePoint rightIntersect = GetIntersection(
                cartPoint,                   // Ray from Cartesian point
                rightOuterEnd,               //   beyond the diamond at 45 degrees Northeast
                new DoublePoint(0, radius),  // Line from North point
                new DoublePoint(radius, 0)); //   to East point
            // Use the Cartesian Y values to find the diamond scale values
            double rightScale = (rightIntersect.Y - (radius / 2)) * 2;

            // Build the return value
            DiamondPoint ret = new DiamondPoint(leftScale, rightScale);

            return(ret);
        }
Beispiel #2
0
 private void joystickMoved(DiamondPoint p)
 {
     //throw new NotImplementedException();
 }
Beispiel #3
0
        void _selectedController_StateChanged(object sender, XboxControllerStateChangedEventArgs e)
        {
            OnPropertyChanged("SelectedController");

            var p = new DoublePoint(-SelectedController.RightThumbStick.X,
                SelectedController.RightThumbStick.Y);

            System.Diagnostics.Trace.WriteLine(string.Format("game x({0}) y({1})", p.X, p.Y));

            if (Math.Abs(p.X) > 10000 || Math.Abs(p.Y) > 10000)
            {
                changeDebounce = 0;

                DiamondPoint = DiamondToolbox.CartesianToDiamond(p, 32767);
                DiamondPoint.Left = Scale(DiamondPoint.Left, -32767, 32767, -100, 100);
                DiamondPoint.Right = Scale(DiamondPoint.Right, -32767, 32767, -100, 100);

                System.Diagnostics.Trace.WriteLine(string.Format("DiamondPoint left({0}) right({1})",
                    DiamondPoint.Left, DiamondPoint.Right));

                string jsn = JsonConvert.SerializeObject(new { Cmd = "MOV", M1 = (float)DiamondPoint.Left, M2 = (float)DiamondPoint.Right });
                if (Mqtt?.IsConnected ?? false)
                    Mqtt.Publish("robot1/Cmd", UTF8Encoding.ASCII.GetBytes(jsn));
            }
            else
            {
                if (++changeDebounce < 10 )
                {
                    string jsn = JsonConvert.SerializeObject(new { Cmd = "MOV", M1 = (float)0, M2 = (float)0 });
                    if (Mqtt?.IsConnected ?? false)
                        Mqtt.Publish("robot1/Cmd", UTF8Encoding.ASCII.GetBytes(jsn));
                }
            }
        }
Beispiel #4
0
        /// <summary>
        ///     CartesianToDiamond is the main method in DiamondToolbox class.
        ///     This method converts a Cartesian coordinate into a Diamond coordinate.
        /// </summary>
        /// <param name="cartPoint">Cartesian coordiant in DoublePoint type</param>
        /// <param name="radius">Maximum Cartesian value positive or negative on either axis in double type</param>
        /// <returns>Diamond coordinate in DiamondPoint type</returns>
        public static DiamondPoint CartesianToDiamond(DoublePoint cartPoint, double radius)
        {
            DoublePoint[] diamondPolygon =
            {
                new DoublePoint(0, radius),
                new DoublePoint(radius, 0),
                new DoublePoint(0, -radius),
                new DoublePoint(-radius, 0)
            };

            // If the Cartesian point is outside of the diamond ...
            if (!IsInsidePolygon(diamondPolygon, cartPoint))
                // If the Cartesian point is on the Y axis ...
                if (cartPoint.X == 0)
                    // If the Cartesian point if North of the Diamond ...
                    if (cartPoint.Y > 0)
                        // Bring it back to the North most point
                        cartPoint.Y = radius;
                    else // If the Cartesian point is South of the Diamond ...
                        // Bring it back to the South most point
                        cartPoint.Y = -radius;
                // If the Cartesian point is on the X axis ...
                else if (cartPoint.Y == 0)
                    // If the Cartesian point if East of the Diamond ...
                    if (cartPoint.X > 0)
                        // Bring it back to the East most point
                        cartPoint.X = radius;
                    else // If the Cartesian point is West of the Diamond ...
                        // Bring it back to the West most point
                        cartPoint.X = -radius;
                else
                    // If the Cartesian point is in the Northeast ...
                    if (cartPoint.X > 0 && cartPoint.Y > 0)
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint, // line from test point
                        new DoublePoint(0, 0), //   to center
                        new DoublePoint(0, radius), // Line from North point
                        new DoublePoint(radius, 0)); //   to East point
                                                     // If the Cartesian point is in the Southeast ...
                else if (cartPoint.X > 0 && cartPoint.Y < 0)
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint, // line from test point
                        new DoublePoint(0, 0), //   to center
                        new DoublePoint(0, -radius), // Line from South point
                        new DoublePoint(radius, 0)); //   to East point
                                                     // If the Cartesian point is in the Southwest ...
                else if (cartPoint.X < 0 && cartPoint.Y < 0)
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint, // line from test point
                        new DoublePoint(0, 0), //   to center
                        new DoublePoint(0, -radius), // Line from South point
                        new DoublePoint(-radius, 0)); //   to West point
                                                      // If the Cartesian point is in the Northwest ...
                else
                    // Move the Cartesian point to the diamond in
                    //   the direction of the center
                    //   This could throw an Exception, but it shouldn't
                    cartPoint = GetIntersection(
                        cartPoint, // line from test point
                        new DoublePoint(0, 0), //   to center
                        new DoublePoint(0, radius), // Line from North point
                        new DoublePoint(-radius, 0)); //   to West point

            // Now that we have migrated the Cartesian point to the diamond ...

            // Calculate the left axis diamond value of the Cartesian point

            // Create a ray from the Cartesian point at a 45 degree angle through
            //   the upper left side of the diamond
            DoublePoint leftOuterEnd =
                new DoublePoint(cartPoint.X - (2 * radius), cartPoint.Y + (2 * radius));
            // Get the point on the ray that intesects the upper left diamond edge
            DoublePoint leftIntersect = GetIntersection(
                cartPoint, // Ray from Cartesian point
                leftOuterEnd, //   beyond the diamond at 45 degrees Northwest
                new DoublePoint(0, radius), // Line from North point
                new DoublePoint(-radius, 0)); //   to West point
            // Use the Cartesian Y values to find the diamond scale values
            double leftScale = (leftIntersect.Y - (radius / 2)) * 2;

            // Calculate the right axis diamond value of the Cartesian point

            // Create a ray from the Cartesian point at a 45 degree angle through
            //   the upper right side of the diamond
            DoublePoint rightOuterEnd =
                new DoublePoint(cartPoint.X + (2 * radius), cartPoint.Y + (2 * radius));
            // Get the point on the ray that intesects the upper right diamond edge
            DoublePoint rightIntersect = GetIntersection(
                cartPoint, // Ray from Cartesian point
                rightOuterEnd, //   beyond the diamond at 45 degrees Northeast
                new DoublePoint(0, radius), // Line from North point
                new DoublePoint(radius, 0)); //   to East point
            // Use the Cartesian Y values to find the diamond scale values
            double rightScale = (rightIntersect.Y - (radius / 2)) * 2;

            // Build the return value
            DiamondPoint ret = new DiamondPoint(leftScale, rightScale);

            return ret;
        }