//private void EditorModeValueChanged(object sender, RoutedEventArgs e) //{ // RadioButton radioButton = sender as RadioButton; // if (radioButton.IsChecked.Value) // { // mouseSelector.EditorMode = true; // } // else // { // mouseSelector.EditorMode = false; // } // pathFinder.Reset(dragCanvas); //} private void GenerateButton_Click(object sender, RoutedEventArgs e) { if (!configuartionSpaceInitialized) { colliderContainer.GenerateAnglesArray(robot, dragCanvas, segmentsIntersector); IntializeConfigurationSpace(); configurationSpace.MarkUnreachableCells(colliderContainer.anglesArray); PostInitialize(); configuartionSpaceInitialized = true; SwitchToPathfindingButton.IsEnabled = true; return; } configurationSpace.Reset(); colliderContainer.Reset(dragCanvas); colliderContainer.GenerateAnglesArray(robot, dragCanvas, segmentsIntersector); configurationSpace.MarkUnreachableCells(colliderContainer.anglesArray); }
public void MoveRobot(Point start, Point end, Strategy strategy = Strategy.First) { //ShowSelectedPosition(start); //ShowSelectedPosition(end); Point startAngles = new Point(), endAngles = new Point(); if (strategy == Strategy.First) { startAngles = robot.CalculateInverseKinematicsFirst(start.X, start.Y); endAngles = robot.CalculateInverseKinematicsFirst(end.X, end.Y); } else if (strategy == Strategy.Second) { startAngles = robot.CalculateInverseKinematicsSecond(start.X, start.Y); endAngles = robot.CalculateInverseKinematicsSecond(end.X, end.Y); } else if (strategy == Strategy.Third) { startAngles = robot.CalculateInverseKinematicsFirst(start.X, start.Y); endAngles = robot.CalculateInverseKinematicsSecond(end.X, end.Y); } else if (strategy == Strategy.Fourth) { startAngles = robot.CalculateInverseKinematicsSecond(start.X, start.Y); endAngles = robot.CalculateInverseKinematicsFirst(end.X, end.Y); } if (Double.IsNaN(startAngles.X) || Double.IsNaN(startAngles.Y) || Double.IsNaN(endAngles.X) || Double.IsNaN(endAngles.Y)) { MessageBox.Show("Chosen points cannot be reached by robot."); return; } //Convert from radians to euler startAngles.X *= 180.0f / Math.PI; startAngles.Y *= 180.0f / Math.PI; endAngles.X *= 180.0f / Math.PI; endAngles.Y *= 180.0f / Math.PI; //transform angles to span from 0 to 360 startAngles.X = (int)(startAngles.X + 360) % 360; startAngles.Y = (int)(startAngles.Y + 360) % 360; endAngles.X = (int)(endAngles.X + 360) % 360; endAngles.Y = (int)(endAngles.Y + 360) % 360; configurationSpace.Reset(); configurationSpace.MarkUnreachableCells(colliderContainer.anglesArray); bool success = configurationSpace.FloodFill(startAngles, endAngles); if (success) { configurationSpace.ColorFill(); path = configurationSpace.TraverseBack(startAngles, endAngles); configurationSpace.Update(); linearInterpolator.Setup(path); linearInterpolator.startTime = DateTime.Now; timer.Start(); } else if (strategy == Strategy.First) { MoveRobot(start, end, Strategy.Second); } else if (strategy == Strategy.Second) { MoveRobot(start, end, Strategy.Third); } else if (strategy == Strategy.Third) { MoveRobot(start, end, Strategy.Fourth); } else { MessageBox.Show("Chosen points cannot be reached by robot."); } }