public void StartSimulation()
{
TurnOnAnimation();
// CollisionCheck();
GetObstaclesInConfigurationSpace();
SavedRobotCopy = Robot1.Clone();
timer = new DispatcherTimer(DispatcherPriority.Render);
pathFrameNumber = Path.Count() - 1;
if (Path.Count > 1)
{
Status2 = "";
timer.Interval = TimeSpan.FromMilliseconds(1000 * SimulationTime / Path.Count);
timer.Tick += TimerOnTick;
timer.Start();
}
else
{
Status2 = " || No path found";
TurnOffAnimation();
}
// OnPropertyChanged(nameof(Status2));
}
void GetObstaclesInConfigurationSpace()
{
int startAlpha0;
int startAlpha1;
int endAlpha0;
int endAlpha1;
if (_showFirst)
{
startAlpha0 = wrapAngle((int)(Robot1.Alpha0 * 180 / Math.PI));
startAlpha1 = wrapAngle((int)(Robot1.Alpha1 * 180 / Math.PI));
}
else
{
startAlpha0 = wrapAngle((int)(Robot1.Alpha0bis * 180 / Math.PI));
startAlpha1 = wrapAngle((int)(Robot1.Alpha1bis * 180 / Math.PI));
}
if (_showFirstEnd)
{
endAlpha0 = wrapAngle((int)(Robot2.Alpha0 * 180 / Math.PI));
endAlpha1 = wrapAngle((int)(Robot2.Alpha1 * 180 / Math.PI));
}
else
{
endAlpha0 = wrapAngle((int)(Robot2.Alpha0bis * 180 / Math.PI));
endAlpha1 = wrapAngle((int)(Robot2.Alpha1bis * 180 / Math.PI));
}
ConfigurationSpaceArray = new int[360, 360];
bitmapHelper.SetColor(240, 248, 255);
Path.Clear();
if (double.IsNaN(Robot1.Alpha0) || double.IsNaN(Robot1.Alpha1) || double.IsNaN(Robot2.Alpha0) || double.IsNaN(Robot2.Alpha1) || CollisionCheck())
{
OnPropertyChanged(nameof(ConfigurationSpaceBitmap));
return;
}
// _configurationSpace.Clear();
Robot tempRobot = Robot1.Clone();
for (int i = 0; i < 360; i++)
{
for (int j = 0; j < 360; j++)
{
ConfigurationSpaceArray[i, j] = 0;
// ConfigurationSpaceArray[i+ j*360] = 0;
foreach (var item in ObstaclesCollection)
{
tempRobot.Alpha0 = Math.PI * i / 180;
tempRobot.Alpha1 = Math.PI * j / 180;
if (item.CollisionCheck(tempRobot.Point0, tempRobot.Point1) ||
item.CollisionCheck(tempRobot.Point1, tempRobot.Point2))
{
ConfigurationSpaceArray[i, j] = -1;
//ConfigurationSpaceBitmap.SetPixel(i, j, System.Drawing.Color.FromArgb(0, 0, 0));
//bitmapHelper.SetPixel(i,j,255,0,0);
}
}
}
}
var maxDistance = arrayFloodFill(ConfigurationSpaceArray, startAlpha0, startAlpha1, endAlpha0, endAlpha1);
if (maxDistance == 0)
{
return;
}
for (int i = 0; i < 360; i++)
{
for (int j = 0; j < 360; j++)
{
if (ConfigurationSpaceArray[i, j] != -1)
{
//bitmapHelper.SetPixel(i, j, 0, (byte)(ConfigurationSpaceArray[i, j] * 255 / 720), 0);
bitmapHelper.SetPixel(i, j, 0, (byte)(ConfigurationSpaceArray[i, j] * 255 / maxDistance), 0);
}
else
{
bitmapHelper.SetPixel(i, j, 255, 0, 0);
ConfigurationSpaceArray[i, j] = -1;
}
}
}
//double[] startAngles = Robot1.CalculateArmAnglesForPosition(StartPosition);
//double[] endAngles = Robot1.CalculateArmAnglesForPosition(EndPosition);
double[] endAngles;
if (_showFirstEnd)
{
endAngles = new double[] { Robot2.Alpha0, Robot2.Alpha1 };
}
else
{
endAngles = new double[] { Robot2.Alpha0bis, Robot2.Alpha1bis };
}
Path.Add(new int[3] {
(int)(endAngles[0] * 180 / Math.PI), (int)(endAngles[1] * 180 / Math.PI), ConfigurationSpaceArray[(int)(endAngles[0] * 180 / Math.PI), (int)(endAngles[1] * 180 / Math.PI)]
});
int value = ConfigurationSpaceArray[Path.Last()[0], Path.Last()[1]];
while (value > 1)
{
if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0] + 1), wrapAngle(Path.Last()[1])] == (value - 1))
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0] + 1), wrapAngle(Path.Last()[1]), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0] + 1), wrapAngle(Path.Last()[1])])
});
}
else if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0] - 1), wrapAngle(Path.Last()[1])] == (value - 1))
{
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0] - 1), wrapAngle(Path.Last()[1]), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0] - 1), wrapAngle(Path.Last()[1])])
});
}
}
else if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] + 1)] == (value - 1))
{
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] + 1), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] + 1)])
});
}
}
else if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] - 1)] == (value - 1))
{
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] - 1), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] - 1)])
});
}
}
else
{
MessageBox.Show("Path not found");
}
value = ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1])];
}
foreach (var item in Path)
{
bitmapHelper.SetPixel(item[0], item[1], 255, 255, 255);
}
OnPropertyChanged(nameof(ConfigurationSpaceBitmap));
}
