/// <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);
}
private void joystickMoved(DiamondPoint p)
{
//throw new NotImplementedException();
}
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));
}
}
}
/// <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;
}