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; }