internal ExplorationTree(CSpace cSpace, Node rootNode, GrowConnectionType connectionType, bool useKDtree, double maxEdgeSize)
{
this.cSpace = cSpace;
this.nodeList = new NodeList(cSpace, growConnectionType, useKDtree);
this.growConnectionType = connectionType;
this.maxEdgeSize = maxEdgeSize;
nodeList.AddNode(rootNode);
}
public CSpacePRM(CSpace cSpace, int N, int k, PRMSampleMethod sampleMethod)
{
this.cSpace = cSpace;
this.N = N;
this.k = k;
this.sampleMethod = sampleMethod;
this.nodeList = new List<Node>();
this.edgeList = new List<Edge>();
}
public NodeList(CSpace cSpace, GrowConnectionType connectionType, bool useKdTree)
{
this.cSpace = cSpace;
this.useKdTree = useKdTree;
this.connectionType = connectionType;
if (useKdTree)
{
kdTree = new KdTree(cSpace);
}
this.nodeList = new List<Node>();
if (connectionType == GrowConnectionType.Edge)
{
this.edgeList = new List<Edge>();
}
}
public MechanismCSpace(Mechanism mechanism, MechanismEnviroment scene, int randomSeed)
{
this.mechanism = (Mechanism)mechanism.Clone();
this.enviroment = scene;
double[] dimensionLowLimit = new double[mechanism.Joints.Count];
double[] dimensionHighLimit = new double[mechanism.Joints.Count];
double[] dimensionWeight = new double[mechanism.Joints.Count];
for (int i = 0; i < mechanism.Joints.Count; i++)
{
dimensionLowLimit[i] = -180;
dimensionHighLimit[i] = 180;
dimensionWeight[i] = 1;
}
this.cSpace = new CSpace(mechanism.Joints.Count, dimensionLowLimit, dimensionHighLimit, dimensionWeight, CheckCollision, randomSeed);
}
internal ExplorationTree(CSpace cSpace, Node rootNode, int sampleCount)
{
this.cSpace = cSpace;
this.nodeList = new NodeList(cSpace, growConnectionType, false);
nodeList.AddNode(rootNode);
switch (samplingMethod)
{
case SamplingMethod.RandomPrior:
sampleList = cSpace.GenerateLatticeSampleList(sampleCount);
break;
case SamplingMethod.LatticePrior:
sampleList = cSpace.GenerateLatticeSampleList(sampleCount);
break;
case SamplingMethod.RandomInteractive:
throw new ArgumentException("Cannot initialize sampleCount with interactive sampling method", "sampleCount");
}
}
public RRTOptimizer(double[] origin, double[] dest, CSpace[] cSpacePool, int threadCount)
{
if (cSpacePool.Length != threadCount)
{
throw new ArgumentException("The cSpacePool must have the same length of the threadCount value");
}
this.cSpacePool = cSpacePool;
this.origin = origin;
this.dest = dest;
this.threadCount = threadCount;
this.threadPool = new Thread[threadCount];
fs = new FileStream("result.csv", FileMode.Append);
sw = new StreamWriter(fs);
stopEvent = new ManualResetEvent(false);
}
private KdTree(CSpace cSpace, Node node, int level)
{
this.cSpace = cSpace;
this.RootNode = node;
this.Level = level;
}
public KdTree(CSpace cSpace)
{
this.cSpace = cSpace;
this.RootNode = null;
this.Level = 0;
}
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;
}
}
}
}
public CSpaceRRT(CSpace cSpace, int k)
{
this.cSpace = cSpace;
this.k = k;
}
