private void ShowSelectedPosition(Point p) { robot.CalculateInverseKinematicsSecond(p.X, p.Y); robot.Reset(dragCanvas); IList <Line> lines = robot.GetLines(); foreach (var line in lines) { line.Stroke = Brushes.Blue; } dragCanvas.Children.Add(lines[0]); dragCanvas.Children.Add(lines[1]); }
public void FindSolutions(Point targetPoint, SegmentsIntersector segmentsIntersector, Canvas dragCanvas, Strategy strategy) { Point targetAngles = new Point(); if (strategy == Strategy.First) { targetAngles = robot.CalculateInverseKinematicsFirst(targetPoint.X, targetPoint.Y); } else if (strategy == Strategy.Second) { targetAngles = robot.CalculateInverseKinematicsSecond(targetPoint.X, targetPoint.Y); } if (Double.IsNaN(targetAngles.X) || Double.IsNaN(targetAngles.Y)) { //target point is unreachable from provided position return; } robot.Reset(dragCanvas); bool isCollision = colliderContainer.CheckCollision(robot, segmentsIntersector); if (isCollision) { //found configuration is in collision with colliders return; } IList <Line> lines = robot.GetLines(); foreach (var line in lines) { line.Stroke = Brushes.Black; } if (strategy == Strategy.First) { if (FirstLineSolution1 != null && SecondLineSolution1 != null) { dragCanvas.Children.Remove(FirstLineSolution1); dragCanvas.Children.Remove(SecondLineSolution1); } FirstLineSolution1 = lines[0]; SecondLineSolution1 = lines[1]; } else if (strategy == Strategy.Second) { if (FirstLineSolution2 != null && SecondLineSolution2 != null) { dragCanvas.Children.Remove(FirstLineSolution2); dragCanvas.Children.Remove(SecondLineSolution2); } FirstLineSolution2 = lines[0]; SecondLineSolution2 = lines[1]; } else if (strategy == Strategy.Third) { if (FirstLineSolution1 != null && SecondLineSolution2 != null) { dragCanvas.Children.Remove(FirstLineSolution1); dragCanvas.Children.Remove(SecondLineSolution2); } FirstLineSolution1 = lines[0]; SecondLineSolution2 = lines[1]; } else if (strategy == Strategy.Fourth) { if (FirstLineSolution2 != null && SecondLineSolution1 != null) { dragCanvas.Children.Remove(FirstLineSolution2); dragCanvas.Children.Remove(SecondLineSolution1); } FirstLineSolution2 = lines[0]; SecondLineSolution1 = lines[1]; } dragCanvas.Children.Add(lines[0]); dragCanvas.Children.Add(lines[1]); }