public void RunTests()
{
Matrix world = Matrix.CreateWorld(Vector3.Zero, Vector3.Forward, Vector3.Up);
Vector3 minBound = new Vector3(-10, -10, -5);
Vector3 maxBound = new Vector3(120, 10, 5);
Mechanism robot = NArticulatedPlanarMechanism.Create(linkModel, new Vector3(100, 0, 0), minBound,
maxBound, 6, world);
MechanismEnviroment enviroment = new MechanismEnviroment(sceneModel);
double[] origin = new double[6] { 150, 0, 0, 0, 0, 0 };
double[] dest = new double[6] { 90, 0, 0, 0, 0, 0 };
FileStream fs = new FileStream("test1.csv", FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
ManualResetEvent stopEvent = new ManualResetEvent(false);
for (int i = 0; i < 10; i++)
{
Node originNode, destNode;
DateTime start, end;
TimeSpan elapsedTime;
int iterations;
MechanismCSpace mechanismCSpace;
CSpaceRRT RRT;
originNode = null;
destNode = null;
mechanismCSpace = null;
RRT = null;
GC.Collect();
GC.WaitForPendingFinalizers();
mechanismCSpace = new MechanismCSpace(robot, enviroment, i);
RRT = new CSpaceRRT(mechanismCSpace.CSpace, 10000);
Thread.Sleep(1000);
start = DateTime.Now;
iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Edge, false, double.PositiveInfinity, stopEvent);
end = DateTime.Now;
elapsedTime = end - start;
sw.WriteLine("Edge;Infinity;false;" + i.ToString() + ";" + iterations.ToString() + ";" + elapsedTime.TotalSeconds.ToString());
sw.Flush();
for (int j = 0; j < 4; j++)
{
double maxSize;
switch (j)
{
case 0:
maxSize = 5.0;
break;
case 1:
maxSize = 10.0;
break;
case 2:
maxSize = 25.0;
break;
case 3:
maxSize = 50.0;
break;
default:
maxSize = double.PositiveInfinity;
break;
}
originNode = null;
destNode = null;
mechanismCSpace = null;
RRT = null;
GC.Collect();
GC.WaitForPendingFinalizers();
mechanismCSpace = new MechanismCSpace(robot, enviroment, i);
RRT = new CSpaceRRT(mechanismCSpace.CSpace, 10000);
Thread.Sleep(1000);
start = DateTime.Now;
iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Node, false, maxSize, stopEvent);
end = DateTime.Now;
elapsedTime = end - start;
sw.WriteLine("Node;" + maxSize.ToString()+ ";false;" + i.ToString() + ";" + iterations.ToString() + ";" + elapsedTime.TotalSeconds.ToString());
sw.Flush();
originNode = null;
destNode = null;
mechanismCSpace = null;
RRT = null;
GC.Collect();
GC.WaitForPendingFinalizers();
mechanismCSpace = new MechanismCSpace(robot, enviroment, i);
RRT = new CSpaceRRT(mechanismCSpace.CSpace, 10000);
Thread.Sleep(1000);
start = DateTime.Now;
iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Node, true, maxSize, stopEvent);
end = DateTime.Now;
elapsedTime = end - start;
sw.WriteLine(";Node;" + maxSize.ToString()+ ";true;" + i.ToString() + ";" + iterations.ToString() + ";" + elapsedTime.TotalSeconds.ToString());
sw.Flush();
}
}
sw.Close();
}
private void calc(CSpace cSpace)
{
CSpaceRRT RRT = new CSpaceRRT(cSpace, maxIterations);
Node originNode, destNode;
int iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Node, true, 10.0, stopEvent);
double distance = destNode.aTotalDist;
if (distance != 0)
{
noResultCount = 0;
lock (sw)
{
if (distance < minDist)
{
minDist = distance;
//maxIterations = iterations;
bestDestNode = destNode;
t1 = RRT.startTree;
t2 = RRT.goalTree;
//sw.WriteLine(iterations.ToString() + ";" + distance.ToString());
//sw.Flush();
}
//results.Add(new Result(iterations, distance));
}
}
else
{
lock (sw)
{
noResultCount++;
if (noResultCount > 50)
{
//maxIterations *= 2;
noResultCount = 0;
}
}
}
}
