public bool CheckCollision(Robot robot, SegmentsIntersector segmentsIntersector)
{
Line firstLine = robot.GetLines()[0];
Line secondLine = robot.GetLines()[1];
bool sentinel = false;
foreach (var extendedRectangle in Colliders)
{
foreach (var line in extendedRectangle.GetLines())
{
bool intersectionWithFirst = segmentsIntersector.Intersect(firstLine, line);
bool intersectionWithSecond = segmentsIntersector.Intersect(secondLine, line);
if (intersectionWithFirst || intersectionWithSecond)
{
sentinel = true;
break;
}
}
if (sentinel)
{
break;
}
}
if (sentinel)
{
return(true);
}
return(false);
}
public bool CheckCollision(Robot robot, SegmentsIntersector segmentsIntersector)
{
Line firstLine = robot.GetLines()[0];
Line secondLine = robot.GetLines()[1];
bool sentinel = false;
foreach (var extendedRectangle in Colliders)
{
foreach (var line in extendedRectangle.GetLines())
{
bool intersectionWithFirst = segmentsIntersector.Intersect(firstLine, line);
bool intersectionWithSecond = segmentsIntersector.Intersect(secondLine, line);
if (intersectionWithFirst || intersectionWithSecond)
{
sentinel = true;
break;
}
}
if (sentinel)
{
break;
}
}
if (sentinel)
{
return true;
}
return false;
}
public void GenerateAnglesArray(Robot robot, Canvas Canvas, SegmentsIntersector segmentsIntersector)
{
int positiveCounter = 0;
for (int i = 0; i < maxAngle; i++)
{
for (int j = 0; j < maxAngle; j++)
{
robot.UpdateAngles(i, j);
robot.Reset(Canvas);
Line firstLine = robot.GetLines()[0];
Line secondLine = robot.GetLines()[1];
bool sentinel = false;
foreach (var extendedRectangle in Colliders)
{
foreach (var line in extendedRectangle.GetLines())
{
bool intersectionWithFirst = segmentsIntersector.Intersect(firstLine, line);
bool intersectionWithSecond = segmentsIntersector.Intersect(secondLine, line);
if (intersectionWithFirst || intersectionWithSecond)
{
sentinel = true;
break;
}
}
if (sentinel)
{
break;
}
}
if (sentinel)
{
anglesArray[i, j] = false;
}
else
{
anglesArray[i, j] = true;
positiveCounter++;
if (i % 4 == 0 && j % 2 == 0)
{
//firstLine.Stroke = new SolidColorBrush() {Color = Color.FromArgb(50, 0, 255, 0)};
secondLine.Stroke = new SolidColorBrush()
{
Color = Color.FromArgb(50, 0, 0, 200)
};
//lineContainer.Add(firstLine);
secondLine.X1 = secondLine.X2 - 1;
secondLine.Y1 = secondLine.Y2 - 1;
lineContainer.Add(secondLine);
Canvas.Children.Add(secondLine);
}
}
}
}
int d = 10;
}
public void GenerateAnglesArray(Robot robot, Canvas Canvas, SegmentsIntersector segmentsIntersector)
{
int positiveCounter = 0;
for (int i = 0; i < maxAngle; i++)
for (int j = 0; j < maxAngle; j++)
{
robot.UpdateAngles(i, j);
robot.Reset(Canvas);
Line firstLine = robot.GetLines()[0];
Line secondLine = robot.GetLines()[1];
bool sentinel = false;
foreach (var extendedRectangle in Colliders)
{
foreach (var line in extendedRectangle.GetLines())
{
bool intersectionWithFirst = segmentsIntersector.Intersect(firstLine, line);
bool intersectionWithSecond = segmentsIntersector.Intersect(secondLine, line);
if (intersectionWithFirst || intersectionWithSecond)
{
sentinel = true;
break;
}
}
if (sentinel)
{
break;
}
}
if (sentinel)
{
anglesArray[i, j] = false;
}
else
{
anglesArray[i, j] = true;
positiveCounter++;
if (i%4 == 0 && j % 2 == 0)
{
//firstLine.Stroke = new SolidColorBrush() {Color = Color.FromArgb(50, 0, 255, 0)};
secondLine.Stroke = new SolidColorBrush() {Color = Color.FromArgb(50, 0, 0, 200)};
//lineContainer.Add(firstLine);
secondLine.X1 = secondLine.X2 - 1;
secondLine.Y1 = secondLine.Y2 - 1;
lineContainer.Add(secondLine);
Canvas.Children.Add(secondLine);
}
}
}
int d = 10;
}
private void PreInitialize()
{
EditorMode = true;
AnimationTime = 1500;
L1 = 200;
L2 = 100;
segmentsIntersector = new SegmentsIntersector();
mouseSelector = new MouseSelector();
mouseSelector.EditorMode = EditorMode;
linearInterpolator = new LinearInterpolator();
timer = new DispatcherTimer();
timer.Interval = TimeSpan.FromMilliseconds(10);
timer.Tick += TimerOnTick;
timer.Stop();
}
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]);
}
public void PresentPossibleSolutions(Point targetPoint, SegmentsIntersector segmentsIntersector, Canvas dragCanvas)
{
Reset(dragCanvas);
FindSolutions(targetPoint, segmentsIntersector, dragCanvas, Strategy.First);
FindSolutions(targetPoint, segmentsIntersector, dragCanvas, Strategy.Second);
}
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]);
}
public void PresentPossibleSolutions(Point targetPoint, SegmentsIntersector segmentsIntersector, Canvas dragCanvas)
{
Reset(dragCanvas);
FindSolutions(targetPoint, segmentsIntersector, dragCanvas, Strategy.First);
FindSolutions(targetPoint, segmentsIntersector, dragCanvas, Strategy.Second);
}
private void PreInitialize()
{
EditorMode = true;
AnimationTime = 1500;
L1 = 200;
L2 = 100;
segmentsIntersector = new SegmentsIntersector();
mouseSelector = new MouseSelector();
mouseSelector.EditorMode = EditorMode;
linearInterpolator = new LinearInterpolator();
timer = new DispatcherTimer();
timer.Interval = TimeSpan.FromMilliseconds(10);
timer.Tick += TimerOnTick;
timer.Stop();
}