├── gpu
    ├── motionPlanningProblem.cu
    ├── motionPlanningSolution.cu
    ├── motionPlanningSolution.cuh
    ├── input.txt
    ├── obstacles.cuh
    ├── sampler.cuh
    ├── 2pBVP.cuh
    ├── PRM.cuh
    ├── Makefile
    ├── FMT.cuh
    ├── helper.cuh
    ├── collisionCheck.cuh
    ├── motionPlanningProblem.cuh
    ├── dubinsAirplane.cuh
    ├── sampler.cu
    ├── GMT.cuh
    ├── PRM.cu
    ├── helper.cu
    ├── 2pBVP.cu
    ├── treesDub.txt
    ├── dubinsAirplane.cu
    ├── collisionCheck.cu
    ├── FMT.cu
    ├── mainGround.cu
    ├── mainDubins.cu
    ├── GMT.cu
    └── gates.txt
├── viz
    ├── plotObstacles.m
    ├── plot3dObstacles.m
    ├── dubinsAirplane.m
    ├── generateObstacles.m
    ├── plotcube.m
    ├── vizSoln.m
    ├── solveDoubleInt2PBVP.m
    └── dubinsCar.m
└── README.md


/gpu/motionPlanningProblem.cu:
--------------------------------------------------------------------------------
1 | #include "motionPlanningProblem.cuh"
2 | 
3 | void printMotionPlanningProblem(MotionPlanningProblem mpp, std::ostream& stream) {
4 | 
5 | }


--------------------------------------------------------------------------------
/gpu/motionPlanningSolution.cu:
--------------------------------------------------------------------------------
1 | #include "motionPlanningSolution.cuh"
2 | 
3 | typedef struct MotionPlanningSolution {
4 | 	std::vector<int> path; // list of path indexes
5 | 	float cost;
6 | 	float cp;
7 | 	float time;
8 | 
9 | } MotionPlanningSolution;


--------------------------------------------------------------------------------
/gpu/motionPlanningSolution.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | motionPlanningSolution.cuh
 3 | Author: Brian Ichter
 4 | 
 5 | Struct definition for a motion planning problem's solution.
 6 | */
 7 | 
 8 | #pragma once
 9 | 
10 | #include <vector>
11 | 
12 | struct MotionPlanningSolution;


--------------------------------------------------------------------------------
/viz/plotObstacles.m:
--------------------------------------------------------------------------------
 1 | function plotObstacles(obstacles)
 2 | for i=1:2:length(obstacles(:,1))
 3 |     a = obstacles(i,1);
 4 |     b = obstacles(i,2);
 5 |     c = obstacles(i+1,1);
 6 |     d = obstacles(i+1,2);
 7 | 
 8 |     fill([a a c c], [d b b d], 'r','EdgeColor','k','facealpha',.4);
 9 | end
10 | end
11 | 
12 | 


--------------------------------------------------------------------------------
/gpu/input.txt:
--------------------------------------------------------------------------------
 1 | 0.1,0.1,0.1,0,0,0,
 2 | 0.9,0.9,0.9,0,0,0,
 3 | 0,0,0,-1,-1,-1,
 4 | 1,1,1,1,1,1,
 5 | 3,
 6 | 0.05, 0.20, -3.1, -3.1, -3.1, -3.1, 0.45, 0.35, 3.1, 3.1, 3.1, 3.1,
 7 | 0.70, 0.30, -3.1, -3.1, -3.1, -3.1, 0.90, 0.50, 3.1, 3.1, 3.1, 3.1,
 8 | 0.30, 0.60, -3.1, -3.1, -3.1, -3.1, 0.80, 0.75, 3.1, 3.1, 3.1, 3.1,
 9 | 
10 | everything here will not be read in
11 | the format is:
12 | 	initial position
13 | 	goal position
14 | 	lower bound on state space
15 | 	upper bound on state space
16 | 	number of obstacles
17 | 	obstacles
18 | 


--------------------------------------------------------------------------------
/viz/plot3dObstacles.m:
--------------------------------------------------------------------------------
 1 | function plot3dObstacles(obstacles, varargin)
 2 | color = [1 0.6 0.6];
 3 | alpha = 0.7;
 4 | 
 5 | if nargin >= 3
 6 |     color = varargin{1};
 7 |     alpha = varargin{2};
 8 | elseif nargin == 2
 9 |     color = varargin{1};
10 | end
11 |     
12 | for i=1:2:length(obstacles(:,1))
13 |     a = obstacles(i,1);
14 |     b = obstacles(i,2);
15 |     c = obstacles(i,3);
16 |     d = obstacles(i+1,1);
17 |     e = obstacles(i+1,2);
18 |     f = obstacles(i+1,3);
19 |     
20 |     plotcube([a-d b-e c-f],[d e f],alpha,color);
21 | end
22 | end


--------------------------------------------------------------------------------
/gpu/obstacles.cuh:
--------------------------------------------------------------------------------
 1 | /* 
 2 | obstacles.cuh
 3 | author: Brian Ichter
 4 | 
 5 | Generates hyperrectangular obstacle sets
 6 | */
 7 | 
 8 | #pragma once
 9 | 
10 | #include <stdio.h>
11 | #include <vector>
12 | #include <iostream>
13 | #include <stdlib.h>
14 | 
15 | #include "helper.cuh"
16 | 
17 | int getObstaclesCount(); // return number of obstacles
18 | void generateObstacles(float *obstacles, int obstaclesLength); // create obstacle array
19 | void inflateObstacles(float *obstacles, float *obstaclesInflated, float inflateFactor, int obstaclesCount);
20 | void addObstacles(float *obstacles, int numToAdd, int obstaclesCount, float obsWidth); // add a number of box obstacles 


--------------------------------------------------------------------------------
/gpu/sampler.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | sampler.cuh
 3 | author: Brian Ichter
 4 | 
 5 | This file includes state sampling methods
 6 | */
 7 | 
 8 | #pragma once
 9 | 
10 | #include <cstdlib>
11 | #include <iostream>
12 | 
13 | void createSamplesIID(int seed, float *samples, float *initial, float *goal, float *lo, float *hi);
14 | void createSamplesHalton(int skip, float *samples, float *initial, float *goal, float *lo, float *hi);
15 | float localHaltonSingleNumber(int n, int b);
16 | 
17 | // halton
18 | // lattice (may want to use for NN benefits)
19 | 
20 | __global__ void sampleFree(float* obstacles, int obstaclesCount, float* samples, bool* isFreeSamples, float *debugOutput);
21 | __global__ void fillSamples(float* samples, float* samplesAll, int* sampleFreeIdx, bool* isFreeSamples, float *debugOutput);
22 | __global__ void createSortHeuristic(float* samples, int initial_idx, float* heuristic, int samplesCount);
23 | bool sampleFreePt(float* obstacles, int obstaclesCount, float* sample);


--------------------------------------------------------------------------------
/viz/dubinsAirplane.m:
--------------------------------------------------------------------------------
 1 | %% time optimal dubins airplane, 4d (x,y,z,theta)
 2 | % following http://msl.cs.uiuc.edu/~lavalle/papers/ChiLav07b.pdf
 3 | % and http://www.et.byu.edu/~beard/papers/preprints/BeardMcLain__.pdf
 4 | % min turning radius is 1, max altitude rate is unbounded, velocity xy is 1
 5 | % note unbounded altitude unlike previous works, has no effect on
 6 | % algorithmic performance (since the paths are computed offline anyway
 7 | % except connecting the initial and goal to the tree) and significantly
 8 | % simplier implementation
 9 | 
10 | % our cost is distance traveled
11 | function [path, topt] = dubinsAirplane(xinit,xterm,numDisc)
12 | [path, cmin] = dubinsCar(xinit(1:3),xterm(1:3),numDisc,1,1);
13 | dz = xterm(4) - xinit(4);
14 | topt = sqrt(cmin^2 + dz^2);
15 | 
16 | dd = path(1:size(path,1)-1,1:2)-path(2:size(path,1),1:2);
17 | dd = sum(sqrt(dd.*dd),2);
18 | dist = cumsum(dd);
19 | 
20 | zpath = xinit(4) + dist/max(dist)*dz;
21 | path = [path [xinit(4); zpath]];
22 | end


--------------------------------------------------------------------------------
/gpu/2pBVP.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | 2pBVP.cuh
 3 | author: Brian Ichter
 4 | 
 5 | This file solves the 3D double integrator two point boundary value problem
 6 | */
 7 | 
 8 | #pragma once
 9 | 
10 | #include <math.h> 
11 | #include "helper.cuh"
12 | 
13 | // find optimal path
14 | float findOptimalPath(float dt, float *splitPath, float *xs, int numWaypoints, int *pathLength);
15 | void discretizePath(float *splitPath, float *x0, float *x1, int numDisc, float topt);
16 | 
17 | __device__ __host__
18 | float findDiscretizedPath(float *splitPath, float *x0, float *x1, int numDisc);
19 | 
20 | // bisection search to find tau for optimal cost
21 | __device__ __host__
22 | float toptBisection(float *x0, float *x1, float tmax);
23 | 
24 | // cost at tau
25 | __device__ __host__
26 | float cost(float tau, float *x0, float *x1);
27 | 
28 | // deriviative of the cost at tau
29 | __device__ __host__
30 | float dcost(float tau, float *x0, float *x1);
31 | 
32 | // put a new point in the path at x
33 | __device__ __host__
34 | void pathPoint(float t, float tau, float *x0, float *x1, float *x);
35 | 
36 | __global__ void fillCoptsTopts(float *samples, float *copts, float *topts, float tmax);


--------------------------------------------------------------------------------
/gpu/PRM.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | PRM.cuh
 3 | author: Brian Ichter
 4 | 
 5 | PRM algorithm
 6 | */
 7 | #pragma once
 8 | 
 9 | #include <thrust/device_vector.h>
10 | #include <thrust/remove.h>
11 | #include <thrust/sort.h>
12 | #include <thrust/scan.h>
13 | #include <iostream>     // std::cout
14 | #include <algorithm>    // std::make_heap, std::pop_heap, std::push_heap, std::sort_heap
15 | #include <vector>       // std::vector
16 | #include <set>
17 | #include <utility>
18 | 
19 | 
20 | #include "collisionCheck.cuh"
21 | #include "helper.cuh"
22 | 
23 | /***********************
24 | CPU functions
25 | ***********************/
26 | // GMT with lambda = 1 (i.e. expand entire wavefront at once)
27 | 
28 | void PRM(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
29 | 	std::vector<float> adjCosts, std::vector<int> nnGoEdges, std::vector<int> nnComeEdges, int nnSize, float *d_discMotions, std::vector<int> nnIdxs,
30 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc, int numEdges,
31 | 	std::vector<float> costs, int *d_edges, int initial_idx, int goal_idx,
32 | 	std::vector<float> c2g);
33 | 
34 | /***********************
35 | GPU kernels
36 | ***********************/


--------------------------------------------------------------------------------
/gpu/Makefile:
--------------------------------------------------------------------------------
 1 | DIM ?= 6
 2 | NUM ?= 1000
 3 | 
 4 | CXX=nvcc
 5 | CXXFLAGS=-O3 -arch=sm_61 -rdc=true -D DIM=$(DIM) -D NUM=$(NUM)
 6 | # compute capability is sm_30 for macbook, sm_52 for phobos, sm_53 for tx1, sm_61 for 1080
 7 | 
 8 | SRC=*.cu
 9 | 
10 | EXPERIMENT=mainExperiment.cu motionPlanningProblem.cu motionPlanningSolution.cu collisionCheck.cu obstacles.cu helper.cu 2pBVP.cu sampler.cu GMT.cu
11 | DUBINS=mainDubins.cu motionPlanningProblem.cu motionPlanningSolution.cu collisionCheck.cu obstacles.cu helper.cu 2pBVP.cu sampler.cu GMT.cu PRM.cu FMT.cu dubinsAirplane.cu
12 | GROUND=mainGround.cu motionPlanningProblem.cu motionPlanningSolution.cu collisionCheck.cu obstacles.cu helper.cu 2pBVP.cu sampler.cu GMT.cu PRM.cu FMT.cu dubinsAirplane.cu
13 | SIM=mainSim.cu motionPlanningProblem.cu motionPlanningSolution.cu collisionCheck.cu obstacles.cu helper.cu 2pBVP.cu sampler.cu GMT.cu PRM.cu FMT.cu dynamics.cu
14 | 
15 | main: $(SRC)
16 | 	$(CXX) $(CXXFLAGS) $(SRC) -o $@ 
17 | 
18 | experiment: $(EXPERIMENT)
19 | 	$(CXX) $(CXXFLAGS) $(EXPERIMENT) -o $@ 
20 | 
21 | dubins: $(DUBINS)
22 | 	$(CXX) $(CXXFLAGS) $(DUBINS) -o $@ 
23 | 
24 | ground: $(GROUND)
25 | 	$(CXX) $(CXXFLAGS) $(GROUND) -o $@ 
26 | 
27 | sim: $(SIM)
28 | 	$(CXX) $(CXXFLAGS) $(SIM) -o $@ 
29 | 
30 | clean:
31 | 	rm -f *.o *~ *~ main ground sim dubins experiment
32 | 	rm -rf *.dSYM
33 | 


--------------------------------------------------------------------------------
/gpu/FMT.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | FMT.cuh
 3 | author: Brian Ichter
 4 | 
 5 | FMT algorithm
 6 | */
 7 | #pragma once
 8 | 
 9 | #include <thrust/device_vector.h>
10 | #include <thrust/remove.h>
11 | #include <thrust/sort.h>
12 | #include <thrust/scan.h>
13 | #include <iostream>     // std::cout
14 | #include <algorithm>    // std::make_heap, std::pop_heap, std::push_heap, std::sort_heap
15 | #include <vector>       // std::vector
16 | #include <set>
17 | #include <utility>
18 | 
19 | 
20 | #include "collisionCheck.cuh"
21 | #include "helper.cuh"
22 | #include "2pBVP.cuh"
23 | #include "dubinsAirplane.cuh"
24 | 
25 | /***********************
26 | CPU functions
27 | ***********************/
28 | // GMT with lambda = 1 (i.e. expand entire wavefront at once)
29 | 
30 | void FMT(float *initial, float *goal, float *obstacles, int obstaclesCount,
31 | 	std::vector<float> adjCosts, std::vector<int> nnGoEdges, std::vector<int> nnComeEdges, 
32 | 	int nnSize, std::vector<float> discMotions, std::vector<int> nnIdxs,
33 | 	int samplesCount, bool *d_isFreeSamples, float r, int numDisc, int numEdges,
34 | 	std::vector<float> costs, int initial_idx, int goal_idx,
35 | 	std::vector<float> samplesAll, std::vector<float> times);
36 | 
37 | void FMTdub(float *initial, float *goal, float *obstacles, int obstaclesCount,
38 | 	std::vector<float> adjCosts, std::vector<int> nnGoEdges, std::vector<int> nnComeEdges, 
39 | 	int nnSize, std::vector<float> discMotions, std::vector<int> nnIdxs,
40 | 	int samplesCount, bool *d_isFreeSamples, float r, int numDisc, int numEdges,
41 | 	std::vector<float> costs, int initial_idx, int goal_idx,
42 | 	std::vector<float> samplesAll, std::vector<float> times,
43 | 	int numControls, std::vector<int> controlEdge, std::vector<float> controlDEdge);
44 | 


--------------------------------------------------------------------------------
/viz/generateObstacles.m:
--------------------------------------------------------------------------------
 1 | function [obstacles] = generateObstacles(dim)
 2 | 
 3 | if dim == 2
 4 |     % given set
 5 |     obstacles = [0.434752 0.542681
 6 |         0.853504 0.947454
 7 |         0.543562 0.0691414
 8 |         0.945129 0.521948
 9 |         0.0529046 0.215034
10 |         0.421858 0.617981];
11 |     obstacles = [0.0 0.2
12 |         0.3 0.5
13 |         0.5 0.2
14 |         0.8 0.5
15 |         0.9 0.2
16 |         1.0 0.5
17 |         0.5 0.0
18 |         0.8 0.1
19 |         0.5 0.7 
20 |         0.8 1.0
21 |         ];
22 | elseif dim == 3
23 |     % 3d obstacle filled set
24 |     obstacles = [0.434752 0.542681 -0.1
25 |         0.853504 0.947454 1.1
26 |         0.543562 0.0691414 -0.1
27 |         0.945129 0.521948 1.1
28 |         0.0529046 0.215034 -0.1
29 |         0.421858 0.617981 1.1];
30 |     obstacles = [0.05 0.20 -0.1
31 |         0.45 0.35 1.1;
32 |         0.70 0.30 -0.1;
33 |         0.90 0.50 1.1;
34 |         0.30 0.60 -0.1;
35 |         0.80 0.75 1.1];
36 | %     obstacles = [0.05 0.20 -0.1
37 | %         0.45 0.35 1.1;
38 | %         0.60 0.30 -0.1;
39 | %         0.90 0.60 1.1;
40 | %         0.30 0.60 -0.1;
41 | %         0.50 0.75 1.1];
42 | %     obstacles = [0.05 0.15 -.10;
43 | %         0.35 0.75 1.10;
44 | %         0.45 0.10 0.10;
45 | %         0.75 0.40 1.10;
46 | %         0.65 0.65 -.10;
47 | %         1.00 0.80 1.10;
48 | %         0.35 0.50 -.10;
49 | %         0.65 0.80 0.50;
50 | %         0.45 0.85 0.60;
51 | %         0.65 0.95 0.85];
52 | % obstacles = [0.0 0.2 -0.1
53 | %     0.3 0.5 1.1
54 | %     0.5 0.2 -0.1
55 | %     0.8 0.5 1.1
56 | %     0.9 0.2 -0.1
57 | %     1.0 0.5 1.1
58 | %     0.5 0.0 -0.1
59 | %     0.8 0.1 1.1
60 | %     0.5 0.7 -0.1 
61 | %     0.8 1.0 1.1
62 | %     ];
63 | end
64 | 
65 | end
66 | 
67 | 


--------------------------------------------------------------------------------
/viz/plotcube.m:
--------------------------------------------------------------------------------
 1 | function plotcube(varargin)
 2 | % PLOTCUBE - Display a 3D-cube in the current axes
 3 | %
 4 | %   PLOTCUBE(EDGES,ORIGIN,ALPHA,COLOR) displays a 3D-cube in the current axes
 5 | %   with the following properties:
 6 | %   * EDGES : 3-elements vector that defines the length of cube edges
 7 | %   * ORIGIN: 3-elements vector that defines the start point of the cube
 8 | %   * ALPHA : scalar that defines the transparency of the cube faces (from 0
 9 | %             to 1)
10 | %   * COLOR : 3-elements vector that defines the faces color of the cube
11 | %
12 | % Example:
13 | %   >> plotcube([5 5 5],[ 2  2  2],.8,[1 0 0]);
14 | %   >> plotcube([5 5 5],[10 10 10],.8,[0 1 0]);
15 | %   >> plotcube([5 5 5],[20 20 20],.8,[0 0 1]);
16 | 
17 | % Default input arguments
18 | inArgs = { ...
19 |   [10 56 100] , ... % Default edge sizes (x,y and z)
20 |   [10 10  10] , ... % Default coordinates of the origin point of the cube
21 |   .7          , ... % Default alpha value for the cube's faces
22 |   [1 0 0]       ... % Default Color for the cube
23 |   };
24 | 
25 | % Replace default input arguments by input values
26 | inArgs(1:nargin) = varargin;
27 | 
28 | % Create all variables
29 | [edges,origin,alpha,clr] = deal(inArgs{:});
30 | 
31 | XYZ = { ...
32 |   [0 0 0 0]  [0 0 1 1]  [0 1 1 0] ; ...
33 |   [1 1 1 1]  [0 0 1 1]  [0 1 1 0] ; ...
34 |   [0 1 1 0]  [0 0 0 0]  [0 0 1 1] ; ...
35 |   [0 1 1 0]  [1 1 1 1]  [0 0 1 1] ; ...
36 |   [0 1 1 0]  [0 0 1 1]  [0 0 0 0] ; ...
37 |   [0 1 1 0]  [0 0 1 1]  [1 1 1 1]   ...
38 |   };
39 | 
40 | XYZ = mat2cell(...
41 |   cellfun( @(x,y,z) x*y+z , ...
42 |     XYZ , ...
43 |     repmat(mat2cell(edges,1,[1 1 1]),6,1) , ...
44 |     repmat(mat2cell(origin,1,[1 1 1]),6,1) , ...
45 |     'UniformOutput',false), ...
46 |   6,[1 1 1]);
47 | 
48 | 
49 | cellfun(@patch,XYZ{1},XYZ{2},XYZ{3},...
50 |   repmat({clr},6,1),...
51 |   repmat({'FaceAlpha'},6,1),...
52 |   repmat({alpha},6,1)...
53 |   );
54 | 
55 | view(3);
56 | 


--------------------------------------------------------------------------------
/gpu/helper.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | helper.cuh
 3 | author: Brian Ichter
 4 | 
 5 | This file includes helper methods used throughout the code. 
 6 | */
 7 | 
 8 | #pragma once
 9 | 
10 | #include <stdio.h>
11 | #include <vector>
12 | #include <iostream>
13 | #include <string>
14 | #include <ctime>
15 | #include <cstdlib>
16 | #include <math.h>
17 | #include <fstream>
18 | #include "cuda.h"
19 | #include "cuda_runtime.h"
20 | 
21 | #define _USE_MATH_DEFINES
22 | 
23 | // A macro for checking the error codes of cuda runtime calls
24 | #define CUDA_ERROR_CHECK(expr) \
25 |   {                            \
26 |     cudaError_t err = expr;    \
27 |     if (err != cudaSuccess)    \
28 |     {                          \
29 |       printf("CUDA call failed!\n\t%s\n", cudaGetErrorString(err)); \
30 |       exit(1);                 \
31 |     }                          \
32 |   }
33 | 
34 | 
35 | void printArray(float* array, int dim1, int dim2, std::ostream &stream);
36 | void printArray(int* array, int dim1, int dim2, std::ostream &stream);
37 | void printArray(bool* array, int dim1, int dim2, std::ostream &stream);
38 | void printArray(char* array, int dim1, int dim2, std::ostream &stream);
39 | float avgArray(float* array, int length);
40 | void copyArray(float* arrayTo, float* arrayFrom, int length);
41 | void multiplyArrays(float* A, float* B, float* C, int rowsA, int colsA, int rowsB, int colsB);
42 | void scalarMultiplyArray(float *A, float b, float *C, int rows, int cols);
43 | void subtractArrays(float* A, float* B, float* C, int rows, int cols);
44 | void horizConcat(float* A, float *B, float *C, int nA, int nB, int m);
45 | void vertConcat(float* A, float *B, float *C, int n, int mA, int mB);
46 | void transpose(float* A, int rows, int cols);
47 | void transpose(float* Atrans, float* A, int rows, int cols) ;
48 | float calculateUnitBallVolume(int dim);
49 | float calculateConnectionBallRadius(int dim, int samplesCount);
50 | // output of this file goes into matlabviz/vizSoln.m
51 | int printSolution(int samplesCount, float *samples, int *d_edges, float *d_costs);


--------------------------------------------------------------------------------
/gpu/collisionCheck.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | collisionCheck.cuh
 3 | author: Brian Ichter
 4 | 
 5 | CUDA collision checker is contained for AABB
 6 | */
 7 | 
 8 | #pragma once
 9 | #include <vector>
10 | 
11 | // splits an motion into its waypoints, returns true if motion is valid
12 | __device__ void waypointCollisionCheck(int v_idx, int w_idx, int obstaclesCount, float* obstacles, 
13 | 	int *nnIdxs, float *discMotions, int discIdx, int numDisc, bool *isCollision, int tid, float *debugOutput);
14 | __device__ bool isMotionValid(float *v, float *w, float *bbMin, float *bbMax, int obstaclesCount, float* obstacles, float *debugOutput);
15 | __device__ bool broadphaseValidQ(float *bbMin, float *bbMax, float *obs, float *debugOutput);
16 | __device__ bool motionValidQ(float *v, float *w, float *obs, float *debugOutput);
17 | __device__ bool faceContainsProjection(float *v, float *w, float lambda, int j, float *obs, float* debugOutput);
18 | __device__ bool faceContainsProjectionError(float *v, float *v_to_w, float lambda, int j, float *obs, float* debugOutput);
19 | __global__ void freeEdges(float *obstacles, int obstaclesCount, float *samples, 
20 | 	bool *isFreeSamples, int numDisc, float *discMotions, bool *isFreeEdges, int numEdges, float *debugOutput);
21 | 
22 | // CPU
23 | void waypointCollisionCheck_h(int v_idx, int w_idx, int obstaclesCount, float* obstacles, 
24 | 	int *nnIdxs, float *discMotions, int discIdx, int numDisc, bool *isCollision, int tid, float *debugOutput);
25 | bool isMotionValid_h(float *v, float *w, float *bbMin, float *bbMax, int obstaclesCount, float* obstacles, float *debugOutput);
26 | bool broadphaseValidQ_h(float *bbMin, float *bbMax, float *obs, float *debugOutput);
27 | bool motionValidQ_h(float *v, float *w, float *obs, float *debugOutput);
28 | bool faceContainsProjection_h(float *v, float *w, float lambda, int j, float *obs, float* debugOutput);
29 | bool faceContainsProjectionError_h(float *v, float *v_to_w, float lambda, int j, float *obs, float* debugOutput);
30 | bool isFreeEdge_h(int edgeIdx, float *obstacles, int obstaclesCount, 
31 | 	int numDisc, std::vector<float> discMotions, float *debugOutput);


--------------------------------------------------------------------------------
/gpu/motionPlanningProblem.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | motionPlanningProblem.cuh
 3 | Author: Brian Ichter
 4 | 
 5 | Struct definition for a motion planning problem.
 6 | */
 7 | 
 8 | #pragma once
 9 | 
10 | #include "motionPlanningSolution.cuh"
11 | #include <iostream>
12 | #include <vector>
13 | 
14 | typedef struct MotionPlanningProblem {
15 | 	const char *filename;
16 | 	int dimW; // workspace dimension
17 | 	int dimC; // configuration space dimension
18 | 	int numSamples; // number of samples
19 | 	std::vector<float> hi; // high bound on configuration space
20 | 	std::vector<float> lo; // low bound on configuration space
21 | 
22 | 	float cpTarget; // target collision probability
23 | 	int mcSamplesNum; // number of Monte Carlo samples for CP calculation
24 | 	float lambda; // expansion cost threshold expansion rate (threshold += rn*lambda at each step)
25 | 	float dt;
26 | 
27 | 	int edgeDiscNum; // for collision checks, number of discretizations in edge representation
28 | 	int obstaclesNum; // 
29 | 	std::vector<float> obstacles;
30 | 	float *d_obstacles;
31 | 
32 | 	std::vector<float> init; // initial state
33 | 	std::vector<float> goal; // goal state
34 | 	int initIdx; // index in samples of 
35 | 	int goalIdx; 
36 | 
37 | 	// calculated
38 | 	float rnPerc; // percentile cutoff of for nn connections (e.g. 0.1 means 10th percentile)
39 | 	float rn; // nearest neighbor radius, as 100*rnPerc percentile connection cost
40 | 
41 | 	// host and device big arrays
42 | 	// samples
43 | 	std::vector<float> samples; // samples[i*DIM + d] is sample i, dimension d 
44 | 	float *d_samples; // size NUM*DIM
45 | 
46 | 	// nn's
47 | 	std::vector<bool> nn; // nn[i*NUM + j] is true if a connection exists from i -> j
48 | 	bool *d_nn; // size NUM*NUM
49 | 	std::vector<bool>  nnT; // (nn' for locality) nn[j*NUM + i] is true if a connection exists from i -> j
50 | 	bool *d_nnT; // size NUM*NUM
51 | 	std::vector<int> nnIdx; // nnIdx[i*NUM*j] gives the index in nnEdges of the edge connecting i -> j
52 | 	int *d_nnIdx; // size NUM*NUM
53 | 	std::vector<float> nnEdges; // discretized edges for nn optimal path connections, nnEdges[i*DIM*edgeDiscNum + j*DIM + d] is the state for edge i, waypoint j, and dimension d
54 | 	float *d_nnEdges; // size NUM*(NUM-1)*rnPerc*DIM*edgeDiscNum
55 | 
56 | 	std::vector<float> costs; // costs for edge i
57 | 	float *d_costs; // size NUM*(NUM-1)*rnPerc
58 | 	std::vector<float> times; // times for edge i
59 | 	float *d_times; // size NUM*(NUM-1)*rnPerc
60 | 
61 | 	// uncertainty (either offsets hardcoded in LQG or matricies for uncertainty)
62 | 
63 | 
64 | 	// precomputed path offsets
65 | 	
66 | 
67 | 	// other preallocated
68 | 	
69 | 
70 | 	MotionPlanningSolution *soln; // solution to problem
71 | } MotionPlanningProblem;
72 | 
73 | void printMotionPlanningProblem(MotionPlanningProblem mpp, std::ostream& stream);
74 | // sizeMotionPlanningProblem // once all ints for sizing are initialized, we size the vectors


--------------------------------------------------------------------------------
/gpu/dubinsAirplane.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | dubinsAirplane.cuh
 3 | author: Brian Ichter
 4 | 
 5 | This file implements local steering for the Dubins aircraft, a vehicle which (in this code)
 6 | can in the x-y plane move (at unit velocity) straight or turn with a bounded turning radius (unit radius)
 7 | and can move freely in the z-direction. The state is [x y z theta] and the minimum distance path consists of 
 8 | dubins curves in the x-y and theta and constant zdot in the z direction.
 9 | */
10 | 
11 | #pragma once
12 | 
13 | #include <iostream>
14 | #include <vector>
15 | #include <algorithm>
16 | #include <math.h>
17 | #include <limits.h>
18 | #include <cfloat>
19 | 
20 | #define _USE_MATH_DEFINES
21 | 
22 | // naming conventions:
23 | // Control is a vector of ints that is 3 long, where each element is the letter for the dubins 
24 | // word. -1 means right, 0 straight, 1 left
25 | // ControlD is a vector of floats that is 3 long and is the duration of each letter in the dubins word.
26 | 
27 | // find optimal cost and word (LSL, RSR, RSL, LSR, RLR, LRL)
28 | // __device__ __host__
29 | float dubinsAirplaneCost(float *x0, float *x1, int* control, float* controlD);
30 | 
31 | // find optimal path given word
32 | // given x0 and x1 (states), control which specifies a word (3 elements -1, 0, or +1), and the duration of each control
33 | void dubinsAirplanePath(float *x0, float *x1, int* control, float* controlD, float* path, int numDisc);
34 | 
35 | // ************* word cost functions
36 | // return the cost of each manuever and the duration of each control (to later build the path)
37 | float dubinsLSLCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD);
38 | float dubinsRSRCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD);
39 | float dubinsRSLCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD);
40 | float dubinsLSRCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD);
41 | float dubinsRLRCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD);
42 | float dubinsLRLCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD);
43 | 
44 | // ************* word path functions
45 | // return the path for each manuever
46 | // void dubinsLSLPath(float d, float a, float b, float* path, int numDisc);
47 | // void dubinsRSRPath(float d, float a, float b, float* path, int numDisc);
48 | // void dubinsRSLPath(float d, float a, float b, float* path, int numDisc);
49 | // void dubinsLRSPath(float d, float a, float b, float* path, int numDisc);
50 | // void dubinsRLRPath(float d, float a, float b, float* path, int numDisc);
51 | // void dubinsLRLPath(float d, float a, float b, float* path, int numDisc);
52 | 
53 | // ************* path builder functions
54 | // ang mod 2*pi
55 | float mod2pi(float ang);
56 | 
57 | // create path from control (control means -1 = right, 0 = straight, 1 = left) and d is the duration
58 | void carControl(int control, float d, float* path, int numDisc);
59 | 
60 | // transform path by initial conditions (rotate and offset)
61 | void transformPath(float *xinit, float* path, int numDisc);
62 | 


--------------------------------------------------------------------------------
/viz/vizSoln.m:
--------------------------------------------------------------------------------
 1 | % Author: brian ichter
 2 | % Solution visualization
 3 | % Currently setup for a double integrator, but just substitude the local
 4 | % planner for dubins car for solveDoubleInt2PBVP (the double integrator for
 5 | % the double integrator)
 6 | 
 7 | solutionData; % put soln.txt output in solutionData.m
 8 | format compact; 
 9 | edges = edges + 1;
10 | maxCost = max(costs);
11 | dt = 0.025;
12 | 
13 | [numSamples,dim] = size(samples);
14 | 
15 | %% obs
16 | obstacles = generateObstacles(3);
17 | dx = 0.003; dy = 0.003; doLabel = false; % label node numbers
18 | 
19 | %% plot 2D results
20 | close all; figure; hold on;
21 | plotObstacles(obstacles);
22 | for i = 1:numSamples
23 |     color = [costs(i)/maxCost 0 1-costs(i)/maxCost];
24 |     edgeIdx = edges(i);
25 |     if edgeIdx == 0
26 |         plot(samples(i,1), samples(i,2), 'g.');
27 |     elseif edgeIdx == -1
28 |         plot(samples(i,1), samples(i,2), 'm.');
29 |     end
30 |     if edgeIdx > 0
31 |         [path] = solveDoubleInt2PBVP(dim, dt, 0, samples(edgeIdx,:), samples(i,:)); % double int
32 | %         plot([samples(i,1) samples(edgeIdx,1)],[samples(i,2) samples(edgeIdx,2)],'k-d',...
33 | %             'MarkerFaceColor',color,'MarkerEdgeColor',color); % straight line
34 |         plot(path(1,:), path(2,:), 'k-d',...
35 |             'MarkerFaceColor',color,'MarkerEdgeColor',color);
36 |         if doLabel
37 |             text(samples(edges(i),1)+dx, samples(edges(i),2)+dy, num2str(edges(i)));
38 |             text(samples(i,1)+dx, samples(i,2)+dy, num2str(i));
39 |         end
40 |     end
41 | end
42 | 
43 | % plot the solution
44 | goalIdx = length(edges);
45 | currentEdge = goalIdx;
46 | while edges(currentEdge) > 0
47 |     nextEdge = edges(currentEdge);
48 |     [path] = solveDoubleInt2PBVP(dim, dt, 0, samples(nextEdge,:), samples(currentEdge,:));
49 |     plot(path(1,:), path(2,:), 'g-d',...
50 |             'LineWidth',5,'MarkerSize',5); % double int
51 |         
52 | %     plot([samples(currentEdge,1),samples(nextEdge,1)],...
53 | %         [samples(currentEdge,2),samples(nextEdge,2)],'g-d',...
54 | %         'LineWidth',5,'MarkerSize',5); % straight line
55 |     currentEdge = nextEdge;
56 | end
57 | 
58 | %% plot 3D and up simulations
59 | if dim > 2
60 |     figure; hold on;
61 |     plot3dObstacles(obstacles);
62 |     for i = 1:0; % turned off because so intensive
63 |         color = [costs(i)/maxCost 0 1-costs(i)/maxCost];
64 |         edgeIdx = edges(i);
65 |         if edgeIdx == 0
66 |             plot3(samples(i,1), samples(i,2), samples(i,3), 'g.');
67 |         elseif edgeIdx == -1
68 |             plot3(samples(i,1), samples(i,2), samples(i,3), 'm.');
69 |         end
70 |         if edgeIdx > 0
71 |             [path] = solveDoubleInt2PBVP(dim, dt, 0, samples(edgeIdx,:), samples(i,:));
72 |             plot3(path(1,:), path(2,:), path(3,:), 'k-o',...
73 |                 'MarkerFaceColor',color,'MarkerEdgeColor',color); % double int
74 | %             plot3([samples(i,1) samples(edgeIdx,1)],[samples(i,2) samples(edgeIdx,2)],[samples(i,3) samples(edgeIdx,3)],'k-o',...
75 | %                 'MarkerFaceColor',color,'MarkerEdgeColor',color); % straight line
76 |         end
77 |     end
78 |     
79 |     % plot the solution
80 |     goalIdx = length(edges);
81 |     currentEdge = goalIdx;
82 |     while edges(currentEdge) > 0
83 |         nextEdge = edges(currentEdge);
84 |         samples(currentEdge,:)
85 |         currentEdge
86 | %         plot3([samples(currentEdge,1),samples(nextEdge,1)],...
87 | %             [samples(currentEdge,2),samples(nextEdge,2)],...
88 | %             [samples(currentEdge,3),samples(nextEdge,3)],'g-d',...
89 | %             'LineWidth',5,'MarkerSize',5); % straight line
90 |         [path] = solveDoubleInt2PBVP(dim, dt, 0, samples(nextEdge,:), samples(currentEdge,:));
91 |         plot3(path(1,:), path(2,:), path(3,:), 'g-d',...
92 |             'LineWidth',5,'MarkerSize',5); % double int
93 |         currentEdge = nextEdge;
94 |     end
95 | end


--------------------------------------------------------------------------------
/gpu/sampler.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Generate state space samples
  4 | */
  5 | 
  6 | #include "sampler.cuh"
  7 | 
  8 | // TODO: add protection on initial and goal indexes
  9 | void createSamplesIID(int seed, float *samples, float *initial, float *goal, float *lo, float *hi) 
 10 | { 
 11 | 	std::srand(seed);
 12 | 	for (int d = 0; d < DIM; ++d) {
 13 | 		for (int i = 0; i < NUM; ++i) {
 14 | 			samples[i*DIM + d] = ((float)std::rand())/RAND_MAX*(hi[d] - lo[d]) + lo[d];
 15 | 		}
 16 | 	}
 17 | 
 18 | 	// replace goal and initial nodes
 19 | 	for (int d = 0; d < DIM; ++d) {
 20 | 		samples[d] = initial[d];
 21 | 		samples[(NUM-1)*DIM + d] = goal[d];
 22 | 	}
 23 | }
 24 | 
 25 | void createSamplesHalton(int skip, float *samples, float *initial, float *goal, float *lo, float *hi) 
 26 | { 	
 27 | 	int numPrimes = 25;
 28 | 	if (skip + DIM > numPrimes) {
 29 | 		std::cout << "in sampler.cu: skip in creating halton seq too high" << std::endl;
 30 | 		return;
 31 | 	}
 32 | 	int bases[] = {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 
 33 | 		43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89, 97};
 34 | 
 35 | 	for (int d = 0; d < DIM; ++d) {
 36 | 		for (int n = 0; n < NUM; ++n) {
 37 | 			samples[n*DIM + d] = localHaltonSingleNumber(n, bases[d + skip])*(hi[d] - lo[d]) + lo[d];
 38 | 		}
 39 | 	}
 40 | 
 41 | 	// replace goal and initial nodes
 42 | 	for (int d = 0; d < DIM; ++d) {
 43 | 		samples[d] = initial[d];
 44 | 		samples[(NUM-1)*DIM + d] = goal[d];
 45 | 	}
 46 | }
 47 | 
 48 | float localHaltonSingleNumber(int n, int b) 
 49 | {
 50 | 	float hn = 0;
 51 | 	int n0 = n;
 52 | 	float f = 1/((float) b);
 53 | 
 54 | 	while (n0 > 0) {
 55 | 		float n1 = n0/b;
 56 | 		int r = n0 - n1*b;
 57 | 		hn += f*r;
 58 | 		f = f/b;
 59 | 		n0 = n1;
 60 | 	}
 61 | 	return hn;
 62 | }
 63 | 
 64 | 
 65 | 
 66 | __global__ 
 67 | void sampleFree(float* obstacles, int obstaclesCount, float* samples, bool* isFreeSamples, float *debugOutput) 
 68 | {
 69 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
 70 | 	if (node >= NUM) 
 71 | 		return;
 72 | 
 73 | 	float nodeLoc[3];
 74 | 	for (int d = 0; d < 3; ++d) {
 75 | 		nodeLoc[d] = samples[node*DIM+d];
 76 | 	}
 77 | 
 78 | 	for (int obs_idx = 0; obs_idx < obstaclesCount; ++obs_idx) {
 79 | 		bool notFree = true;
 80 | 		for (int d = 0; d < 3; ++d) {
 81 | 			notFree = notFree && 
 82 | 			nodeLoc[d] > obstacles[obs_idx*2*DIM + d] && 
 83 | 			nodeLoc[d] < obstacles[obs_idx*2*DIM + DIM + d];
 84 | 			if (!notFree)
 85 | 				break;
 86 | 		}
 87 | 		if (notFree) {
 88 | 			isFreeSamples[node] = false;
 89 | 			return;
 90 | 		}
 91 | 	}
 92 | 	isFreeSamples[node] = true;
 93 | }
 94 | 
 95 | __global__ 
 96 | void fillSamples(float* samples, float* samplesAll, int* sampleFreeIdx, bool* isFreeSamples, float *debugOutput) 
 97 | {
 98 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
 99 | 	if (node >= NUM) 
100 | 		return;
101 | 	if (!isFreeSamples[node])
102 | 		return;
103 | 
104 | 	for (int d = 0; d < DIM; ++d) {
105 | 		samples[sampleFreeIdx[node]*DIM+d] = samplesAll[node*DIM+d];
106 | 	}
107 | }
108 | 
109 | __global__ 
110 | void createSortHeuristic(float* samples, int initial_idx, float* heuristic, int samplesCount) 
111 | {
112 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
113 | 	if (node >= samplesCount) 
114 | 		return;
115 | 
116 | 	float heuristicValue = 0;
117 | 	for (int d = 0; d < DIM; ++d) {
118 | 		float dist = samples[node*DIM + d] - samples[initial_idx*DIM + d];  
119 | 		heuristicValue += dist*dist;	
120 | 	}
121 | 	for (int d = 0; d < DIM; ++d) {
122 | 		heuristic[node*DIM+d] = heuristicValue;
123 | 	}
124 | }
125 | 
126 | // returns false if not free, true if it is free (i.e., valid)
127 | bool sampleFreePt(float* obstacles, int obstaclesCount, float* sample) 
128 | {
129 | 	float nodeLoc[3];
130 | 	for (int d = 0; d < 3; ++d) {
131 | 		nodeLoc[d] = sample[d];
132 | 	}
133 | 
134 | 	for (int obs_idx = 0; obs_idx < obstaclesCount; ++obs_idx) {
135 | 		bool notFree = true;
136 | 		for (int d = 0; d < 3; ++d) {
137 | 			notFree = notFree && 
138 | 			nodeLoc[d] > obstacles[obs_idx*2*DIM + d] && 
139 | 			nodeLoc[d] < obstacles[obs_idx*2*DIM + DIM + d];
140 | 			if (!notFree)
141 | 				break;
142 | 		}
143 | 		if (notFree) {
144 | 			return false;
145 | 		}
146 | 	}
147 | 	return true;
148 | }
149 | 


--------------------------------------------------------------------------------
/gpu/GMT.cuh:
--------------------------------------------------------------------------------
 1 | /*
 2 | GMT.cuh
 3 | author: Brian Ichter
 4 | 
 5 | Group Marching Tree algorithm
 6 | */
 7 | #pragma once
 8 | 
 9 | #include <thrust/device_vector.h>
10 | #include <thrust/remove.h>
11 | #include <thrust/sort.h>
12 | #include <thrust/scan.h>
13 | 
14 | #include "collisionCheck.cuh"
15 | #include "helper.cuh"
16 | #include "2pBVP.cuh"
17 | 
18 | /***********************
19 | CPU functions
20 | ***********************/
21 | // GMT if lambda < 1 (not currently implemented, but a similar goemetric version is in geometricGMT.cu)
22 | void GMT(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
23 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_samples, int samplesCount,
24 | 	float lambda, float r, float *d_costs, int *d_edges, int initial_idx, int goal_idx);
25 | 
26 | // pregenerated wavefront with initial condition attached
27 | void GMTinit(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
28 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
29 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
30 | 	float *d_costs, int *d_edges, int initial_idx, int goal_idx);
31 | 
32 | // pregenerated wavefront with initial condition attached and moving goal, return goal index
33 | int GMTinitGoal(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
34 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
35 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
36 | 	float *d_costs, int *d_edges, int initial_idx);
37 | 
38 | // GMT with lambda = 1 (i.e. expand entire wavefront at once)
39 | void GMTwavefront(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
40 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
41 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
42 | 	float *d_costs, int *d_edges, int initial_idx, int goal_idx);
43 | 
44 | void outputSolution();
45 | 
46 | /***********************
47 | GPU kernels
48 | ***********************/
49 | // set default values for arrays
50 | __global__ void setupArrays(bool *wavefront, bool *wavefrontNew, bool *wavefrontWas, bool *unvisited, 
51 | 	bool *isFreeSamples, float *costGoal, float *costs, int *edges, int samplesCount);
52 | // find new nodes to expand to
53 | __global__ void findWavefront(bool *unvisited, bool *wavefront, bool *wavefrontNew, bool *wavefrontWas,
54 | 	int *nnEdgesGo, int nnSize, int wavefrontSize, int *wavefrontActiveIdx, float *debugOutput);
55 | // update arrays to represent new wavefront
56 | __global__ void fillWavefront(int samplesCount, int *wavefrontActiveIdx, int *wavefrontScanIdx, bool *wavefront);
57 | // find the optimal connection to the tree
58 | __global__ void findOptimalConnection(bool *wavefront, int *edges, float *costs, float *distancesCome, 
59 | 	int *nnEdgesCome, int nnSize, int wavefrontSize, int *wavefrontActiveIdx, float *debugOutput);
60 | // check if optimal connection is collision free
61 | __global__ void verifyExpansion(int obstaclesCount, bool *wavefrontNew, int *edges,
62 | 	float *samples, float *obstacles, float *costs,
63 | 	int *nnIdxs, float *discMotions, int numDisc, bool *isCollision,
64 | 	int wavefrontSize, int *wavefrontActiveIdx, float *debugOutput);
65 | // remove edges found invalid through verifyExpansion (didn't work in verifyExpansion)
66 | __global__ void removeInvalidExpansions(int obstaclesCount, bool *wavefrontNew, int *edges,
67 | 	float *samples, float *obstacles, float *costs,
68 | 	int *nnIdxs, float *discMotions, int numDisc, bool *isCollision,
69 | 	int wavefrontSize, int *wavefrontActiveIdx, float *debugOutput);
70 | // update arrays to represent new wavefront
71 | __global__ void updateWavefront(int samplesCount, int goal_idx,
72 | 	bool *unvisited, bool *wavefront, bool *wavefrontNew, bool *wavefrontWas,
73 | 	float *costs, float *costGoal, float *debugOutput);
74 | // attach initial to the graph
75 | __global__ void attachWavefront(int samplesCount, float *samples, float *initial, 
76 | 	float r, int goal_idx, bool *wavefront, int *edges,
77 | 	int obstaclesCount, float *obstacles, 
78 | 	float* costs, float* costGoal, float* debugOutput);
79 | // build inGoal array to store samples that are inside the goal region
80 | __global__ void buildInGoal(int samplesCount,float *samples, float *goal, float goalD2,
81 | 	bool *inGoal, float *debugOutput);


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # GMT\*
 2 | This repository houses some example code from the paper "Group Marching Tree: Sampling-Based Approximately Optimal Motion Planning on GPUs" presented at the IEEE International Conference on Robotic Computing (IRC) 2017 by Brian Ichter, Edward Schmerling, and Marco Pavone of Stanford's Autonomous Systems Lab. The paper can be found here <https://arxiv.org/pdf/1705.02403.pdf>.
 3 | 
 4 | ## Disclaimers
 5 | The code contained herein is all of "research grade", in that is messy, largely uncommented, and intended for reference more than anything else. It is unlikely to be useful out of the box, but may help elucidate descriptions in the paper or guide some coding strategies for users implementing GMT\*/similar algorithms. For further disclaimers, please see the __Disclaimer__ section here, <https://github.com/schmrlng/MotionPlanning.jl>.
 6 | 
 7 | ## Instructions
 8 | ### What's included
 9 | CUDA C Code- this is the meat of the algorithm that runs on the GPU.
10 | 
11 | MATLAB Code- this code visualizes solutions from the GPU code (output in `soln.txt`)
12 | 
13 | ### Running the code
14 | The code is run through the `mainGround.cu` (for the double integrator) and `mainDubins.cu` (for the Dubins airplane). To compile, type `make <dubins,ground> NUM=<NUM> DIM=<DIM>`, where `<dubins,ground>` is a Dubins aircraft or a 6D double integrator, `<NUM>` is the number of state space samples, and `<DIM>` is the dimension of the state space (4 for dubins, 6 for double integrator). The code can then be run through `/.ground <file.txt>` where `<file.txt>` is the name of a file which contains the problem specification. Three example problems are given in `input.txt` and `gates.txt` for the double integrator and in `treesDub.txt` for the Dubins aircraft.
15 | 
16 | ### Adding a new dynamical system
17 | To use some of the code framework to add a new system, one would primarily need to add a new CUDA file similar to `2pBVP.cu` and `dubinsAirplane.cu` which performs all local steering functionality. Depending on the geometry of the system, a new collision checker would also be required (if it can be checked with just straight line paths, perhaps discretized to model a larger curve, then the current collision checker should be useable).
18 | 
19 | ## Notes
20 | Much of the offline precomputation is in no way optimized (particularly for Dubins). It is currently implemented on the CPU, but could see an additional ~100x speed up if implemented on the GPU. 
21 | 
22 | Only a group cost threshold factor (lambda) of 1 is implemented for the kinodynamic planning problem, but an example of arbitrary values is shown for the geometric planning problem in `geometricGMT.cu`. The extension should be relatively straight forward. If using a known value of lambda (i.e., once you decide on a value for your problem based on the desire for speed or low-cost), it is likely most performant to hardcode the data structures (as I have done for 1 here). 
23 | 
24 | The collision checker is currently fairly basic, in that it only performs a broadphase AABB check for obstacles. It can however be replaced as needed.
25 | 
26 | ## Contact
27 | If you're using the code, let me know! I'm happy to discuss. You can contact me either through github or email me at <ichter@stanford.edu>.
28 | 
29 | ## License
30 | Copyright 2017 Brian Ichter
31 | 
32 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
33 | 
34 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
35 | 
36 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
37 | 
38 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
39 | 
40 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
41 | 


--------------------------------------------------------------------------------
/viz/solveDoubleInt2PBVP.m:
--------------------------------------------------------------------------------
 1 | function [path, topt, toff, copt] = solveDoubleInt2PBVP(dim,dt,toff,x0,x1)
 2 | % 2d is planar, 3d is with gravity down
 3 | % should probably put a max itr counter on the bisection search
 4 | 
 5 | topt = topt_bisection(x0,x1,10,1e-4,dim);
 6 | T = ceil((topt-toff)/dt);
 7 | path = zeros(dim,T);
 8 | for ti = 1:T
 9 |     t = dt*(ti-1)+toff;
10 |     path(:,ti) = x(t,topt,x0,x1,dim);
11 | end
12 | copt = cost(topt,x0,x1,dim);
13 | toff = (t-topt)+dt;
14 | % if dim == 2
15 | %     plot(path(1,:),path(2,:))
16 | % else
17 | %     plot3(path(1,:),path(2,:),path(3,:))
18 | % end
19 | 
20 |     function x = x(t,tau,x0,x1,dim)
21 |         if dim == 4
22 |             x = [(x0(1)*(2*t + tau)*(t - tau)^2 + t*(x1(1)*t*(3*tau - 2*t) + ...
23 |                     (t - tau)*tau*(t*(x0(3) + x1(3)) - x0(3)*tau)))/tau^3;
24 |                 (x0(2)*(2*t + tau)*(t - tau)^2 + t*(x1(2)*t*(3*tau - 2*t) + ...
25 |                     (t - tau)*tau*(t*(x0(4) + x1(4)) - x0(4)*tau)))/tau^3;
26 |                 (6*x0(1)*t*(t - tau) - 6*x1(1)*t*(t - tau) + ...
27 |                     tau*(3*(x0(3) + x1(3))*t^2 - ...
28 |                     2*(2*x0(3) + x1(3))*tau*t + x0(3)*tau^2))/tau^3;
29 |                 (6*x0(2)*t*(t - tau) - 6*x1(2)*t*(t - tau) + ...
30 |                     tau*(3*(x0(4) + x1(4))*t^2 - 2*(2*x0(4) + x1(4))*tau*t + ...
31 |                     x0(4)*tau^2))/tau^3];
32 |         elseif dim == 6
33 |             x = [t*x0(4) + x0(1) + (t^3*(2*x0(1) - 2*x1(1) + (x0(4) + x1(4))*tau))/tau^3 - ...
34 |                     (t^2*(3*x0(1) - 3*x1(1) + (2*x0(4) + x1(4))*tau))/tau^2;
35 |                 t*x0(5) + x0(2) + (t^3*(2*x0(2) - 2*x1(2) + (x0(5) + x1(5))*tau))/tau^3 - ...
36 |                     (t^2*(3*x0(2) - 3*x1(2) + (2*x0(5) + x1(5))*tau))/tau^2;
37 |                 (t*x0(6)*tau^3 + x0(3)*tau^3 + t^2*tau*(-3.*x0(3) + 3.*x1(3) - 2.*x0(6)*tau - 1.*x1(6)*tau) + ...
38 |                      t^3*(2.*x0(3) - 2.*x1(3) + (x0(6) + x1(6))*tau))/tau^3;
39 |                 -(2*t*(3*x0(1) - 3*x1(1) + x1(4)*tau))/tau^2 + (3*t^2*(2*x0(1) - 2*x1(1) + x1(4)*tau))/tau^3 + ...
40 |                     (x0(4)*(3*t^2 - 4*t*tau + tau^2))/tau^2;
41 |                 -(2*t*(3*x0(2) - 3*x1(2) + x1(5)*tau))/tau^2 + ...
42 |                     (3*t^2*(2*x0(2) - 2*x1(2) + x1(5)*tau))/tau^3 + (x0(5)*(3*t^2 - 4*t*tau + tau^2))/tau^2; 
43 |                 (x0(6)*tau^3 + t*tau*(-6.*x0(3) + 6.*x1(3) - 4.*x0(6)*tau - 2.*x1(6)*tau) + ...
44 |                      t^2*(6.*x0(3) - 6.*x1(3) + 3.*x0(6)*tau + 3.*x1(6)*tau))/tau^3];
45 |         end
46 |     end
47 |     
48 |     function topt = topt_bisection(x0,x1,tmax,TOL,dim)
49 |         tu = tmax;
50 |         if dc(tmax,x0,x1,dim) < 0
51 |             topt = tmax;
52 |             return;
53 |         end
54 |         tl = 0.01;
55 |         while dc(tl,x0,x1,dim) > 0
56 |             tl = tl/2;
57 |         end
58 |         topt = 0;
59 |         dcval = 1;
60 |         while abs(dcval) > TOL
61 |             topt = (tu+tl)/2;
62 |             dcval = dc(topt,x0,x1,dim);
63 |             if dcval > 0
64 |                 tu = topt;
65 |             else
66 |                 tl = topt;
67 |             end
68 |         end
69 |     end
70 | 
71 |     function dc = dc(tau,x0,x1,dim)
72 |         dtau = 1e-6;
73 |         dc = (cost(tau+dtau/2,x0,x1,dim) - cost(tau-dtau/2,x0,x1,dim))/dtau;
74 |     end
75 | 
76 |     function c = cost(tau,x0,x1,dim)
77 |         if dim == 4
78 |             c = (1/(tau^3))*(12*x0(1)^2 + 12*x1(1)^2 + 12*x0(2)^2 - 24*x0(2)*x1(2) + ...
79 |                 12*x1(2)^2 - 12*x1(1)*(x0(3) + x1(3))*tau + 12*x0(2)*x0(4)*tau - ...
80 |                 12*x1(2)*x0(4)*tau + 12*x0(2)*x1(4)*tau - 12*x1(2)*x1(4)*tau + ...
81 |                 4*x0(3)^2*tau^2 + 4*x0(3)*x1(3)*tau^2 + 4*x1(3)^2*tau^2 + ...
82 |                 4*x0(4)^2*tau^2 + 4*x0(4)*x1(4)*tau^2 + ...
83 |                 4*x1(4)^2*tau^2 + tau^4 + 12*x0(1)*(-2*x1(1) + (x0(3) + x1(3))*tau));
84 |         elseif dim == 6
85 |             c = (12*x0(1)^2)/tau^3 + (12*x0(3)^2)/tau^3 + (12*x0(2)^2)/tau^3 - (24*x0(1)*x1(1))/tau^3 + (12*x1(1)^2)/tau^3 - ...
86 |                 (24*x0(3)*x1(3))/tau^3 + (12*x1(3)^2)/tau^3 - (24*x0(2)*x1(2))/tau^3 + (12*x1(2)^2)/tau^3 + (12*x0(4)*x0(1))/tau^2 + ...
87 |                 (12*x0(5)*x0(2))/tau^2 + (12*x0(2)*x1(5))/tau^2 + (12*x0(1)*x1(4))/tau^2 - (12*x0(4)*x1(1))/tau^2 - (12*x1(4)*x1(1))/tau^2 - ...
88 |                 (12*x0(5)*x1(2))/tau^2 - (12*x1(5)*x1(2))/tau^2 + (4*x0(5)^2)/tau + (4*x0(4)^2)/tau + (4*x0(6)^2)/tau + (4*x0(5)*x1(5))/tau + ...
89 |                 (4*x1(5)^2)/tau + (4*x0(4)*x1(4))/tau + (4*x1(4)^2)/tau + (4*x1(6)^2)/tau + 101.*tau + ...
90 |                 (12.*x0(6)*(x0(3) - 1.*x1(3) + (1/3*x1(6) - 5/3*tau)*tau))/tau^2 + ...
91 |                 (12.*x1(6)*(x0(3) - 1.*x1(3) + 5/3*tau^2))/tau^2;
92 |         end
93 |     end
94 | end


--------------------------------------------------------------------------------
/gpu/PRM.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Solve the planning problem with probabilistic roadmaps.
  4 | */
  5 | 
  6 | #include "PRM.cuh"
  7 | 
  8 | 
  9 | void PRM(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
 10 | 	std::vector<float> adjCosts, std::vector<int> nnGoEdges, std::vector<int> nnComeEdges, int nnSize, float *d_discMotions, std::vector<int> nnIdxs,
 11 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc, int numEdges,
 12 | 	std::vector<float> costs, int *d_edges, int initial_idx, int goal_idx,
 13 | 	std::vector<float> c2g)
 14 | {
 15 | 	// instantiate memory for online comp
 16 | 	double t_totalStart = std::clock();
 17 | 
 18 | 	thrust::device_vector<bool> d_isFreeEdges_thrust(numEdges);
 19 | 	bool* d_isFreeEdges = thrust::raw_pointer_cast(d_isFreeEdges_thrust.data());
 20 | 	bool isFreeEdges[numEdges];
 21 | 	bool isFreeSamples[NUM];
 22 | 
 23 | 	// remove edges in collision (fast)
 24 | 	// track these edges and potentially remove from storage?
 25 | 	double t_edgesValidStart = std::clock();
 26 | 	const int blockSizeEdgesValid = 192;
 27 | 	const int gridSizeEdgesValid = std::min((numEdges + blockSizeEdgesValid - 1) / blockSizeEdgesValid, 2147483647);
 28 | 	if (gridSizeEdgesValid == 2147483647)
 29 | 		std::cout << "...... ERROR: increase grid size for freeEdges" << std::endl;
 30 | 	float d_debugOutput[1]; d_debugOutput[0] = 0;
 31 | 	freeEdges<<<gridSizeEdgesValid,blockSizeEdgesValid>>>(
 32 | 		d_obstacles, obstaclesCount, d_samples, 
 33 | 		d_isFreeSamples, numDisc, d_discMotions, 
 34 | 		d_isFreeEdges, numEdges, d_debugOutput);
 35 | 	cudaDeviceSynchronize();
 36 | 	float t_edgesValid = (std::clock() - t_edgesValidStart) / (double) CLOCKS_PER_SEC;
 37 | 	std::cout << "Edge validity check took: " << t_edgesValid << " s";
 38 | 
 39 | 	double t_memTransStart = std::clock();
 40 | 	cudaMemcpy(isFreeEdges, d_isFreeEdges, sizeof(bool)*numEdges, cudaMemcpyDeviceToHost);
 41 | 	cudaMemcpy(isFreeSamples, d_isFreeSamples, sizeof(bool)*NUM, cudaMemcpyDeviceToHost);
 42 | 	
 43 | 	// initialCondition
 44 | 	std::vector<bool> visit(NUM,false); // tracks if a node has been visited
 45 | 
 46 | 	costs[initial_idx] = 0;
 47 | 	std::vector<int> pathIdx(NUM,-1);
 48 | 
 49 | 	double t_memTrans = (std::clock() - t_memTransStart) / (double) CLOCKS_PER_SEC;
 50 | 	double t_sel = 0;
 51 | 	double t_search = 0;
 52 | 	double t_conns = 0;
 53 | 	double t_searchStart = std::clock();
 54 | 
 55 | 	std::set<std::pair<float, int> > myset;
 56 | 	std::set<std::pair<float, int> >::iterator it;
 57 | 
 58 | 	std::pair<float, int> mypair(c2g[initial_idx], initial_idx);
 59 | 	myset.insert(mypair);
 60 | 
 61 | 	for (int i = 0; i < NUM; ++i) {
 62 | 		// select node to visit
 63 | 		double t_selStart = std::clock();
 64 | 		// int cur = -1;
 65 | 		// for (int j = 0; j < NUM; ++j) {
 66 | 		// 	if (visit[j]) continue;
 67 | 		// 	if (cur == -1 || costs[j]+c2g[j] < costs[cur]+c2g[cur]) {
 68 | 		// 		cur = j;
 69 | 		// 	}
 70 | 		// }
 71 | 		it = myset.begin();
 72 | 		int cur = it->second;
 73 | 		myset.erase(it);
 74 | 		t_sel += (std::clock() - t_selStart) / (double) CLOCKS_PER_SEC;
 75 | 		if (cur < 0) break; // none left to visit, i.e. failure
 76 | 		if (cur == goal_idx) break; // we have a solution
 77 | 
 78 | 		// std::cout << "node idx, cur = " << cur << " cost = " << costs[cur] << ", ";
 79 | 
 80 | 		double t_connsStart = std::clock();
 81 | 
 82 | 		visit[cur] = true;
 83 | 		for (int j = 0; j < nnSize; ++j) {
 84 | 			int nodeIdx = nnGoEdges[cur*nnSize + j];
 85 | 			if (nodeIdx < 0 || nodeIdx > NUM) break; // out of neighbors (or overreaching for some bug reason)
 86 | 			if (visit[nodeIdx]) continue; // this node is already set
 87 | 			if (isFreeEdges[nnIdxs[NUM*nodeIdx + cur]]) { // there is an edge and it is free
 88 | 				float pathCost = costs[cur] + adjCosts[cur*NUM + nodeIdx];
 89 | 				if (pathCost < costs[nodeIdx]) {
 90 | 					// reset entry in set for visiting
 91 | 					std::pair<float, int> mypair(costs[nodeIdx] + c2g[nodeIdx], nodeIdx);
 92 | 					it = myset.find(mypair);
 93 | 					myset.erase(mypair);
 94 | 					std::pair<float, int> mypairNew(pathCost + c2g[nodeIdx], nodeIdx);
 95 | 					myset.insert(mypairNew);
 96 | 
 97 | 					costs[nodeIdx] = pathCost;
 98 | 					pathIdx[nodeIdx] = cur;
 99 | 				}
100 | 			}
101 | 		}
102 | 		t_conns += (std::clock() - t_connsStart) / (double) CLOCKS_PER_SEC;
103 | 
104 | 	}
105 | 	t_search += (std::clock() - t_searchStart) / (double) CLOCKS_PER_SEC;
106 | 	double t_total = (std::clock() - t_totalStart) / (double) CLOCKS_PER_SEC;
107 | 
108 | 	std::cout << "timing, selection = " << t_sel << ", connection = " << t_conns << ", and search = " << t_search << ", mem = " << t_memTrans << ", total = " << t_total <<std::endl;
109 | 
110 | 	// std::cout << std::endl << "PRM Costs: " << std::endl;
111 | 	// for (std::vector<float>::const_iterator i = costs.begin(); i != costs.end(); ++i)
112 |     //    	std::cout << *i << ' ';
113 |     //    std::cout << std::endl;
114 | 
115 | 	// std::cout << std::endl << "PRM Tree: " << std::endl;
116 | 	// for (std::vector<int>::const_iterator i = pathIdx.begin(); i != pathIdx.end(); ++i)
117 |  //    	std::cout << *i << ' ';
118 |  //    std::cout << std::endl;
119 | 
120 | 	std::cout << "PRM final cost was " << costs[goal_idx] << std::endl;
121 | }
122 | 
123 | 
124 | 
125 | 


--------------------------------------------------------------------------------
/gpu/helper.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Helper functions like printing, averaging, multiplying, etc.
  4 | */
  5 | 
  6 | #include "helper.cuh"
  7 | 
  8 | void printArray(float* array, int dim1, int dim2, std::ostream& stream)
  9 | {
 10 | 	for (int i = 0; i < dim1; ++i) {
 11 | 		for (int j = 0; j < dim2; ++j) {
 12 | 			stream << array[i*dim2 + j] << ",";
 13 | 		}
 14 | 		stream << std::endl;
 15 | 	}
 16 | }
 17 | 
 18 | void printArray(int* array, int dim1, int dim2, std::ostream& stream)
 19 | {
 20 | 	for (int i = 0; i < dim1; ++i) {
 21 | 		for (int j = 0; j < dim2; ++j) {
 22 | 			stream << array[i*dim2 + j] << ",";
 23 | 		}
 24 | 		stream << std::endl;
 25 | 	}
 26 | }
 27 | 
 28 | void printArray(bool* array, int dim1, int dim2, std::ostream& stream)
 29 | {
 30 | 	for (int i = 0; i < dim1; ++i) {
 31 | 		for (int j = 0; j < dim2; ++j) {
 32 | 			stream << array[i*dim2 + j] << ",";
 33 | 		}
 34 | 		stream << std::endl;
 35 | 	}
 36 | }
 37 | 
 38 | void printArray(char* array, int dim1, int dim2, std::ostream& stream)
 39 | {
 40 | 	for (int i = 0; i < dim1; ++i) {
 41 | 		for (int j = 0; j < dim2; ++j) {
 42 | 			stream << array[i*dim2 + j] << ",";
 43 | 		}
 44 | 		stream << std::endl;
 45 | 	}
 46 | }
 47 | 
 48 | float avgArray(float* array, int length)
 49 | {
 50 | 	int n = 0;
 51 | 	float sum = 0;
 52 | 	for (int i = 0; i < length; ++i) {
 53 | 		if (array[i] > 0.0001) {
 54 | 			++n;
 55 | 			sum += array[i];
 56 | 		}
 57 | 	}
 58 | 	std::cout << "sum is " << sum << " and " << n <<  " of " << length << " remain" << std::endl;
 59 |  	return sum/n;
 60 | }
 61 | 
 62 | void copyArray(float* arrayTo, float* arrayFrom, int length)
 63 | {
 64 | 	for (int i = 0; i < length; ++i) {
 65 | 		arrayTo[i] = arrayFrom[i];
 66 | 	}
 67 | }
 68 | 
 69 | float calculateUnitBallVolume(int dim) 
 70 | {
 71 | 	if (dim == 0) 
 72 | 		return 1;
 73 | 	else if (dim == 1)
 74 | 		return 2;
 75 | 	return 2*M_PI/dim*calculateUnitBallVolume(dim-2);
 76 | }
 77 | 
 78 | float calculateConnectionBallRadius(int dim, int samplesCount)
 79 | {
 80 | 	float coverage = 0.0;
 81 | 	float eta = 0.5;
 82 | 	float dim_recip = 1.0/dim;
 83 | 	float gammaval = (1.0+eta)*2.0*std::pow(dim_recip,dim_recip)*std::pow(((1-coverage)/calculateUnitBallVolume(dim)),dim_recip);
 84 | 	float scalingFn = log((float) samplesCount)/samplesCount;
 85 | 	return gammaval * std::pow(scalingFn, dim_recip);
 86 | }
 87 | 
 88 | void multiplyArrays(float* A, float* B, float* C, int rowsA, int colsA, int rowsB, int colsB)
 89 | {
 90 | 	if (colsA != rowsB)
 91 | 		std::cout << " ERROR: Matrix dimensions do not match: [" << rowsA << "x" << colsA << "] * [" << rowsB << "x" << colsB << "]" << std::cout;
 92 | 	int inners = colsA;
 93 | 	for (int i = 0; i < rowsA; ++i)
 94 | 	    for (int j = 0; j < colsB; ++j) {
 95 | 		    C[i*colsB+j]=0;
 96 | 		    for (int k = 0; k < inners; ++k)
 97 | 		    	C[i*colsB+j] += A[i*inners+k]*B[k*colsB+j];
 98 | 		}
 99 | }
100 | 
101 | void scalarMultiplyArray(float *A, float b, float *C, int rows, int cols) {
102 | 	for (int i = 0; i < rows; ++i)
103 | 	    for (int j = 0; j < cols; ++j)
104 | 		    C[i*cols+j] = b*A[i*cols+j];
105 | }
106 | 
107 | void subtractArrays(float* A, float* B, float* C, int rows, int cols)
108 | {
109 | 	for (int i = 0; i < rows; ++i)
110 | 	    for (int j = 0; j < cols; ++j)
111 | 		    C[i*cols+j] = A[i*cols+j] - B[i*cols+j];
112 | }
113 | 
114 | void horizConcat(float* A, float *B, float *C, int nA, int nB, int m)
115 | {
116 | 	int n = nA + nB;
117 | 	for (int i = 0; i < m; ++i) {
118 | 	    for (int j = 0; j < nA; ++j)
119 | 	    	C[i*n+j] = A[i*nA+j];
120 | 	    for (int j = nA; j < n; ++j)
121 | 	    	C[i*n+j] = B[i*nB+(j-nA)];
122 | 	}
123 | }
124 | 
125 | void vertConcat(float* A, float *B, float *C, int n, int mA, int mB) 
126 | {
127 | 	int m = mA + mB;
128 | 	for (int i = 0; i < n; ++i) {
129 | 		for (int j = 0; j < mA; ++j)
130 | 			C[j*n+i] = A[j*n+i];
131 | 		for (int j = mA; j < m; ++j)
132 | 			C[j*n+i] = B[(j-mA)*n+i];
133 | 	}
134 | }
135 | 
136 | void transpose(float* A, int rows, int cols) 
137 | {
138 | 	float trans[rows*cols]; 
139 | 	for (int i = 0; i < rows; ++i)
140 | 		for (int j = 0; j < cols; ++j)
141 | 			trans[i + j*rows] = A[j + i*cols];	
142 | 	for (int i = 0; i < rows; ++i)
143 | 		for (int j = 0; j < cols; ++j)
144 | 			A[j + i*cols] = trans[j + i*cols];
145 | }
146 | 
147 | void transpose(float* Atrans, float* A, int rows, int cols) 
148 | {
149 | 	float trans[rows*cols]; 
150 | 	for (int i = 0; i < rows; ++i)
151 | 		for (int j = 0; j < cols; ++j)
152 | 			trans[i + j*rows] = A[j + i*cols];	
153 | 	for (int i = 0; i < rows; ++i)
154 | 		for (int j = 0; j < cols; ++j)
155 | 			Atrans[j + i*cols] = trans[j + i*cols];
156 | }
157 | 
158 | int printSolution(int samplesCount, float *d_samples, int *d_edges, float *d_costs) 
159 | {
160 | 	std::ofstream solnFile;
161 | 	solnFile.open("soln.txt");
162 | 
163 | 	int edges[samplesCount];
164 | 	float costs[samplesCount];
165 | 	float samples[samplesCount*DIM];
166 | 	cudaMemcpy(edges, d_edges, sizeof(int)*samplesCount, cudaMemcpyDeviceToHost);
167 | 	cudaMemcpy(costs, d_costs, sizeof(float)*samplesCount, cudaMemcpyDeviceToHost);
168 | 	cudaMemcpy(samples, d_samples, sizeof(float)*DIM*samplesCount, cudaMemcpyDeviceToHost);
169 | 	solnFile << "edges = [";
170 | 	printArray(edges, 1, samplesCount, solnFile);
171 | 	solnFile << "];" << std::endl << "samples = [";
172 | 	printArray(samples, samplesCount, DIM, solnFile);
173 | 	solnFile << "];" << std::endl << "costs = [";
174 | 	printArray(costs, 1, samplesCount, solnFile);
175 | 	solnFile << "];" << std::endl;
176 | 
177 | 	solnFile.close();
178 | 	return 0;
179 | }


--------------------------------------------------------------------------------
/viz/dubinsCar.m:
--------------------------------------------------------------------------------
  1 | % time optimal dubins car path, 2d (x, y, theta)
  2 | function [path, cmin] = dubinsCar(x0,x1,numDisc,v,thetadotBound)
  3 | v = x1(1:2) - x0(1:2);
  4 | d = norm(v); % distance
  5 | th = atan2(v(2),v(1)); % angle between spatial locations
  6 | a = mod2piF(x0(3) - th); % angular difference from line
  7 | b = mod2piF(x1(3) - th); % angular difference from line
  8 | 
  9 | dt = 0.01;
 10 | 
 11 | path = [];
 12 | cmin = inf;
 13 | [path, cmin] = dubinsLSL(d, a, b, cmin, path);
 14 | [path, cmin] = dubinsRSR(d, a, b, cmin, path);
 15 | [path, cmin] = dubinsRSL(d, a, b, cmin, path);
 16 | [path, cmin] = dubinsLSR(d, a, b, cmin, path);
 17 | [path, cmin] = dubinsRLR(d, a, b, cmin, path);
 18 | [path, cmin] = dubinsLRL(d, a, b, cmin, path);
 19 | path = transformPath(path, x0);
 20 | end
 21 | 
 22 | function path = transformPath(path, xinit)
 23 |     th = xinit(3);
 24 |     path(:,1:2) = path(:,1:2)*[cos(th) sin(th); -sin(th) cos(th)];
 25 |     path = path + repmat(xinit,size(path,1),1);
 26 | end
 27 | %% Control
 28 | function path = carControl(type, d, varargin)
 29 |     numDisc = 8;
 30 |     if length(varargin) == 1
 31 |         numDisc = varargin{1};
 32 |     end
 33 |     t = linspace(0, d, numDisc)';
 34 |     if type < 0 % right turn
 35 |         path = [sin(t) cos(t)-1 -t];
 36 |     elseif type > 0 % left turn 
 37 |         path = [sin(t) -cos(t)+1 t];
 38 |     else % straight line
 39 |         path = [t zeros(size(t)) zeros(size(t))]; 
 40 |     end    
 41 | end
 42 | %% Word functions
 43 | function [path, c] = dubinsLSL(d, a, b, c, path)
 44 | ca = cos(a); sa = sin(a); cb = cos(b); sb = sin(b);
 45 | tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sa - sb));
 46 | if tmp < 0
 47 |     return;
 48 | end
 49 | th = atan2(cb - ca, d + sa - sb);
 50 | t = mod2piF(-a + th);
 51 | p = sqrt(max(tmp, 0));
 52 | q = mod2piF(b - th);
 53 | cnew = t + p + q;
 54 | disp(['LSL: (' num2str(t) ', ' num2str(p) ', ' num2str(q) '), c = ' num2str(cnew)]); 
 55 | if (cnew <= c)
 56 |     c = cnew;
 57 |     path1 = carControl(1, t);
 58 |     path2 = transformPath(carControl(0, p), path1(size(path1,1),:));
 59 |     path3 = transformPath(carControl(1, q), path2(size(path2,1),:));
 60 |     path = [path1; path2; path3];
 61 | end
 62 | end
 63 | 
 64 | function [path, c] = dubinsRSR(d, a, b, c, path)
 65 | ca = cos(a); sa = sin(a); cb = cos(b); sb = sin(b);
 66 | tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sb - sa));
 67 | if tmp < 0
 68 |     return;
 69 | end
 70 | th = atan2(ca - cb, d - sa + sb);
 71 | t = mod2piF(a - th);
 72 | p = sqrt(max(tmp, 0));
 73 | q = mod2piF(-b + th);
 74 | cnew = t + p + q;
 75 | disp(['RSR: (' num2str(t) ', ' num2str(p) ', ' num2str(q) '), c = ' num2str(cnew)]); 
 76 | if (cnew <= c)
 77 |     c = cnew;
 78 |     path1 = carControl(-1, t);
 79 |     path2 = transformPath(carControl(0, p), path1(size(path1,1),:));
 80 |     path3 = transformPath(carControl(-1, q), path2(size(path2,1),:));
 81 |     path = [path1; path2; path3];
 82 | end
 83 | end
 84 | 
 85 | function [path, c] = dubinsRSL(d, a, b, c, path)
 86 | ca = cos(a); sa = sin(a); cb = cos(b); sb = sin(b);
 87 | tmp = d * d - 2 + 2 * (ca*cb + sa*sb - d * (sa + sb));
 88 | if tmp < 0
 89 |     return;
 90 | end
 91 | p = sqrt(max(tmp, 0));
 92 | th = atan2(ca + cb, d - sa - sb) - atan2(2, p);
 93 | t = mod2piF(a - th);
 94 | q = mod2piF(b - th);
 95 | cnew = t + p + q;
 96 | disp(['RSL: (' num2str(t) ', ' num2str(p) ', ' num2str(q) '), c = ' num2str(cnew)]); 
 97 | if (cnew <= c)
 98 |     c = cnew;
 99 |     path1 = carControl(-1, t);
100 |     path2 = transformPath(carControl(0, p), path1(size(path1,1),:));
101 |     path3 = transformPath(carControl(1, q), path2(size(path2,1),:));
102 |     path = [path1; path2; path3];
103 | end
104 | end
105 | 
106 | function [path, c] = dubinsLSR(d, a, b, c, path)
107 | ca = cos(a); sa = sin(a); cb = cos(b); sb = sin(b);
108 | tmp = -2 + d * d + 2 * (ca*cb + sa*sb + d * (sa + sb));
109 | if tmp < 0
110 |     return;
111 | end
112 | p = sqrt(max(tmp, 0));
113 | th = atan2(-ca - cb, d + sa + sb) - atan2(-2, p);
114 | t = mod2piF(-a + th);
115 | q = mod2piF(-b + th);
116 | cnew = t + p + q;
117 | disp(['LSR: (' num2str(t) ', ' num2str(p) ', ' num2str(q) '), c = ' num2str(cnew)]); 
118 | if (cnew <= c)
119 |     c = cnew;
120 |     path1 = carControl(1, t);
121 |     path2 = transformPath(carControl(0, p), path1(size(path1,1),:));
122 |     path3 = transformPath(carControl(-1, q), path2(size(path2,1),:));
123 |     path = [path1; path2; path3];
124 | end
125 | end
126 | 
127 | function [path, c] = dubinsRLR(d, a, b, c, path)
128 | ca = cos(a); sa = sin(a); cb = cos(b); sb = sin(b);
129 | tmp = (6 - d * d  + 2 * (ca*cb + sa*sb + d * (sa - sb))) / 8;
130 | if abs(tmp) >= 1
131 |     return;
132 | end
133 | p = 2*pi - acos(tmp);
134 | th = atan2(ca - cb, d - sa + sb);
135 | t = mod2piF(a - th + p/2);
136 | q = mod2piF(a - b - t + p);
137 | cnew = t + p + q;
138 | disp(['RLR: (' num2str(t) ', ' num2str(p) ', ' num2str(q) '), c = ' num2str(cnew)]); 
139 | if (cnew <= c)
140 |     c = cnew;
141 |     path1 = carControl(-1, t);
142 |     path2 = transformPath(carControl(1, p), path1(size(path1,1),:));
143 |     path3 = transformPath(carControl(-1, q), path2(size(path2,1),:));
144 |     path = [path1; path2; path3];
145 | end
146 | end
147 | 
148 | function [path, c] = dubinsLRL(d, a, b, c, path)
149 | ca = cos(a); sa = sin(a); cb = cos(b); sb = sin(b);
150 | tmp = (6 - d * d  + 2 * (ca*cb + sa*sb - d * (sa - sb))) / 8;
151 | if abs(tmp) >= 1
152 |     return;
153 | end
154 | p = 2*pi - acos(tmp);
155 | th = atan2(-ca + cb, d + sa - sb);
156 | t = mod2piF(-a + th + p/2);
157 | q = mod2piF(b - a - t + p);
158 | cnew = t + p + q;
159 | disp(['LRL: (' num2str(t) ', ' num2str(p) ', ' num2str(q) '), c = ' num2str(cnew)]); 
160 | if (cnew <= c)
161 |     c = cnew;
162 |     path1 = carControl(1, t);
163 |     path2 = transformPath(carControl(-1, p), path1(size(path1,1),:));
164 |     path3 = transformPath(carControl(1, q), path2(size(path2,1),:));
165 |     path = [path1; path2; path3];
166 | end
167 | end
168 | %% Helper functions
169 | function ang = mod2piF(ang)
170 |     ang = mod(ang, 2*pi);
171 | end
172 | 
173 | %%


--------------------------------------------------------------------------------
/gpu/2pBVP.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Solves a two point boundary value problem for double integrator dynamics given a mixed time/control effort
  4 | cost function.
  5 | */
  6 | #include "2pBVP.cuh"
  7 | 
  8 | const float g = -9.81;
  9 | 
 10 | // fill splitPath with the optimal path along waypoints and return the time of the path
 11 | float findOptimalPath(float dt, float *splitPath, float *xs, int numWaypoints, int *pathLength)
 12 | {
 13 |     float tmax = 5;
 14 |     float toff = 0;
 15 |     float topt = 0;
 16 |     float x0[DIM], x1[DIM];
 17 |     int Tprev = 0;
 18 |     float t = 0;
 19 |     float ttotal = 0;
 20 |     for (int i = 0; i < numWaypoints-1; ++i) {
 21 |         // load waypoints into x0 and x1
 22 |         for (int d = 0; d < DIM; ++d) {
 23 |             x0[d] = xs[d + i*DIM];
 24 |             x1[d] = xs[d + (i+1)*DIM];
 25 |         }
 26 |         // perform bisection search to find optimal time
 27 |         topt = toptBisection(x0, x1, tmax);
 28 |         // std::cout << "optimal time is " << topt << std::endl;
 29 |         // load up the splitPath array with the path
 30 |         int T = (topt-toff)/dt + 1;
 31 |         for (int tidx = 0; tidx < T; ++tidx) {
 32 |             t = dt*tidx + toff;
 33 |             pathPoint(t, topt, x0, x1, &splitPath[Tprev*DIM + tidx*DIM]);
 34 |         }
 35 |         // update timing
 36 |         Tprev += T;
 37 |         toff = (t-topt) + dt;
 38 |         ttotal += topt;
 39 |     }
 40 |     *pathLength = Tprev;
 41 |     return ttotal;
 42 | }
 43 | 
 44 | // given the optimal time, fill splitPath
 45 | void discretizePath(float *splitPath, float *x0, float *x1, int numDisc, float topt)
 46 | {
 47 |     float dt = topt/numDisc;
 48 |     // load up start and end points
 49 |     for (int d = 0; d < DIM; ++d) { 
 50 |         splitPath[d] = x0[d];
 51 |         splitPath[d + numDisc*DIM] = x1[d];
 52 |     }
 53 | 
 54 |     // load way points
 55 |     for (int i = 1; i < numDisc; ++i) {
 56 |         pathPoint(dt*i, topt, x0, x1, &splitPath[i*DIM]);
 57 |     }
 58 | }
 59 | 
 60 | // find the optimal path between two points
 61 | __device__ __host__
 62 | float findDiscretizedPath(float *splitPath, float *x0, float *x1, int numDisc)
 63 | {
 64 |     float tmax = 20;
 65 |     float topt =toptBisection(x0, x1, tmax);
 66 |     
 67 |     float dt = topt/numDisc;
 68 |     for (int d = 0; d < DIM; ++d) {
 69 |         splitPath[d] = x0[d];
 70 |         splitPath[d + numDisc*DIM] = x1[d];
 71 |     }
 72 | 
 73 |     for (int i = 1; i < numDisc; ++i) {
 74 |         pathPoint(dt*i, topt, x0, x1, &splitPath[i*DIM]);
 75 |     }
 76 | 
 77 |     return topt;
 78 | }
 79 | 
 80 | // bisection search to find the time for the optimal path
 81 | __device__ __host__
 82 | float toptBisection(float *x0, float *x1, float tmax)
 83 | {
 84 |     float TOL = 0.0001;
 85 | 	float tu = tmax;
 86 |     if (dcost(tmax,x0,x1) < 0) {
 87 |         return tmax;
 88 |     }
 89 |     float tl = 0.01;
 90 |     while (dcost(tl,x0,x1) > 0) {
 91 |         tl = tl/2;
 92 |     }
 93 |     float topt = 0;
 94 |     float dcval = 1;
 95 |     int maxItrs = 50;
 96 |     int itr = 0;
 97 |     while (abs(dcval) > TOL && itr < maxItrs && ++itr) {
 98 |         topt = (tu+tl)/2;
 99 |         dcval = dcost(topt,x0,x1);
100 |         if (dcval > 0)
101 |             tu = topt;
102 |         else 
103 |             tl = topt;
104 |     }
105 | 	return topt;
106 | }
107 | 
108 | // compute the cost for the optimal path
109 | __device__ __host__
110 | float cost(float tau, float *x0, float *x1)
111 | {
112 | 	float cost = (1/pow(tau,3))*(12*pow(x0[0],2) + 12*pow(x0[2],2) + 12*pow(x0[1],2) + 12*pow(x1[0],2) + 12*pow(x1[2],2) - 24*x0[1]*x1[1] + 
113 | 	    12*pow(x1[1],2) + 12*x0[4]*x0[1]*tau + 12*x0[1]*x1[4]*tau - 12*x0[3]*x1[0]*tau - 12*x1[3]*x1[0]*tau - 
114 | 	    12*x0[5]*x1[2]*tau - 12*x1[5]*x1[2]*tau - 12*x0[4]*x1[1]*tau - 12*x1[4]*x1[1]*tau + 4*pow(x0[4],2)*pow(tau,2) + 
115 | 	    4*pow(x0[3],2)*pow(tau,2) + 4*pow(x0[5],2)*pow(tau,2) + 4*x0[4]*x1[4]*pow(tau,2) + 4*pow(x1[4],2)*pow(tau,2) + 4*x0[3]*x1[3]*pow(tau,2) + 
116 | 	    4*pow(x1[3],2)*pow(tau,2) + 4*x0[5]*x1[5]*pow(tau,2) + 4*pow(x1[5],2)*pow(tau,2) + 2*g*x0[5]*pow(tau,3) - 2*g*x1[5]*pow(tau,3) + pow(tau,4) + 
117 | 	    pow(g,2)*pow(tau,4) + 12*x0[0]*(-2*x1[0] + (x0[3] + x1[3])*tau) + 12*x0[2]*(-2*x1[2] + (x0[5] + x1[5])*tau));
118 | 	return cost;
119 | }
120 | 
121 | // derivative of cost
122 | __device__ __host__
123 | float dcost(float tau, float *x0, float *x1)
124 | {
125 |     float dtau = 0.000001;
126 | 	float dcost = (cost(tau+dtau/2,x0,x1) - cost(tau-dtau/2,x0,x1))/dtau;
127 | 	return dcost;
128 | }
129 | 
130 | // fills out the state along a given path at time tau
131 | __device__ __host__
132 | void pathPoint(float t, float tau, float *x0, float *x1, float *x)
133 | {
134 | 	x[0] = t*x0[3] + x0[0] + (pow(t,3)*(2*x0[0] - 2*x1[0] + (x0[3] + x1[3])*tau))/pow(tau,3) - 
135 |             (pow(t,2)*(3*x0[0] - 3*x1[0] + (2*x0[3] + x1[3])*tau))/pow(tau,2);
136 | 	x[1] = t*x0[4] + x0[1] + (pow(t,3)*(2*x0[1] - 2*x1[1] + (x0[4] + x1[4])*tau))/pow(tau,3) - 
137 |             (pow(t,2)*(3*x0[1] - 3*x1[1] + (2*x0[4] + x1[4])*tau))/pow(tau,2);
138 | 	x[2] = (t*x0[5]*pow(tau,3) + x0[2]*pow(tau,3) + pow(t,2)*tau*(-3.*x0[2] + 3.*x1[2] - 2.*x0[5]*tau - 1.*x1[5]*tau) + 
139 |              pow(t,3)*(2.*x0[2] - 2.*x1[2] + (x0[5] + x1[5])*tau))/pow(tau,3);
140 | 	x[3] = -(2*t*(3*x0[0] - 3*x1[0] + x1[3]*tau))/pow(tau,2) + (3*pow(t,2)*(2*x0[0] - 2*x1[0] + x1[3]*tau))/pow(tau,3) + 
141 |             (x0[3]*(3*pow(t,2) - 4*t*tau + pow(tau,2)))/pow(tau,2);
142 | 	x[4] = -(2*t*(3*x0[1] - 3*x1[1] + x1[4]*tau))/pow(tau,2) + 
143 |             (3*pow(t,2)*(2*x0[1] - 2*x1[1] + x1[4]*tau))/pow(tau,3) + (x0[4]*(3*pow(t,2) - 4*t*tau + pow(tau,2)))/pow(tau,2), 
144 | 	x[5] = (x0[5]*pow(tau,3) + t*tau*(-6.*x0[2] + 6.*x1[2] - 4.*x0[5]*tau - 2.*x1[5]*tau) + 
145 |              pow(t,2)*(6.*x0[2] - 6.*x1[2] + 3.*x0[5]*tau + 3.*x1[5]*tau))/pow(tau,3);
146 | }
147 | 
148 | __global__ 
149 | void fillCoptsTopts(float *samples, float *copts, float *topts, float tmax) {
150 |     int tid = blockIdx.x * blockDim.x + threadIdx.x;
151 |     if (tid >= NUM*NUM)
152 |         return;
153 | 
154 |     int i = tid/NUM;
155 |     int j = tid%NUM;
156 |     if (i == j) // connection to the same node
157 |         return;
158 | 
159 |     int idx = i*(NUM-1)+j;
160 |     if (i < j) 
161 |         idx--;
162 |     
163 |     topts[idx] = toptBisection(&(samples[i*DIM]), &(samples[j*DIM]), tmax);
164 |     copts[idx] = cost(topts[idx], &(samples[i*DIM]), &(samples[j*DIM]));
165 | }


--------------------------------------------------------------------------------
/gpu/treesDub.txt:
--------------------------------------------------------------------------------
  1 | 0.1,0.1,0.1,1.571,
  2 | 4.9,4.9,4.0,1.571,
  3 | 0,0,0,0,
  4 | 5,5,5,6.28319,
  5 | 158,
  6 | 2.435,1.6292,0,-15.5,2.5649,1.7042,1,50,
  7 | 2.4625,1.6017,0,-15.5,2.5375,1.7316,1,50,
  8 | 2.2402,1.5166,1,-15.5,2.7598,1.8166,3,50,
  9 | 2.35,1.4068,1,-15.5,2.65,1.9264,3,50,
 10 | 2.3701,1.5917,3,-15.5,2.6299,1.7416,5,50,
 11 | 2.425,1.5368,3,-15.5,2.575,1.7966,5,50,
 12 | 1.2175,3.3146,0,-15.5,1.2825,3.3521,3.75,50,
 13 | 1.2312,3.3009,0,-15.5,1.2687,3.3658,3.75,50,
 14 | 0.92525,3.1459,2.5,-15.5,1.5748,3.5208,5,50,
 15 | 1.0625,3.0085,2.5,-15.5,1.4375,3.6581,5,50,
 16 | 3.6851,0.51805,0,-15.5,3.8149,0.59305,1,50,
 17 | 3.7125,0.49061,0,-15.5,3.7875,0.6205,1,50,
 18 | 3.4902,0.40555,1,-15.5,4.0098,0.70555,3,50,
 19 | 3.6,0.29575,1,-15.5,3.9,0.81535,3,50,
 20 | 3.6201,0.48056,3,-15.5,3.8799,0.63055,5,50,
 21 | 3.675,0.42565,3,-15.5,3.825,0.68545,5,50,
 22 | 0.5925,2.2035,0,-15.5,0.6575,2.2409,3.75,50,
 23 | 0.60625,2.1898,0,-15.5,0.64375,2.2547,3.75,50,
 24 | 0.30024,2.0347,2.5,-15.5,0.94975,2.4097,5,50,
 25 | 0.4375,1.8975,2.5,-15.5,0.8125,2.547,5,50,
 26 | 3.0925,3.8701,0,-15.5,3.1575,3.9076,3.75,50,
 27 | 3.1062,3.8564,0,-15.5,3.1438,3.9214,3.75,50,
 28 | 2.8003,3.7014,2.5,-15.5,3.4497,4.0764,5,50,
 29 | 2.9375,3.5641,2.5,-15.5,3.3125,4.2136,5,50,
 30 | 1.81,1.0736,0,-15.5,1.94,1.1486,1,50,
 31 | 1.8375,1.0461,0,-15.5,1.9125,1.1761,1,50,
 32 | 1.6152,0.9611,1,-15.5,2.1348,1.2611,3,50,
 33 | 1.725,0.8513,1,-15.5,2.025,1.3709,3,50,
 34 | 1.7451,1.0361,3,-15.5,2.0049,1.1861,5,50,
 35 | 1.8,0.9812,3,-15.5,1.95,1.241,5,50,
 36 | 4.3101,2.7403,0,-15.5,4.4399,2.8153,1,50,
 37 | 4.3375,2.7128,0,-15.5,4.4125,2.8428,1,50,
 38 | 4.1152,2.6278,1,-15.5,4.6348,2.9278,3,50,
 39 | 4.225,2.5179,1,-15.5,4.525,3.0376,3,50,
 40 | 4.2451,2.7028,3,-15.5,4.5049,2.8528,5,50,
 41 | 4.3,2.6479,3,-15.5,4.45,2.9077,5,50,
 42 | 0.28002,4.4257,0,-15.5,0.34498,4.4632,3.75,50,
 43 | 0.29375,4.412,0,-15.5,0.33125,4.4769,3.75,50,
 44 | -0.012259,4.2569,2.5,-15.5,0.63725,4.6319,5,50,
 45 | 0.125,4.1197,2.5,-15.5,0.5,4.7692,5,50,
 46 | 2.78,0.16643,0,-15.5,2.845,0.20393,3.75,50,
 47 | 2.7937,0.15271,0,-15.5,2.8313,0.21766,3.75,50,
 48 | 2.4878,-0.0023148,2.5,-15.5,3.1372,0.37269,5,50,
 49 | 2.625,-0.13958,2.5,-15.5,3,0.50995,5,50,
 50 | 1.4975,1.8144,0,-15.5,1.6275,1.8893,1,50,
 51 | 1.525,1.7869,0,-15.5,1.6,1.9168,1,50,
 52 | 1.3027,1.7019,1,-15.5,1.8223,2.0019,3,50,
 53 | 1.4125,1.5921,1,-15.5,1.7125,2.1117,3,50,
 54 | 1.4326,1.7769,3,-15.5,1.6924,1.9268,5,50,
 55 | 1.4875,1.7219,3,-15.5,1.6375,1.9817,5,50,
 56 | 4.03,3.4997,0,-15.5,4.095,3.5373,3.75,50,
 57 | 4.0438,3.4861,0,-15.5,4.0812,3.551,3.75,50,
 58 | 3.7378,3.331,2.5,-15.5,4.3872,3.706,5,50,
 59 | 3.875,3.1938,2.5,-15.5,4.25,3.8433,5,50,
 60 | 0.87255,0.70325,0,-15.5,1.0025,0.77825,1,50,
 61 | 0.9,0.6758,0,-15.5,0.975,0.8057,1,50,
 62 | 0.6777,0.59075,1,-15.5,1.1973,0.89075,3,50,
 63 | 0.7875,0.48094,1,-15.5,1.0875,1.0006,3,50,
 64 | 0.8076,0.66575,3,-15.5,1.0674,0.81575,5,50,
 65 | 0.8625,0.61085,3,-15.5,1.0125,0.87065,5,50,
 66 | 3.3726,2.3699,0,-15.5,3.5024,2.4449,1,50,
 67 | 3.4,2.3424,0,-15.5,3.475,2.4724,1,50,
 68 | 3.1777,2.2574,1,-15.5,3.6973,2.5574,3,50,
 69 | 3.2875,2.1476,1,-15.5,3.5875,2.6672,3,50,
 70 | 3.3076,2.3324,3,-15.5,3.5674,2.4824,5,50,
 71 | 3.3625,2.2775,3,-15.5,3.5125,2.5373,5,50,
 72 | 2.1225,4.0366,0,-15.5,2.2525,4.1116,1,50,
 73 | 2.15,4.0091,0,-15.5,2.225,4.1391,1,50,
 74 | 1.9277,3.9241,1,-15.5,2.4473,4.2241,3,50,
 75 | 2.0375,3.8143,1,-15.5,2.3375,4.3339,3,50,
 76 | 2.0576,3.999,3,-15.5,2.3174,4.149,5,50,
 77 | 2.1125,3.9442,3,-15.5,2.2625,4.204,5,50,
 78 | 4.6226,1.2588,0,-15.5,4.7524,1.3338,1,50,
 79 | 4.65,1.2313,0,-15.5,4.725,1.3613,1,50,
 80 | 4.4277,1.1463,1,-15.5,4.9473,1.4463,3,50,
 81 | 4.5375,1.0365,1,-15.5,4.8375,1.5561,3,50,
 82 | 4.5576,1.2213,3,-15.5,4.8174,1.3713,5,50,
 83 | 4.6125,1.1664,3,-15.5,4.7625,1.4262,5,50,
 84 | 0.0913,2.9255,0,-15.5,0.2212,3.0004,1,50,
 85 | 0.11875,2.898,0,-15.5,0.19375,3.0279,1,50,
 86 | -0.10356,2.813,1,-15.5,0.41606,3.1129,3,50,
 87 | 0.00625,2.7032,1,-15.5,0.30625,3.2227,3,50,
 88 | 0.026346,2.888,3,-15.5,0.28615,3.0379,5,50,
 89 | 0.08125,2.833,3,-15.5,0.23125,3.0928,5,50,
 90 | 2.5913,4.5922,0,-15.5,2.7212,4.6671,1,50,
 91 | 2.6188,4.5647,0,-15.5,2.6937,4.6946,1,50,
 92 | 2.3964,4.4797,1,-15.5,2.9161,4.7797,3,50,
 93 | 2.5062,4.3698,1,-15.5,2.8063,4.8895,3,50,
 94 | 2.5263,4.5547,3,-15.5,2.7862,4.7046,5,50,
 95 | 2.5812,4.4998,3,-15.5,2.7313,4.7595,5,50,
 96 | 1.3738,0.35162,0,-15.5,1.4387,0.38912,3.75,50,
 97 | 1.3875,0.3379,0,-15.5,1.425,0.40285,3.75,50,
 98 | 1.0815,0.18287,2.5,-15.5,1.731,0.55785,5,50,
 99 | 1.2188,0.045611,2.5,-15.5,1.5938,0.69515,5,50,
100 | 3.8413,1.9995,0,-15.5,3.9712,2.0745,1,50,
101 | 3.8688,1.9721,0,-15.5,3.9437,2.102,1,50,
102 | 3.6464,1.8871,1,-15.5,4.1661,2.1871,3,50,
103 | 3.7562,1.7772,1,-15.5,4.0563,2.2969,3,50,
104 | 3.7763,1.962,3,-15.5,4.0362,2.112,5,50,
105 | 3.8312,1.9071,3,-15.5,3.9813,2.1669,5,50,
106 | 0.7163,3.6662,0,-15.5,0.8462,3.7412,1,50,
107 | 0.74375,3.6387,0,-15.5,0.81875,3.7687,1,50,
108 | 0.52145,3.5537,1,-15.5,1.0411,3.8537,3,50,
109 | 0.63125,3.4439,1,-15.5,0.93125,3.9635,3,50,
110 | 0.65135,3.6287,3,-15.5,0.91115,3.7787,5,50,
111 | 0.70625,3.5738,3,-15.5,0.85625,3.8336,5,50,
112 | 3.2488,0.9072,0,-15.5,3.3137,0.9447,3.75,50,
113 | 3.2625,0.89345,0,-15.5,3.3,0.9584,3.75,50,
114 | 2.9565,0.73845,2.5,-15.5,3.606,1.1135,5,50,
115 | 3.0938,0.60115,2.5,-15.5,3.4688,1.2507,5,50,
116 | 1.9663,2.5551,0,-15.5,2.0962,2.6301,1,50,
117 | 1.9937,2.5277,0,-15.5,2.0688,2.6576,1,50,
118 | 1.7714,2.4426,1,-15.5,2.2911,2.7426,3,50,
119 | 1.8812,2.3328,1,-15.5,2.1813,2.8524,3,50,
120 | 1.9013,2.5176,3,-15.5,2.1612,2.6676,5,50,
121 | 1.9562,2.4627,3,-15.5,2.1063,2.7225,5,50,
122 | 4.4663,4.2218,0,-15.5,4.5962,4.2967,1,50,
123 | 4.4938,4.1943,0,-15.5,4.5687,4.3242,1,50,
124 | 4.2714,4.1092,1,-15.5,4.7911,4.4093,3,50,
125 | 4.3812,3.9994,1,-15.5,4.6813,4.5191,3,50,
126 | 4.4013,4.1842,3,-15.5,4.6612,4.3342,5,50,
127 | 4.4562,4.1293,3,-15.5,4.6063,4.3891,5,50,
128 | 0.4038,1.444,0,-15.5,0.5337,1.519,1,50,
129 | 0.43125,1.4165,0,-15.5,0.50625,1.5465,1,50,
130 | 0.20894,1.3315,1,-15.5,0.72855,1.6315,3,50,
131 | 0.31875,1.2216,1,-15.5,0.61875,1.7413,3,50,
132 | 0.33884,1.4065,3,-15.5,0.59865,1.5565,5,50,
133 | 0.39375,1.3516,3,-15.5,0.54375,1.6114,5,50,
134 | 2.9038,3.1106,0,-15.5,3.0337,3.1856,1,50,
135 | 2.9313,3.0832,0,-15.5,3.0062,3.2131,1,50,
136 | 2.7089,2.9981,1,-15.5,3.2286,3.2982,3,50,
137 | 2.8187,2.8883,1,-15.5,3.1188,3.408,3,50,
138 | 2.8388,3.0732,3,-15.5,3.0987,3.2232,5,50,
139 | 2.8937,3.0183,3,-15.5,3.0438,3.2781,5,50,
140 | 1.6538,4.7773,0,-15.5,1.7837,4.8523,1,50,
141 | 1.6812,4.7499,0,-15.5,1.7563,4.8797,1,50,
142 | 1.4589,4.6648,1,-15.5,1.9786,4.9648,3,50,
143 | 1.5687,4.555,1,-15.5,1.8688,5.0745,3,50,
144 | 1.5888,4.7398,3,-15.5,1.8487,4.8898,5,50,
145 | 1.6437,4.6849,3,-15.5,1.7938,4.9447,5,50,
146 | 4.1863,0.042978,0,-15.5,4.2512,0.08048,3.75,50,
147 | 4.2,0.029253,0,-15.5,4.2375,0.094205,3.75,50,
148 | 3.894,-0.12577,2.5,-15.5,4.5435,0.24923,5,50,
149 | 4.0312,-0.26303,2.5,-15.5,4.4062,0.38649,5,50,
150 | 1.0288,1.6909,0,-15.5,1.1587,1.7659,1,50,
151 | 1.0562,1.6634,0,-15.5,1.1313,1.7933,1,50,
152 | 0.83395,1.5784,1,-15.5,1.3536,1.8784,3,50,
153 | 0.94375,1.4686,1,-15.5,1.2437,1.9882,3,50,
154 | 0.96385,1.6534,3,-15.5,1.2237,1.8034,5,50,
155 | 1.0187,1.5985,3,-15.5,1.1688,1.8583,5,50,
156 | 3.5613,3.3763,0,-15.5,3.6262,3.4138,3.75,50,
157 | 3.575,3.3626,0,-15.5,3.6125,3.4275,3.75,50,
158 | 3.269,3.2076,2.5,-15.5,3.9185,3.5825,5,50,
159 | 3.4062,3.0703,2.5,-15.5,3.7812,3.7198,5,50,
160 | 2.3112,0.59855,0,-15.5,2.3763,0.63605,3.75,50,
161 | 2.325,0.5848,0,-15.5,2.3625,0.64975,3.75,50,
162 | 2.019,0.42979,2.5,-15.5,2.6685,0.8048,5,50,
163 | 2.1562,0.29253,2.5,-15.5,2.5312,0.94205,5,50,
164 | 
165 | everything here will not be read in
166 | the format is:
167 | 	initial position
168 | 	goal position
169 | 	lower bound on state space
170 | 	upper bound on state space
171 | 	number of obstacles
172 | 	obstacles


--------------------------------------------------------------------------------
/gpu/dubinsAirplane.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Solves two point boundary value problem for the dubins airplane. 
  4 | */
  5 | 
  6 | #include "dubinsAirplane.cuh"
  7 | 
  8 | // (control means -1 = right, 0 = straight, 1 = left) 
  9 | // state is [x y z theta]
 10 | float dubinsAirplaneCost(float *x0, float *x1, int* control, float* controlD)
 11 | {
 12 | 	float v[2] = {x1[0] - x0[0], x1[1] - x0[1]};
 13 | 	float d = sqrt(pow(v[0],2) + pow(v[1],2));
 14 | 	float th = atan2(v[1], v[0]);
 15 | 	float a = mod2pi(x0[3] - th);
 16 | 	float b = mod2pi(x1[3] - th);
 17 | 	float cmin = FLT_MAX;
 18 | 	float ca = cos(a); float sa = sin(a); float cb = cos(b); float sb = sin(b);
 19 | 
 20 | 	cmin = dubinsLSLCost(d, a, ca, sa, b, cb, sb, cmin, control, controlD);
 21 | 	cmin = dubinsRSRCost(d, a, ca, sa, b, cb, sb, cmin, control, controlD);
 22 | 	cmin = dubinsRSLCost(d, a, ca, sa, b, cb, sb, cmin, control, controlD);
 23 | 	cmin = dubinsLSRCost(d, a, ca, sa, b, cb, sb, cmin, control, controlD);
 24 | 	cmin = dubinsRLRCost(d, a, ca, sa, b, cb, sb, cmin, control, controlD);
 25 | 	cmin = dubinsLRLCost(d, a, ca, sa, b, cb, sb, cmin, control, controlD);
 26 | 	cmin = sqrt(pow(cmin,2) + pow(x1[2]-x0[2],2));
 27 | 	return cmin;
 28 | }
 29 | 
 30 | float dubinsLSLCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD)
 31 | {
 32 | 	float tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sa - sb));
 33 | 	if (tmp < 0) { return c; }
 34 | 	float th = atan2(cb - ca, d + sa - sb);
 35 | 	float t = mod2pi(-a + th);
 36 | 	float p = sqrt(std::max(tmp, (float) 0));
 37 | 	float q = mod2pi(b - th);
 38 | 	float cnew = t + p + q;
 39 | 	// std::cout << "LSL: (" << t << ", " << p << ", " << q << "), c = " << cnew << std::endl;
 40 | 	if (cnew <= c) {
 41 | 		c = cnew; 
 42 | 		control[0] = 1; 	control[1] = 0; 	control[2] = 1;
 43 | 		controlD[0] = t; 	controlD[1] = p; 	controlD[2] = q;
 44 | 	}
 45 | 	return c;
 46 | }
 47 | 
 48 | float dubinsRSRCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD)
 49 | {
 50 | 	float tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sb - sa));
 51 | 	if (tmp < 0) { return c; }
 52 | 	float th = atan2(ca - cb, d - sa + sb);
 53 | 	float t = mod2pi(a - th);
 54 | 	float p = sqrt(std::max(tmp, (float) 0));
 55 | 	float q = mod2pi(-b + th);
 56 | 	float cnew = t + p + q;
 57 | 	// std::cout << "RSR: (" << t << ", " << p << ", " << q << "), c = " << cnew << std::endl;
 58 | 	if (cnew <= c) {
 59 | 		c = cnew; 
 60 | 		control[0] = -1; 	control[1] = 0; 	control[2] = -1;
 61 | 		controlD[0] = t; 	controlD[1] = p; 	controlD[2] = q;
 62 | 	}
 63 | 	return c;
 64 | }
 65 | 
 66 | float dubinsRSLCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD)
 67 | {
 68 | 	float tmp = d * d - 2 + 2 * (ca*cb + sa*sb - d * (sa + sb));
 69 | 	if (tmp < 0) { return c; }
 70 | 	float p = sqrt(std::max(tmp, (float) 0));
 71 | 	float th = atan2(ca + cb, d - sa - sb) - atan2(2, p);
 72 | 	float t = mod2pi(a - th);
 73 | 	float q = mod2pi(b - th);
 74 | 	float cnew = t + p + q;
 75 | 	// std::cout << "RSL: (" << t << ", " << p << ", " << q << "), c = " << cnew << std::endl;
 76 | 	if (cnew <= c) {
 77 | 		c = cnew; 
 78 | 		control[0] = -1; 	control[1] = 0; 	control[2] = 1;
 79 | 		controlD[0] = t; 	controlD[1] = p; 	controlD[2] = q;
 80 | 	}
 81 | 	return c;
 82 | }
 83 | 
 84 | float dubinsLSRCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD)
 85 | {
 86 | 	float tmp = -2 + d * d + 2 * (ca*cb + sa*sb + d * (sa + sb));
 87 | 	if (tmp < 0) { return c; }
 88 | 	float p = sqrt(std::max(tmp, (float) 0));
 89 | 	float th = atan2(-ca - cb, d + sa + sb) - atan2(-2, p);
 90 | 	float t = mod2pi(-a + th);
 91 | 	float q = mod2pi(-b + th);
 92 | 	float cnew = t + p + q;
 93 | 	// std::cout << "LSR: (" << t << ", " << p << ", " << q << "), c = " << cnew << std::endl;
 94 | 	if (cnew <= c) {
 95 | 		c = cnew; 
 96 | 		control[0] = 1; 	control[1] = 0; 	control[2] = -1;
 97 | 		controlD[0] = t; 	controlD[1] = p; 	controlD[2] = q;
 98 | 	}
 99 | 	return c;
100 | }
101 | 
102 | float dubinsRLRCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD)
103 | {
104 | 	float tmp = (6 - d * d  + 2 * (ca*cb + sa*sb + d * (sa - sb))) / 8;
105 | 	if (abs(tmp) >= 1) { return c; }
106 | 	float p = 2*M_PI - acos(tmp);
107 | 	float th = atan2(ca - cb, d - sa + sb);
108 | 	float t = mod2pi(a - th + p/2);
109 | 	float q = mod2pi(a - b - t + p);
110 | 	float cnew = t + p + q;
111 | 	// std::cout << "RLR: (" << t << ", " << p << ", " << q << "), c = " << cnew << std::endl;
112 | 	if (cnew <= c) {
113 | 		c = cnew; 
114 | 		control[0] = -1; 	control[1] = 1; 	control[2] = -1;
115 | 		controlD[0] = t; 	controlD[1] = p; 	controlD[2] = q;
116 | 	}
117 | 	return c;
118 | }
119 | 
120 | float dubinsLRLCost(float d, float a, float ca, float sa, float b, float cb, float sb, float c, int* control, float* controlD)
121 | {
122 | 	float tmp = (6 - d * d  + 2 * (ca*cb + sa*sb - d * (sa - sb))) / 8;
123 | 	if (abs(tmp) >= 1) { return c; }
124 | 	float p = 2*M_PI - acos(tmp);
125 | 	float th = atan2(-ca + cb, d + sa - sb);
126 | 	float t = mod2pi(-a + th + p/2);
127 | 	float q = mod2pi(b - a - t + p);
128 | 	float cnew = t + p + q;
129 | 	// std::cout << "LRL: (" << t << ", " << p << ", " << q << "), c = " << cnew << std::endl;
130 | 	if (cnew <= c) {
131 | 		c = cnew; 
132 | 		control[0] = 1; 	control[1] = -1; 	control[2] = 1;
133 | 		controlD[0] = t; 	controlD[1] = p; 	controlD[2] = q;
134 | 	}
135 | 	return c;
136 | }
137 | 
138 | float mod2pi(float ang)
139 | {
140 |     float twoPi = 2.0 * M_PI;
141 |     return ang - twoPi * floor( ang / twoPi );
142 | }
143 | 
144 | void dubinsAirplanePath(float *x0, float *x1, int* control, float* controlD, float* path, int 
145 | 	numDisc)
146 | {
147 | 	// load first control segment and transform
148 | 	carControl(control[0], controlD[0], &path[0], numDisc);
149 | 	transformPath(x0, &path[0], numDisc);
150 | 
151 | 	// load second control segment and transform
152 | 	carControl(control[1], controlD[1], &path[DIM*numDisc*1], numDisc);
153 | 	transformPath(&path[DIM*numDisc*1-DIM], &path[DIM*numDisc*1], numDisc);
154 | 
155 | 	// load third control segment and transform
156 | 	carControl(control[2], controlD[2], &path[DIM*numDisc*2], numDisc);
157 | 	transformPath(&path[DIM*numDisc*2-DIM], &path[DIM*numDisc*2], numDisc);
158 | 
159 | 	// add the z component
160 | 	float localCumSum[numDisc*3];
161 | 	float normLocalCumSum[numDisc*3];
162 | 	float cumSum = 0;
163 | 	float dz = x1[2] - x0[2];
164 | 
165 | 	localCumSum[0] = 0;
166 | 	for (int i = 1; i < numDisc*3; ++i) {
167 | 		float dx = path[i*DIM] - path[(i-1)*DIM];
168 | 		float dy = path[i*DIM+1] - path[(i-1)*DIM+1];
169 | 		cumSum += sqrt(pow(dx,2) + pow(dy,2));
170 | 		localCumSum[i] = cumSum;
171 | 	}
172 | 	for (int i = 1; i < numDisc*3; ++i) 
173 | 		normLocalCumSum[i] = localCumSum[i]/cumSum;
174 | 	path[2] = x0[2];
175 | 	for (int i = 1; i < numDisc*3; ++i) 
176 | 		path[i*DIM + 2] = x0[2] + normLocalCumSum[i]*dz;
177 | }
178 | 
179 | 
180 | void carControl(int control, float d, float* path, int numDisc)
181 | {
182 | 	float dt = d/((float) numDisc-1);
183 | 	for (int i = 0; i < numDisc; ++i) {
184 | 		if (control < 0) { // right turn
185 | 			path[DIM*i + 0] = sin(dt*i); // x component
186 | 			path[DIM*i + 1] = cos(dt*i)-1; // y component
187 | 			path[DIM*i + 3] = -dt*i; // theta component
188 | 		} else if (control == 0) { // straight line
189 | 			path[DIM*i + 0] = dt*i; // x component
190 | 			path[DIM*i + 1] = 0; // y component
191 | 			path[DIM*i + 3] = 0; // theta component
192 | 		} else { // left turn
193 | 			path[DIM*i + 0] = sin(dt*i); // x component
194 | 			path[DIM*i + 1] = 1-cos(dt*i); // y component
195 | 			path[DIM*i + 3] = dt*i; // theta component
196 | 		}
197 | 	}
198 | }
199 | 
200 | void transformPath(float *xinit, float* path, int numDisc)
201 | {
202 | 	float xi = xinit[0];
203 | 	float yi = xinit[1];
204 | 	float thi = xinit[3]; 
205 | 	for (int i = 0; i < numDisc; ++i) {
206 | 		float x = path[i*DIM + 0];
207 | 		float y = path[i*DIM + 1];
208 | 		path[i*DIM + 0] = cos(thi)*x - sin(thi)*y + xi;
209 | 		path[i*DIM + 1] = sin(thi)*x + cos(thi)*y + yi; 
210 | 		path[i*DIM + 3] += thi;
211 | 	}
212 | }
213 | 


--------------------------------------------------------------------------------
/gpu/collisionCheck.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Check collision between to states. Currently hardcoded for only 3 workspace dimensions.
  4 | */
  5 | 
  6 | #include "collisionCheck.cuh"
  7 | 
  8 | __global__ 
  9 | void freeEdges(float *obstacles, int obstaclesCount, float *samples, 
 10 | 	bool *isFreeSamples, int numDisc, float *discMotions, 
 11 | 	bool *isFreeEdges, int numEdges, float *debugOutput) {
 12 | 	
 13 | 	int tid = blockIdx.x * blockDim.x + threadIdx.x;
 14 | 	if (tid >= numEdges)
 15 | 		return;
 16 | 
 17 | 	float v[DIM], w[DIM];
 18 | 	float bbMin[DIM], bbMax[DIM];
 19 | 	bool motionValid = true;
 20 | 	for (int i = 0; i < numDisc; ++i) {
 21 | 		if (!motionValid)
 22 | 			break;
 23 | 		int baseIdx = tid*(numDisc+1)*DIM + i*DIM;
 24 | 		for (int d = 0; d < DIM; ++d) {
 25 | 			v[d] = discMotions[baseIdx + d];
 26 | 			w[d] = discMotions[baseIdx + d + DIM];
 27 | 
 28 | 			if (v[d] > w[d]) {
 29 | 				bbMin[d] = w[d];
 30 | 				bbMax[d] = v[d];
 31 | 			} else {
 32 | 				bbMin[d] = v[d];
 33 | 				bbMax[d] = w[d];
 34 | 			}
 35 | 		}
 36 | 		motionValid = motionValid && isMotionValid(v, w, bbMin, bbMax, obstaclesCount, obstacles, debugOutput);
 37 | 	}
 38 | 
 39 | 	isFreeEdges[tid] = motionValid;
 40 | }
 41 | 
 42 | __device__ 
 43 | void waypointCollisionCheck(int v_idx, int w_idx, int obstaclesCount, float* obstacles, 
 44 | 	int *nnIdxs, float *discMotions, int discIdx, int numDisc, bool *isCollision, int tid, float *debugOutput)
 45 | { 
 46 | 	// motion from w_idx to v_idx
 47 | 	int discMotionsIdx = nnIdxs[v_idx*NUM + w_idx];
 48 | 
 49 | 	// calculate bounds of the bounding box
 50 | 	float v[DIM], w[DIM]; // TODO: do v and w need ot be vectors?
 51 | 	float bbMin[DIM], bbMax[DIM];
 52 | 	for (int d = 0; d < DIM; ++d) {
 53 | 		v[d] = discMotions[discMotionsIdx*(numDisc+1)*DIM + discIdx*DIM + d];
 54 | 		w[d] = discMotions[discMotionsIdx*(numDisc+1)*DIM + (discIdx+1)*DIM + d];
 55 | 
 56 | 		if (v[d] > w[d]) {
 57 | 			bbMin[d] = w[d];
 58 | 			bbMax[d] = v[d];
 59 | 		} else {
 60 | 			bbMin[d] = v[d];
 61 | 			bbMax[d] = w[d];
 62 | 		}
 63 | 	}
 64 | 
 65 | 	isCollision[tid] = !isMotionValid(v, w, bbMin, bbMax, obstaclesCount, obstacles, debugOutput);
 66 | }
 67 | 
 68 | __device__
 69 | bool isMotionValid(float *v, float *w, float *bbMin, float *bbMax, int obstaclesCount, float* obstacles, float *debugOutput)
 70 | {
 71 | 	// TODO: eventually put each point (v, w) into shared memory
 72 | 	// TODO: read http://http.developer.nvidia.com/GPUGems3/gpugems3_ch32.html
 73 | 	// identify which obstacle this processor is checking against
 74 | 
 75 | 	// I don't think necessary, but routine to check if point is within an obstacle
 76 | 	// for (int obsIdx = 0; obsIdx < obstaclesCount; ++obsIdx) {
 77 | 	// 	bool notFree = true;
 78 | 	// 	for (int d = 0; d < 3; ++d) {
 79 | 	// 		notFree = notFree && 
 80 | 	// 		v[d] > obstacles[obsIdx*2*DIM + d] && 
 81 | 	// 		v[d] < obstacles[obsIdx*2*DIM + DIM + d];
 82 | 	// 		if (!notFree)
 83 | 	// 			break;
 84 | 	// 	}
 85 | 	// 	if (notFree) {
 86 | 	// 		return false;
 87 | 	// 	}
 88 | 	// }
 89 | 
 90 | 	// bool same = true;
 91 | 	// for (int d = 0; d < DIM/2; ++d)
 92 | 	// 	same = same && (v[d] == w[d]);
 93 | 	// if (same)
 94 | 	// 	return true;
 95 | 
 96 | 	// go through each obstacle and do broad then narrow phase collision checking
 97 | 	for (int obsIdx = 0; obsIdx < obstaclesCount; ++obsIdx) {
 98 | 		float obs[DIM*2];
 99 | 		for (int d = 0; d < DIM; ++d) {
100 | 			obs[d] = obstacles[obsIdx*2*DIM + d];
101 | 			obs[DIM+d] = obstacles[obsIdx*2*DIM + DIM + d];
102 | 		}
103 | 		if (!broadphaseValidQ(bbMin, bbMax, obs, debugOutput)) {
104 | 			bool motionValid = motionValidQ(v, w, obs, debugOutput);
105 | 			if (!motionValid) {
106 | 				return false;
107 | 			}
108 | 		}
109 | 	}
110 | 	return true;
111 | }
112 | 
113 | __device__
114 | bool broadphaseValidQ(float *bbMin, float *bbMax, float *obs, float *debugOutput) 
115 | {
116 | 	for (int d = 0; d < 3; ++d) {
117 | 		if (bbMax[d] <= obs[d] || obs[DIM+d] <= bbMin[d]) 
118 | 			return true;
119 | 	}
120 | 	return false;
121 | }
122 | 
123 | __device__
124 | bool motionValidQ(float *v, float *w, float *obs, float *debugOutput) 
125 | {
126 | 	float v_to_w[3];
127 | 
128 | 	for (int d = 0; d < 3; ++d) {
129 | 		float lambda;
130 | 		v_to_w[d] = w[d] - v[d];
131 | 		if (v[d] < obs[d]) {
132 | 			lambda = (obs[d] - v[d])/v_to_w[d];
133 | 		} else {
134 | 			lambda = (obs[DIM + d] - v[d])/v_to_w[d];
135 | 		}
136 | 		if (faceContainsProjection(v, w, lambda, d, obs, debugOutput))
137 | 			return false;
138 | 	}
139 | 	return true;
140 | }
141 | 
142 | __device__
143 | bool faceContainsProjection(float *v, float *w, float lambda, int j, float *obs, 
144 | 	float* debugOutput)
145 | {
146 | 	for (int d = 0; d < 3; ++d) {
147 | 		float projection = v[d] + (w[d] - v[d])*lambda;
148 | 		if (d != j && !(obs[d] <= projection && projection <= obs[DIM+d]))
149 | 			return false;
150 | 	}
151 | 	return true;
152 | }
153 | 
154 | // odd bug when called with v_to_w where the value is not passed correctly 
155 | // resulting in v_to_w[d] = -2e+30 (for example), and collisions being allowed through
156 | // this code is left here to remind me of the error/so I can figure it out later
157 | __device__
158 | bool faceContainsProjectionError(float *v, float *v_to_w, float lambda, int j, float *obs, 
159 | 	float* debugOutput)
160 | {
161 | 	for (int d = 0; d < 3; ++d) {
162 | 		float projection = v[d] + v_to_w[d]*lambda;
163 | 		if (d != j && !(obs[d] <= projection && projection <= obs[DIM+d]))
164 | 			return false;
165 | 	}
166 | 	return true;
167 | }
168 | 
169 | // CPU versions
170 | 
171 | bool isFreeEdge_h(int edgeIdx, float *obstacles, int obstaclesCount, 
172 | 	int numDisc, std::vector<float> discMotions, float *debugOutput) {
173 | 
174 | 	float v[DIM], w[DIM];
175 | 	float bbMin[DIM], bbMax[DIM];
176 | 	bool motionValid = true;
177 | 	for (int i = 0; i < numDisc; ++i) {
178 | 		if (!motionValid)
179 | 			break;
180 | 		int baseIdx = edgeIdx*(numDisc+1)*DIM + i*DIM;
181 | 		for (int d = 0; d < DIM; ++d) {
182 | 			v[d] = discMotions[baseIdx + d];
183 | 			w[d] = discMotions[baseIdx + d + DIM];
184 | 
185 | 			if (v[d] > w[d]) {
186 | 				bbMin[d] = w[d];
187 | 				bbMax[d] = v[d];
188 | 			} else {
189 | 				bbMin[d] = v[d];
190 | 				bbMax[d] = w[d];
191 | 			}
192 | 		}
193 | 		motionValid = motionValid && isMotionValid_h(v, w, bbMin, bbMax, obstaclesCount, obstacles, debugOutput);
194 | 	}
195 | 
196 | 	return motionValid;
197 | }
198 | 
199 |  
200 | void waypointCollisionCheck_h(int v_idx, int w_idx, int obstaclesCount, float* obstacles, 
201 | 	int *nnIdxs, float *discMotions, int discIdx, int numDisc, bool *isCollision, int tid, float *debugOutput)
202 | { 
203 | 	// motion from w_idx to v_idx
204 | 	int discMotionsIdx = nnIdxs[v_idx*NUM + w_idx];
205 | 
206 | 	// calculate bounds of the bounding box
207 | 	float v[DIM], w[DIM]; // TODO: do v and w need ot be vectors?
208 | 	float bbMin[DIM], bbMax[DIM];
209 | 	for (int d = 0; d < DIM; ++d) {
210 | 		v[d] = discMotions[discMotionsIdx*(numDisc+1)*DIM + discIdx*DIM + d];
211 | 		w[d] = discMotions[discMotionsIdx*(numDisc+1)*DIM + (discIdx+1)*DIM + d];
212 | 
213 | 		if (v[d] > w[d]) {
214 | 			bbMin[d] = w[d];
215 | 			bbMax[d] = v[d];
216 | 		} else {
217 | 			bbMin[d] = v[d];
218 | 			bbMax[d] = w[d];
219 | 		}
220 | 	}
221 | 
222 | 	isCollision[tid] = !isMotionValid_h(v, w, bbMin, bbMax, obstaclesCount, obstacles, debugOutput);
223 | }
224 | 
225 | bool isMotionValid_h(float *v, float *w, float *bbMin, float *bbMax, int obstaclesCount, float* obstacles, float *debugOutput)
226 | {
227 | 	// TODO: eventually put each point (v, w) into shared memory
228 | 	// TODO: read http://http.developer.nvidia.com/GPUGems3/gpugems3_ch32.html
229 | 	// identify which obstacle this processor is checking against
230 | 
231 | 	// I don't think necessary, but routine to check if point is within an obstacle
232 | 	// for (int obsIdx = 0; obsIdx < obstaclesCount; ++obsIdx) {
233 | 	// 	bool notFree = true;
234 | 	// 	for (int d = 0; d < 3; ++d) {
235 | 	// 		notFree = notFree && 
236 | 	// 		v[d] > obstacles[obsIdx*2*DIM + d] && 
237 | 	// 		v[d] < obstacles[obsIdx*2*DIM + DIM + d];
238 | 	// 		if (!notFree)
239 | 	// 			break;
240 | 	// 	}
241 | 	// 	if (notFree) {
242 | 	// 		return false;
243 | 	// 	}
244 | 	// }
245 | 	
246 | 	bool same = true;
247 | 	for (int d = 0; d < DIM/2; ++d)
248 | 		same = same && (v[d] == w[d]);
249 | 	if (same)
250 | 		return true;
251 | 
252 | 	// go through each obstacle and do broad then narrow phase collision checking
253 | 	for (int obsIdx = 0; obsIdx < obstaclesCount; ++obsIdx) {
254 | 		float obs[DIM*2];
255 | 		for (int d = 0; d < DIM; ++d) {
256 | 			obs[d] = obstacles[obsIdx*2*DIM + d];
257 | 			obs[DIM+d] = obstacles[obsIdx*2*DIM + DIM + d];
258 | 		}
259 | 		if (!broadphaseValidQ_h(bbMin, bbMax, obs, debugOutput)) {
260 | 			bool motionValid = motionValidQ_h(v, w, obs, debugOutput);
261 | 			if (!motionValid) {
262 | 				return false;
263 | 			}
264 | 		}
265 | 	}
266 | 	return true;
267 | }
268 | 
269 | bool broadphaseValidQ_h(float *bbMin, float *bbMax, float *obs, float *debugOutput) 
270 | {
271 | 	for (int d = 0; d < 3; ++d) {
272 | 		if (bbMax[d] <= obs[d] || obs[DIM+d] <= bbMin[d]) 
273 | 			return true;
274 | 	}
275 | 	return false;
276 | }
277 | 
278 | bool motionValidQ_h(float *v, float *w, float *obs, float *debugOutput) 
279 | {
280 | 	float v_to_w[3];
281 | 
282 | 	for (int d = 0; d < 3; ++d) {
283 | 		float lambda;
284 | 		v_to_w[d] = w[d] - v[d];
285 | 		if (v[d] < obs[d]) {
286 | 			lambda = (obs[d] - v[d])/v_to_w[d];
287 | 		} else {
288 | 			lambda = (obs[DIM + d] - v[d])/v_to_w[d];
289 | 		}
290 | 		if (faceContainsProjection_h(v, w, lambda, d, obs, debugOutput))
291 | 			return false;
292 | 	}
293 | 	return true;
294 | }
295 | 
296 | bool faceContainsProjection_h(float *v, float *w, float lambda, int j, float *obs, 
297 | 	float* debugOutput)
298 | {
299 | 	for (int d = 0; d < 3; ++d) {
300 | 		float projection = v[d] + (w[d] - v[d])*lambda;
301 | 		if (d != j && !(obs[d] <= projection && projection <= obs[DIM+d]))
302 | 			return false;
303 | 	}
304 | 	return true;
305 | }
306 | 
307 | // odd bug when called with v_to_w where the value is not passed correctly 
308 | // resulting in v_to_w[d] = -2e+30 (for example), and collisions being allowed through
309 | // this code is left here to remind me of the error/so I can figure it out later
310 | bool faceContainsProjectionError_h(float *v, float *v_to_w, float lambda, int j, float *obs, 
311 | 	float* debugOutput)
312 | {
313 | 	for (int d = 0; d < 3; ++d) {
314 | 		float projection = v[d] + v_to_w[d]*lambda;
315 | 		if (d != j && !(obs[d] <= projection && projection <= obs[DIM+d]))
316 | 			return false;
317 | 	}
318 | 	return true;
319 | }


--------------------------------------------------------------------------------
/gpu/FMT.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Runs the Fast marching tree algorithm.
  4 | */
  5 | 
  6 | #include "FMT.cuh"
  7 | 
  8 | void FMT(float *initial, float *goal, float *obstacles, int obstaclesCount,
  9 | 	std::vector<float> adjCosts, std::vector<int> nnGoEdges, std::vector<int> nnComeEdges, 
 10 | 	int nnSize, std::vector<float> discMotions, std::vector<int> nnIdxs,
 11 | 	int samplesCount, bool *d_isFreeSamples, float r, int numDisc, int numEdges,
 12 | 	std::vector<float> costs, int initial_idx, int goal_idx,
 13 | 	std::vector<float> samplesAll, std::vector<float> times)
 14 | {
 15 | 	// instantiate memory for online comp
 16 | 	double t_totalStart = std::clock();
 17 | 
 18 | 	bool isFreeSamples[NUM];
 19 | 	cudaMemcpy(isFreeSamples, d_isFreeSamples, sizeof(bool)*NUM, cudaMemcpyDeviceToHost);
 20 | 
 21 | 	// timing 	
 22 | 	double t_sel = 0;
 23 | 	double t_search = 0;
 24 | 	double t_conns = 0;
 25 | 	double t_cc = 0;
 26 | 	double t_oneStep = 0;
 27 | 	double t_searchStart = std::clock();
 28 | 
 29 | 	// initialCondition
 30 | 	std::vector<bool> closed(NUM,false); // tracks if a node has been visited
 31 | 	std::vector<bool> open(NUM,false); // tracks if a node is open
 32 | 	std::vector<bool> unvisited(NUM,true); // tracks if a node has been visited
 33 | 	std::set<std::pair<float, int> > openSet;
 34 | 	std::set<std::pair<float, int> >::iterator it;
 35 | 	std::vector<int> pathIdx(NUM,-1);
 36 | 
 37 | 	for (int i = 0; i < NUM; ++i) { // remove visited nodes
 38 | 		if(!isFreeSamples[i]) {
 39 | 			closed[i] = true;
 40 | 			unvisited[i] = false;
 41 | 		}
 42 | 	}
 43 | 	
 44 | 	costs[initial_idx] = 0;
 45 | 	open[initial_idx] = true;
 46 | 	unvisited[initial_idx] = false;
 47 | 	
 48 | 	std::pair<float, int> nodePair(0, initial_idx);
 49 | 	openSet.insert(nodePair);
 50 | 
 51 | 	for (int i = 0; i < NUM; ++i) {
 52 | 		// select node to closed
 53 | 		double t_selStart = std::clock();
 54 | 		if (openSet.empty()) break; // open set is empty, i.e. failure
 55 | 
 56 | 		// find min node to expand from
 57 | 		it = openSet.begin();
 58 | 		int cur = it->second;
 59 | 		float curCost = it->first;
 60 | 		openSet.erase(it);
 61 | 		t_sel += (std::clock() - t_selStart) / (double) CLOCKS_PER_SEC;
 62 | 
 63 | 		// std::cout << "selecting (" << cur << ", " << curCost << ")" << std::endl;
 64 | 		if (cur < 0) break; // none left to closed, i.e. failure
 65 | 		if (cur == goal_idx) break; // we have a solution
 66 | 
 67 | 		double t_connsStart = std::clock();
 68 | 
 69 | 		for (int j = 0; j < nnSize; ++j) { // select node to expand to
 70 | 			int nextIdx = nnGoEdges[cur*nnSize + j];
 71 | 
 72 | 			if (nextIdx < 0 || nextIdx > NUM) break; // out of neighbors or overreach bug
 73 | 			if (closed[nextIdx]) continue; // this node has been closed
 74 | 			if (!unvisited[nextIdx]) continue; // this node has been set
 75 | 			
 76 | 			double t_oneStepStart = std::clock();
 77 | 			// determine optimal one step connection
 78 | 			int oneStepIdx = -1;
 79 | 			int oneStepCost = 10000;
 80 | 			for (int k = 0; k < nnSize; ++k) { 
 81 | 				int prevIdx = nnComeEdges[nextIdx*nnSize + k];
 82 | 				if (prevIdx < 0 || prevIdx > NUM) break; // out of neighbors or overreach bug
 83 | 
 84 | 				if (!open[prevIdx]) continue; // node to connect to has been closed or unvisited
 85 | 
 86 | 				if (costs[prevIdx] + adjCosts[prevIdx*NUM + nextIdx] < oneStepCost) {
 87 | 					oneStepCost = costs[prevIdx] + adjCosts[prevIdx*NUM + nextIdx];
 88 | 					oneStepIdx = prevIdx;
 89 | 				}
 90 | 			}
 91 | 			t_oneStep += (std::clock() - t_oneStepStart) / (double) CLOCKS_PER_SEC;
 92 | 			// std::cout << "connecting (" << oneStepIdx << ", " << oneStepCost << ")" << std::endl;
 93 | 			if (oneStepIdx == -1) continue; // nothing to connect to
 94 | 
 95 | 			// creating the path online as the memory access was slower
 96 | 			std::vector<float> discMotion(DIM*(numDisc+1));
 97 | 			findDiscretizedPath(discMotion.data(), &(samplesAll[oneStepIdx*DIM]), &(samplesAll[nextIdx*DIM]), numDisc);
 98 | 
 99 | 			double t_ccStart = std::clock();
100 | 			int edgeIdx = nnIdxs[NUM*nextIdx + oneStepIdx];
101 | 			if (isFreeEdge_h(0, obstacles, obstaclesCount, numDisc, discMotion, NULL)) {
102 | 				float pathCost = costs[oneStepIdx] + adjCosts[oneStepIdx*NUM + nextIdx];
103 | 
104 | 				// place in open set
105 | 				open[nextIdx] = true; 
106 | 				unvisited[nextIdx] = false; // has been visited
107 | 				std::pair<float, int> nodePairNew(pathCost, nextIdx);
108 | 				openSet.insert(nodePairNew);
109 | 
110 | 				costs[nextIdx] = pathCost;
111 | 				pathIdx[nextIdx] = oneStepIdx;
112 | 			}
113 | 			t_cc += (std::clock() - t_ccStart) / (double) CLOCKS_PER_SEC;
114 | 		}
115 | 		closed[cur] = true; // place in closed set
116 | 		open[cur] = false; // remove from open set
117 | 
118 | 		t_conns += (std::clock() - t_connsStart) / (double) CLOCKS_PER_SEC;
119 | 		// in loop timing
120 | 		// std::cout << "FMT timing, selection = " << t_sel << ", and search = " << t_search << ", cc = " << t_cc << ", onestep = " << t_oneStep <<std::endl;
121 | 	}
122 | 	t_search += (std::clock() - t_searchStart) / (double) CLOCKS_PER_SEC;
123 | 	double t_total = (std::clock() - t_totalStart) / (double) CLOCKS_PER_SEC;
124 | 
125 | 	// std::cout << std::endl << "FMT Costs: " << std::endl;
126 | 	// for (std::vector<float>::const_iterator i = costs.begin(); i != costs.end(); ++i)
127 |  //       	std::cout << *i << ' ';
128 |  //       std::cout << std::endl;
129 | 
130 | 	// std::cout << std::endl << "FMT Tree: " << std::endl;
131 | 	// for (std::vector<int>::const_iterator i = pathIdx.begin(); i != pathIdx.end(); ++i)
132 | 	//    	std::cout << *i << ' ';
133 | 	// std::cout << std::endl;
134 | 
135 | 	std::cout << "FMT timing, selection = " << t_sel << ", connection = " << t_conns << ", and search = " << t_search << ", cc = " << t_cc << ", onestep = " << t_oneStep << ", total = " << t_total <<std::endl;
136 | 
137 | 	std::cout << "FMT final cost was " << costs[goal_idx] << std::endl;
138 | }
139 | 
140 | 
141 | 
142 | void FMTdub(float *initial, float *goal, float *obstacles, int obstaclesCount,
143 | 	std::vector<float> adjCosts, std::vector<int> nnGoEdges, std::vector<int> nnComeEdges, 
144 | 	int nnSize, std::vector<float> discMotions, std::vector<int> nnIdxs,
145 | 	int samplesCount, bool *d_isFreeSamples, float r, int numDisc, int numEdges,
146 | 	std::vector<float> costs, int initial_idx, int goal_idx,
147 | 	std::vector<float> samplesAll, std::vector<float> times,
148 | 	int numControls, std::vector<int> controlEdge, std::vector<float> controlDEdge)
149 | {
150 | 	// instantiate memory for online comp
151 | 	double t_totalStart = std::clock();
152 | 
153 | 	bool isFreeSamples[NUM];
154 | 	cudaMemcpy(isFreeSamples, d_isFreeSamples, sizeof(bool)*NUM, cudaMemcpyDeviceToHost);
155 | 
156 | 	// timing 	
157 | 	double t_sel = 0;
158 | 	double t_search = 0;
159 | 	double t_conns = 0;
160 | 	double t_cc = 0;
161 | 	double t_oneStep = 0;
162 | 	double t_searchStart = std::clock();
163 | 
164 | 	// initialCondition
165 | 	std::vector<bool> closed(NUM,false); // tracks if a node has been visited
166 | 	std::vector<bool> open(NUM,false); // tracks if a node is open
167 | 	std::vector<bool> unvisited(NUM,true); // tracks if a node has been visited
168 | 	std::set<std::pair<float, int> > openSet;
169 | 	std::set<std::pair<float, int> >::iterator it;
170 | 	std::vector<int> pathIdx(NUM,-1);
171 | 
172 | 	for (int i = 0; i < NUM; ++i) { // remove visited nodes
173 | 		if(!isFreeSamples[i]) {
174 | 			closed[i] = true;
175 | 			unvisited[i] = false;
176 | 		}
177 | 	}
178 | 	
179 | 	costs[initial_idx] = 0;
180 | 	open[initial_idx] = true;
181 | 	unvisited[initial_idx] = false;
182 | 	
183 | 	std::pair<float, int> nodePair(0, initial_idx);
184 | 	openSet.insert(nodePair);
185 | 
186 | 	for (int i = 0; i < NUM; ++i) {
187 | 		// select node to closed
188 | 		double t_selStart = std::clock();
189 | 		if (openSet.empty()) break; // open set is empty, i.e. failure
190 | 
191 | 		// find min node to expand from
192 | 		it = openSet.begin();
193 | 		int cur = it->second;
194 | 		float curCost = it->first;
195 | 		openSet.erase(it);
196 | 		t_sel += (std::clock() - t_selStart) / (double) CLOCKS_PER_SEC;
197 | 
198 | 		// std::cout << "selecting (" << cur << ", " << curCost << ")" << std::endl;
199 | 		if (cur < 0) break; // none left to closed, i.e. failure
200 | 		if (cur == goal_idx) break; // we have a solution
201 | 
202 | 		double t_connsStart = std::clock();
203 | 
204 | 		for (int j = 0; j < nnSize; ++j) { // select node to expand to
205 | 			int nextIdx = nnGoEdges[cur*nnSize + j];
206 | 
207 | 			if (nextIdx < 0 || nextIdx > NUM) break; // out of neighbors or overreach bug
208 | 			if (closed[nextIdx]) continue; // this node has been closed
209 | 			if (!unvisited[nextIdx]) continue; // this node has been set
210 | 			
211 | 			double t_oneStepStart = std::clock();
212 | 			// determine optimal one step connection
213 | 			int oneStepIdx = -1;
214 | 			int oneStepCost = 10000;
215 | 			for (int k = 0; k < nnSize; ++k) { 
216 | 				int prevIdx = nnComeEdges[nextIdx*nnSize + k];
217 | 				if (prevIdx < 0 || prevIdx > NUM) break; // out of neighbors or overreach bug
218 | 
219 | 				if (!open[prevIdx]) continue; // node to connect to has been closed or unvisited
220 | 
221 | 				if (costs[prevIdx] + adjCosts[prevIdx*NUM + nextIdx] < oneStepCost) {
222 | 					oneStepCost = costs[prevIdx] + adjCosts[prevIdx*NUM + nextIdx];
223 | 					oneStepIdx = prevIdx;
224 | 				}
225 | 			}
226 | 			t_oneStep += (std::clock() - t_oneStepStart) / (double) CLOCKS_PER_SEC;
227 | 			// std::cout << "connecting (" << oneStepIdx << ", " << oneStepCost << ")" << std::endl;
228 | 			if (oneStepIdx == -1) continue; // nothing to connect to
229 | 			int edgeIdx = nnIdxs[NUM*nextIdx + oneStepIdx];
230 | 
231 | 			std::vector<float> discMotion(DIM*(numDisc*numControls));
232 | 
233 | 			// copy in discretized motion			
234 | 			// for (int i = 0; i < DIM*numDisc*numControls; ++i)
235 | 				// discMotion[i] = discMotions[edgeIdx*numControls*numDisc*DIM + i];
236 | 
237 | 			// compute discretized motion from scratch
238 | 	 		// std::vector<int> control(numControls);
239 | 	 		// std::vector<float> controlD(numControls);
240 | 			// float cmin = dubinsAirplaneCost(&(samplesAll[oneStepIdx*DIM]), &(samplesAll[nextIdx*DIM]), control.data(), controlD.data());
241 | 			// dubinsAirplanePath(&(samplesAll[oneStepIdx*DIM]), &(samplesAll[nextIdx*DIM]), 
242 | 			// control.data(), controlD.data(), 
243 | 			// discMotion.data(), numDisc);
244 | 
245 |  			// compute discretized motion with controls set
246 |  			// computing online as the memory access was slower
247 |  			dubinsAirplanePath(&(samplesAll[oneStepIdx*DIM]), &(samplesAll[nextIdx*DIM]), 
248 | 				&(controlEdge[edgeIdx*numControls]), &(controlDEdge[edgeIdx*numControls]),
249 | 				discMotion.data(), numDisc);
250 | 
251 | 			double t_ccStart = std::clock();
252 | 			if (isFreeEdge_h(0, obstacles, obstaclesCount, numDisc*numControls-1, 
253 | 				discMotion, NULL)) {
254 | 				float pathCost = costs[oneStepIdx] + adjCosts[oneStepIdx*NUM + nextIdx];
255 | 
256 | 				// place in open set
257 | 				open[nextIdx] = true; 
258 | 				unvisited[nextIdx] = false; // has been visited
259 | 				std::pair<float, int> nodePairNew(pathCost, nextIdx);
260 | 				openSet.insert(nodePairNew);
261 | 
262 | 				costs[nextIdx] = pathCost;
263 | 				pathIdx[nextIdx] = oneStepIdx;
264 | 			}
265 | 			t_cc += (std::clock() - t_ccStart) / (double) CLOCKS_PER_SEC;
266 | 		}
267 | 		closed[cur] = true; // place in closed set
268 | 		open[cur] = false; // remove from open set
269 | 
270 | 		t_conns += (std::clock() - t_connsStart) / (double) CLOCKS_PER_SEC;
271 | 		// in loop timing
272 | 		// std::cout << "FMT timing, selection = " << t_sel << ", and search = " << t_search << ", cc = " << t_cc << ", onestep = " << t_oneStep <<std::endl;
273 | 	}
274 | 	t_search += (std::clock() - t_searchStart) / (double) CLOCKS_PER_SEC;
275 | 	double t_total = (std::clock() - t_totalStart) / (double) CLOCKS_PER_SEC;
276 | 
277 | 	// std::cout << std::endl << "FMT Costs: " << std::endl;
278 | 	// for (std::vector<float>::const_iterator i = costs.begin(); i != costs.end(); ++i)
279 |  //       	std::cout << *i << ' ';
280 |  //       std::cout << std::endl;
281 | 
282 | 	// std::cout << std::endl << "FMT Tree: " << std::endl;
283 | 	// for (std::vector<int>::const_iterator i = pathIdx.begin(); i != pathIdx.end(); ++i)
284 | 	//    	std::cout << *i << ' ';
285 | 	// std::cout << std::endl;
286 | 
287 | 	std::cout << "FMT timing, selection = " << t_sel << ", connection = " << t_conns << ", and search = " << t_search << ", cc = " << t_cc << ", onestep = " << t_oneStep << ", total = " << t_total <<std::endl;
288 | 
289 | 	std::cout << "FMT final cost was " << costs[goal_idx] << std::endl;
290 | }
291 | 
292 | 
293 | 
294 | 	


--------------------------------------------------------------------------------
/gpu/mainGround.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | mainGround.cu
  3 | Author: Brian Ichter
  4 | 
  5 | This runs the GMT* algorithm (and FMT, PRM) for a double integrator system (representing a quadrotor model). This main file 
  6 | is used primarily for timing results and evaluations of solution quality.
  7 | 
  8 | Run instructions:
  9 | 	TODO 
 10 | 
 11 | */
 12 | 
 13 | #include <iostream>
 14 | #include <fstream>
 15 | #include <ctime>
 16 | #include <cstdlib>
 17 | #include <algorithm> 
 18 | #include <vector>
 19 | #include <thrust/host_vector.h>
 20 | #include <thrust/device_vector.h>
 21 | #include <thrust/remove.h>
 22 | #include <thrust/sort.h>
 23 | #include <thrust/scan.h>
 24 | #include <thrust/extrema.h>
 25 | #include <thrust/reduce.h>
 26 | #include <limits>
 27 | #include <sstream>
 28 | 
 29 | #include "motionPlanningProblem.cuh"
 30 | #include "motionPlanningSolution.cuh"
 31 | #include "obstacles.cuh"
 32 | #include "helper.cuh"
 33 | #include "sampler.cuh"
 34 | #include "2pBVP.cuh"
 35 | #include "GMT.cuh"
 36 | #include "PRM.cuh"
 37 | #include "FMT.cuh"
 38 | 
 39 | // compiler inputs
 40 | #ifndef DIM
 41 | #error Please define DIM.
 42 | #endif
 43 | #ifndef NUM
 44 | #error Please define NUM.
 45 | #endif
 46 | 
 47 | // ***************** offline settings (paste setup here)
 48 | float lo[DIM] = {0, 0, 0, -1, -1, -1};
 49 | float hi[DIM] = {1, 1, 1, 1, 1, 1};
 50 | int edgeDiscNum = 12;
 51 | float dt = 0.05; // time step for dynamic propagation
 52 | int numDisc = 4; // number of discretizations of kinodynamic paths
 53 | float ms = 1000;
 54 | bool verbose = true;
 55 | 
 56 | int main(int argc, const char* argv[]) {
 57 | 	std::cout << "*********** Beginning Ground Run (DIM = " << DIM << ", NUM = " << NUM << ") **********" << std::endl;
 58 | 
 59 | 	// check setup is 3D DI
 60 | 	if (DIM != 6) {
 61 | 		std::cout << "DIM must be 6, currently only implemented for the 3D double integrator" << std::endl;
 62 | 		return -1;
 63 | 	}
 64 | 
 65 | 	// check a file has been specific
 66 | 	if (argc != 2) {
 67 | 		std::cout << "Must specify an problem setup filename, i.e. $ ./ground file.txt" << std::endl;
 68 | 		return -1;
 69 | 	}
 70 | 
 71 | 	// set cuda device
 72 | 	int count = 0;
 73 | 	cudaGetDeviceCount(&count);
 74 | 	cudaError_t code;
 75 | 	int deviceNum = count-1;
 76 | 	cudaSetDevice(deviceNum);
 77 | 	std::cout << "Number of CUDA devices = " << count << ", selecting " << deviceNum << std::endl;
 78 | 	code = cudaPeekAtLastError();
 79 | 	if (cudaSuccess != code) { std::cout << "ERROR on selecting device: " << cudaGetErrorString(code) << std::endl; }
 80 | 	
 81 | 	// create MotionPlanningProblem, which is currently not used except for print statements
 82 | 	MotionPlanningProblem mpp;
 83 | 	mpp.filename = argv[1];
 84 | 	mpp.dimC = DIM;
 85 | 	mpp.dimW = DIM/2;
 86 | 	mpp.numSamples = NUM;
 87 | 	mpp.edgeDiscNum = edgeDiscNum;
 88 | 	mpp.dt = dt;
 89 | 	mpp.hi.resize(mpp.dimC);
 90 | 	mpp.lo.resize(mpp.dimC);
 91 | 
 92 | 	for (int i = 0; i < DIM; ++i) {
 93 | 		mpp.hi[i] = hi[i];
 94 | 		mpp.lo[i] = lo[i];
 95 | 	}
 96 | 
 97 | 	// init and goal states (must then connect to the final tree)
 98 | 	std::vector<float> init(DIM, 0.1);
 99 | 	std::vector<float> goal(DIM, 0.9);
100 | 	// goal[0] = 0.8; goal[1] = 0.8214;
101 | 	// init[1] = 0.05; 
102 | 	for (int d = DIM/2; d < DIM; ++d) {
103 | 	 	init[d] = 0;
104 | 	 	goal[d] = 0;
105 | 	}
106 | 	int goalIdx = NUM-1;
107 | 	int initIdx = 0;
108 | 
109 | 	// obstacles
110 | 	int numObstacles = getObstaclesCount();
111 | 	std::vector<float> obstacles(numObstacles*2*DIM);
112 | 	generateObstacles(obstacles.data(), numObstacles*2*DIM);
113 | 
114 | 	std::ifstream file (mpp.filename);
115 | 	std::string readValue;
116 | 	if (file.good()) {
117 | 		// read in init
118 | 		for (int d = 0; d < DIM; ++d) {
119 | 			getline(file, readValue, ',');	
120 | 	        std::stringstream convertorInit(readValue);
121 | 	        convertorInit >> init[d];
122 | 		}
123 | 
124 | 		for (int d = 0; d < DIM; ++d) {
125 | 			getline(file, readValue, ',');	
126 | 	        std::stringstream convertorGoal(readValue);
127 | 	        convertorGoal >> goal[d];
128 | 		}
129 | 
130 | 		for (int d = 0; d < DIM; ++d) {
131 | 			getline(file, readValue, ',');	
132 | 	        std::stringstream convertorLo(readValue);
133 | 	        convertorLo >> lo[d];
134 | 		}
135 | 
136 | 		for (int d = 0; d < DIM; ++d) {
137 | 			getline(file, readValue, ',');	
138 | 	        std::stringstream convertorHi(readValue);
139 | 	        convertorHi >> hi[d];
140 | 		}
141 | 
142 | 		getline(file, readValue, ',');
143 | 		std::stringstream convertorNumObs(readValue);	
144 | 		convertorNumObs >> numObstacles;
145 | 
146 | 		obstacles.resize(numObstacles*DIM*2);
147 | 		for (int obs = 0; obs < numObstacles; ++obs) {
148 | 			for (int d = 0; d < DIM*2; ++d) {
149 | 				getline(file, readValue, ',');	
150 | 				std::stringstream convertorObs(readValue);
151 | 				convertorObs >> obstacles[obs*DIM*2 + d];
152 | 			}
153 | 		}
154 | 
155 | 		if (verbose) {
156 | 			std::cout << "***** Inputs from " << mpp.filename << " are: *****" << std::endl;
157 | 		}
158 | 		if (verbose) {
159 | 			std::cout << "init is: "; printArray(&init[0],1,DIM,std::cout);
160 | 		}
161 | 		if (verbose) {
162 | 			std::cout << "goal is: "; printArray(&goal[0],1,DIM,std::cout);
163 | 		}
164 | 		if (verbose) {
165 | 			std::cout << "lo is: "; printArray(&lo[0],1,DIM,std::cout);
166 | 		}
167 | 		if (verbose) {
168 | 			std::cout << "hi is: "; printArray(&hi[0],1,DIM,std::cout);
169 | 		}
170 | 		if (verbose) {
171 | 			std::cout << "obstacle count = " << numObstacles << std::endl;
172 | 		}
173 | 		if(verbose) {
174 | 			printArray(&obstacles[0],numObstacles,2*DIM,std::cout);
175 | 		}
176 | 
177 | 	}  else {
178 | 		std::cout << "didn't read in file, bad file!" << std::endl;
179 | 	}
180 | 
181 | 	std::cout << "--- Motion planning problem, " << mpp.filename << " ---" << std::endl;
182 | 	std::cout << "Sample count = " << mpp.numSamples << ", C-space dim = " << mpp.dimC << ", Workspace dim = " << mpp.dimW << std::endl;
183 | 	std::cout << "hi = ["; for (int i = 0; i < mpp.dimC; ++i) { std::cout << hi[i] << " "; } std::cout << "], ";
184 | 	std::cout << "lo = ["; for (int i = 0; i < mpp.dimC; ++i) { std::cout << lo[i] << " "; } std::cout << "]" << std::endl;
185 | 	std::cout << "edge discretizations " << mpp.edgeDiscNum << ", dt = " << mpp.dt << std::endl;
186 | 	std::cout << "Obstacle set, count = " << numObstacles << ":" << std::endl;
187 | 	printArray(obstacles.data(), numObstacles, 2*DIM, std::cout);
188 | 
189 | 	// read in 
190 | 	// hi, lo
191 | 	// goal, init
192 | 	// obstacles, obstacles count
193 | 
194 | 	/*********************** create array to return debugging information ***********************/
195 | 	float *d_debugOutput;
196 | 	cudaMalloc(&d_debugOutput, sizeof(float)*NUM);
197 | 
198 | 	// ***************** precomputation
199 | 
200 | 	// Sampling- to allow more parallelism we sample NUM points on X here (rather than Xfree)
201 | 	// and perform sample free online once obstacles are known.
202 | 	std::vector<float> samplesAll (DIM*NUM);
203 | 	createSamplesHalton(0, samplesAll.data(), &(init[0]), &(goal[0]), lo, hi);
204 | 	thrust::device_vector<float> d_samples_thrust(DIM*NUM);
205 | 	float *d_samples = thrust::raw_pointer_cast(d_samples_thrust.data());
206 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_samples, samplesAll.data(), sizeof(float)*DIM*NUM, cudaMemcpyHostToDevice));
207 | 
208 | 	// calculate nn
209 | 	double t_calc10thStart = std::clock();
210 | 	std::vector<float> topts ((NUM-1)*(NUM));
211 | 	std::vector<float> copts ((NUM-1)*(NUM));
212 | 	float rnPerc = 0.05;
213 | 	int rnIdx = (int) ((NUM-1)*(NUM)*rnPerc); // some percentile of the number of edges
214 | 	int numEdges = rnIdx;
215 | 	std::cout << "rnIdx is " << rnIdx << std::endl;
216 | 	int idx;
217 | 	float tmax = 5;
218 | 
219 | 	// GPU version of finding rn and costs/times
220 | 	thrust::device_vector<float> d_copts_thrust((NUM-1)*(NUM));
221 | 	float* d_copts = thrust::raw_pointer_cast(d_copts_thrust.data());
222 | 	thrust::device_vector<float> d_topts_thrust((NUM-1)*(NUM));
223 | 	float* d_topts = thrust::raw_pointer_cast(d_topts_thrust.data());
224 | 	const int blockSizeFillCoptsTopts = 192;
225 | 	const int gridSizeFillCoptsTopts = std::min((NUM*NUM + blockSizeFillCoptsTopts - 1) / blockSizeFillCoptsTopts, 2147483647);
226 | 	if (gridSizeFillCoptsTopts == 2147483647)
227 | 		std::cout << "...... ERROR: increase grid size for fillCoptsTopts" << std::endl;
228 | 	fillCoptsTopts<<<gridSizeFillCoptsTopts, blockSizeFillCoptsTopts>>>(
229 | 		d_samples, d_copts, d_topts, tmax);
230 | 	cudaDeviceSynchronize();
231 | 
232 | 	CUDA_ERROR_CHECK(cudaMemcpy(topts.data(), d_topts, sizeof(float)*((NUM-1)*(NUM)), cudaMemcpyDeviceToHost));
233 | 	CUDA_ERROR_CHECK(cudaMemcpy(copts.data(), d_copts, sizeof(float)*((NUM-1)*(NUM)), cudaMemcpyDeviceToHost));
234 | 	thrust::sort(d_copts_thrust.begin(), d_copts_thrust.end());
235 | 	float rn = d_copts_thrust[rnIdx];
236 | 
237 | 	double t_2pbvpTestStart = std::clock();
238 | 	float x0[DIM], x1[DIM];
239 | 
240 | 	double t_discMotionsStart = std::clock();
241 | 	std::vector<int> nnIdxs(NUM*NUM,-3);
242 | 	int nnGoSizes[NUM];
243 | 	int nnComeSizes[NUM];
244 | 	for (int i = 0; i < NUM; ++i) {
245 | 		nnGoSizes[i] = 0;
246 | 		nnComeSizes[i] = 0;
247 | 	}
248 | 	std::vector<float> discMotions (numEdges*(numDisc+1)*DIM,0); // array of motions, but its a vector who idk for some reason it won't work as an array
249 | 	int nnIdx = 0; // index position in NN discretization array
250 | 	idx = 0; // index position in copts vector above
251 | 
252 | 	std::vector<float> coptsEdge (numEdges); // edge index accessed copts
253 | 	std::vector<float> toptsEdge (numEdges); // edge index accessed topts
254 | 	std::vector<float> c2g (NUM);
255 | 	for (int i = 0; i < NUM; ++i) {
256 | 		for (int d = 0; d < DIM; ++d)
257 | 			x0[d] = samplesAll[d + DIM*i];
258 | 		for (int j = 0; j < NUM; ++j) {
259 | 			if (j == i)
260 | 				continue;
261 | 			if (copts[idx] < rn) {
262 | 				coptsEdge[nnIdx] = copts[idx];
263 | 				toptsEdge[nnIdx] = topts[idx];
264 | 				for (int d = 0; d < DIM; ++d)
265 | 					x1[d] = samplesAll[d + DIM*j];
266 | 				nnIdxs[j*NUM+i] = nnIdx; // look up for discrete motions from i -> j
267 | 				findDiscretizedPath(&(discMotions[nnIdx*DIM*(numDisc+1)]), x0, x1, numDisc); // TODO: give topt
268 | 				nnGoSizes[i]++;
269 | 				nnComeSizes[j]++;
270 | 				nnIdx++;
271 | 			}
272 | 			idx++;
273 | 		}
274 | 		float topt_c2g = toptBisection(&(samplesAll[i*DIM]), &(samplesAll[goalIdx*DIM]), tmax);
275 | 		c2g[i] = cost(topt_c2g, &(samplesAll[i*DIM]), &(samplesAll[goalIdx*DIM]));
276 | 	}
277 | 	double t_discMotions = (std::clock() - t_discMotionsStart) / (double) CLOCKS_PER_SEC;
278 | 	std::cout << "Discretizing motions took: " << t_discMotions*ms << " ms for " << nnIdx << " solves" << std::endl;
279 | 
280 | 	float *d_toptsEdge;
281 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_toptsEdge, sizeof(float)*numEdges));
282 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_toptsEdge, toptsEdge.data(), sizeof(float)*numEdges, cudaMemcpyHostToDevice));
283 | 	float *d_coptsEdge;
284 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_coptsEdge, sizeof(float)*numEdges));
285 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_coptsEdge, coptsEdge.data(), sizeof(float)*numEdges, cudaMemcpyHostToDevice));
286 | 
287 | 	int maxNNSize = 0;
288 | 	for (int i = 0; i < NUM; ++i) {
289 | 		if (maxNNSize < nnGoSizes[i])
290 | 			maxNNSize = nnGoSizes[i];
291 | 		if (maxNNSize < nnComeSizes[i])
292 | 			maxNNSize = nnComeSizes[i];
293 | 	}
294 | 	std::cout << "max number of nn is " << maxNNSize << std::endl;
295 | 
296 | 	std::vector<float> distancesCome (NUM*maxNNSize, 0);
297 | 	std::vector<int> nnGoEdges (NUM*maxNNSize, -1); // edge gives indices (i,j) to check nnIdx to then find the discretized path
298 | 	std::vector<int> nnComeEdges (NUM*maxNNSize, -1); // edge gives indices (j,i) to check nnIdx to then find the discretized path
299 | 	std::vector<float> adjCosts (NUM*NUM,10000);
300 | 	std::vector<float> adjTimes (NUM*NUM,10000);
301 | 	idx = 0;
302 | 	for (int i = 0; i < NUM; ++i) {
303 | 		nnGoSizes[i] = 0; // clear nnSizes again
304 | 		nnComeSizes[i] = 0; // clear nnSizes again
305 | 	}
306 | 	for (int i = 0; i < NUM; ++i) {
307 | 		for (int j = 0; j < NUM; ++j) {
308 | 			if (j == i)
309 | 				continue;
310 | 			if (copts[idx] < rn) {
311 | 				nnGoEdges[i*maxNNSize + nnGoSizes[i]] = j; // edge from i to j (i -> j)
312 | 				nnComeEdges[j*maxNNSize + nnComeSizes[j]] = i;
313 | 				distancesCome[j*maxNNSize + nnComeSizes[j]] = copts[idx];
314 | 				nnGoSizes[i]++;
315 | 				nnComeSizes[j]++;
316 | 				adjCosts[i*NUM + j] = copts[idx]; // cost to go from i to j
317 | 				adjTimes[i*NUM + j] = topts[idx]; // time to go from i to j
318 | 			}
319 | 			idx++;
320 | 		}
321 | 	}	
322 | 
323 | 	// put NN onto device
324 | 	float *d_discMotions;
325 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_discMotions, sizeof(float)*numEdges*(numDisc+1)*DIM));
326 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_discMotions, &discMotions[0], sizeof(float)*numEdges*(numDisc+1)*DIM, cudaMemcpyHostToDevice));
327 | 	// std::cout << "**** disc motions = " << std::endl;
328 | 	// printArray(&discMotions[0], 30, DIM, std::cout);
329 | 
330 | 	int *d_nnIdxs;
331 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_nnIdxs, sizeof(int)*NUM*NUM));
332 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_nnIdxs, &(nnIdxs[0]), sizeof(int)*NUM*NUM, cudaMemcpyHostToDevice));
333 | 
334 | 	float *d_distancesCome;
335 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_distancesCome, sizeof(float)*NUM*maxNNSize));
336 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_distancesCome, &(distancesCome[0]), sizeof(float)*NUM*maxNNSize, cudaMemcpyHostToDevice));
337 | 
338 | 	int *d_nnGoEdges;
339 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_nnGoEdges, sizeof(int)*NUM*maxNNSize));
340 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_nnGoEdges, &(nnGoEdges[0]), sizeof(int)*NUM*maxNNSize, cudaMemcpyHostToDevice));
341 | 
342 | 	int *d_nnComeEdges;
343 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_nnComeEdges, sizeof(int)*NUM*maxNNSize));
344 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_nnComeEdges, &(nnComeEdges[0]), sizeof(int)*NUM*maxNNSize, cudaMemcpyHostToDevice));
345 | 
346 | 	float *d_costs;
347 | 	cudaMalloc(&d_costs, sizeof(float)*NUM);
348 | 	thrust::device_vector<int> d_edges(NUM);
349 | 	int* d_edges_ptr = thrust::raw_pointer_cast(d_edges.data());
350 | 
351 | 
352 | 	// ******** online computation
353 | 
354 | 	// load obstacles on device
355 | 	float *d_obstacles;
356 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_obstacles, sizeof(float)*2*numObstacles*DIM));
357 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_obstacles, obstacles.data(), sizeof(float)*2*numObstacles*DIM, cudaMemcpyHostToDevice));
358 | 
359 | 	// sample free	
360 | 	bool isFreeSamples[NUM];
361 | 	thrust::device_vector<bool> d_isFreeSamples_thrust(NUM);
362 | 	bool* d_isFreeSamples = thrust::raw_pointer_cast(d_isFreeSamples_thrust.data());
363 | 
364 | 	double t_sampleFreeStart = std::clock();
365 | 	const int blockSizeSF = 192;
366 | 	const int gridSizeSF = std::min((NUM + blockSizeSF - 1) / blockSizeSF, 2147483647);
367 | 	if (gridSizeSF == 2147483647)
368 | 		std::cout << "...... ERROR: increase grid size for sampleFree" << std::endl;
369 | 	sampleFree<<<gridSizeSF, blockSizeSF>>>(
370 | 		d_obstacles, numObstacles, d_samples, d_isFreeSamples, d_debugOutput);
371 | 	cudaDeviceSynchronize();
372 | 	code = cudaPeekAtLastError();
373 | 	if (cudaSuccess != code) { std::cout << "ERROR on freeEdges: " << cudaGetErrorString(code) << std::endl; }
374 | 
375 | 	double t_sampleFree = (std::clock() - t_sampleFreeStart) / (double) CLOCKS_PER_SEC;
376 | 	std::cout << "Sample free took: " << t_sampleFree << " s" << std::endl;
377 | 	CUDA_ERROR_CHECK(cudaMemcpy(isFreeSamples, d_isFreeSamples, sizeof(bool)*NUM, cudaMemcpyDeviceToHost));
378 | 
379 | 	// run GMT
380 | 	double t_gmtStart = std::clock();
381 | 	std::cout << "Running wavefront expansion GMT" << std::endl;
382 | 	GMTwavefront(&(init[0]), &(goal[0]), d_obstacles, numObstacles,
383 | 		d_distancesCome, d_nnGoEdges, d_nnComeEdges, maxNNSize, d_discMotions, d_nnIdxs,
384 | 		d_samples, NUM, d_isFreeSamples, rn, numDisc,
385 | 		d_costs, d_edges_ptr, initIdx, goalIdx) ;
386 | 
387 | 	double t_gmt = (std::clock() - t_gmtStart) / (double) CLOCKS_PER_SEC;
388 | 	std::cout << "******** GMT took: " << t_gmt << " s" << std::endl;
389 | 	float costGoal = 0;
390 | 	cudaMemcpy(&costGoal, d_costs+goalIdx, sizeof(float), cudaMemcpyDeviceToHost);
391 | 	std::cout << "Solution cost: " << costGoal << std::endl;
392 | 	
393 | 	// ***************** output results
394 | 	std::ofstream matlabData;
395 | 	matlabData.open ("matlabInflationData.txt");
396 | 	matlabData << "obstacles.data() = ["; 
397 | 	printArray(obstacles.data(), 2*numObstacles, DIM, matlabData); 
398 | 	matlabData << "];" << std::endl;
399 | 
400 | 	true && printSolution(NUM, d_samples, d_edges_ptr, d_costs);
401 | 	matlabData.close();
402 | 
403 | 	// ***************** PRM
404 | 	std::vector<float> costs(NUM,10000);
405 | 	
406 | 	double t_PRMStart = std::clock();
407 | 	std::cout << "Running PRM" << std::endl;
408 | 	PRM(&(init[0]), &(goal[0]), d_obstacles, numObstacles,
409 | 		adjCosts, nnGoEdges, nnComeEdges, maxNNSize, d_discMotions, nnIdxs,
410 | 		d_samples, NUM, d_isFreeSamples, rn, numDisc, numEdges,
411 | 		costs, d_edges_ptr, initIdx, goalIdx,
412 | 		c2g);
413 | 	double t_PRM = (std::clock() - t_PRMStart) / (double) CLOCKS_PER_SEC;
414 | 	std::cout << "******** PRM took: " << t_PRM << " s" << std::endl;
415 | 
416 | 	// ***************** FMT
417 | 	double t_FMTStart = std::clock();
418 | 	std::cout << "Running FMT" << std::endl;
419 | 	// call FMT
420 | 	FMT(&(init[0]), &(goal[0]), obstacles.data(), numObstacles,
421 | 		adjCosts, nnGoEdges, nnComeEdges, maxNNSize, discMotions, nnIdxs,
422 | 		NUM, d_isFreeSamples, rn, numDisc, numEdges,
423 | 		costs, initIdx, goalIdx,
424 | 		samplesAll, adjTimes);
425 | 	
426 | 	double t_FMT = (std::clock() - t_FMTStart) / (double) CLOCKS_PER_SEC;
427 | 	std::cout << "******** FMT took: " << t_FMT << " s" << std::endl;
428 | 
429 | 	// ***************** free memory
430 | 	cudaFree(d_obstacles);
431 | 	cudaFree(d_toptsEdge);
432 | 	cudaFree(d_coptsEdge);
433 | 	cudaFree(d_discMotions);
434 | 	cudaFree(d_nnIdxs);
435 | 	cudaFree(d_distancesCome);
436 | 	cudaFree(d_nnGoEdges);
437 | 	cudaFree(d_nnComeEdges);
438 | 	cudaFree(d_costs);
439 | }
440 | 


--------------------------------------------------------------------------------
/gpu/mainDubins.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | mainGround.cu
  3 | Author: Brian Ichter
  4 | 
  5 | This runs the GMT* and MCMP algorithms for a double integrator system (representing a quadrotor model). This main file 
  6 | is used primarily for timing results and evaluations of solution quality, rather than to be run directly with the quad
  7 | (we use mainQuad.cu for this).
  8 | 
  9 | Run instructions:
 10 | 	TODO 
 11 | 
 12 | */
 13 | 
 14 | #include <iostream>
 15 | #include <fstream>
 16 | #include <ctime>
 17 | #include <cstdlib>
 18 | #include <algorithm> 
 19 | #include <vector>
 20 | #include <thrust/host_vector.h>
 21 | #include <thrust/device_vector.h>
 22 | #include <thrust/remove.h>
 23 | #include <thrust/sort.h>
 24 | #include <thrust/scan.h>
 25 | #include <thrust/extrema.h>
 26 | #include <thrust/reduce.h>
 27 | #include <limits>
 28 | #include <sstream>
 29 | 
 30 | #include "motionPlanningProblem.cuh"
 31 | #include "motionPlanningSolution.cuh"
 32 | #include "obstacles.cuh"
 33 | #include "helper.cuh"
 34 | #include "sampler.cuh"
 35 | #include "2pBVP.cuh"
 36 | #include "GMT.cuh"
 37 | #include "PRM.cuh"
 38 | #include "FMT.cuh"
 39 | #include "dubinsAirplane.cuh"
 40 | 
 41 | // compiler inputs
 42 | #ifndef DIM
 43 | #error Please define DIM.
 44 | #endif
 45 | #ifndef NUM
 46 | #error Please define NUM.
 47 | #endif
 48 | 
 49 | // horrible coding, but will get the job done for now
 50 | int DIMdubMain = 4;
 51 | 
 52 | // ***************** offline settings (paste setup here)
 53 | float lo[DIM] = {0, 0, 0, 0};
 54 | float hi[DIM] = {5, 5, 5, 2*M_PI};
 55 | int edgeDiscNum = 8;
 56 | float dt = 0.05; // time step for dynamic propagation
 57 | int numDisc = 4; // number of discretizations of kinodynamic paths
 58 | int numControls = 3; // number of total controls (i.e., Dubins word length)
 59 | float ms = 1000;
 60 | bool verbose = true;
 61 | 
 62 | int main(int argc, const char* argv[]) {
 63 | 	float x0dub[4] = {0, 0, 0, M_PI/2};
 64 | 	float x1dub[4] = {1, -3, 1, 0};
 65 | 	std::vector<int> control(numControls);
 66 | 	std::vector<float> controlD(numControls);
 67 | 	std::vector<float> path(DIMdubMain*numDisc*numControls);
 68 | 
 69 | 	float cmin = dubinsAirplaneCost(x0dub, x1dub, control.data(), controlD.data());
 70 | 	std::cout << "cost = " << cmin << ", word is " << control[0] << ", " << control[1] << ", " << control[2] << std::endl;
 71 | 	std::cout << "durations (" << controlD[0] << ", " << controlD[1] << ", " << controlD[2] << ")" << std::endl;
 72 | 
 73 | 	dubinsAirplanePath(x0dub, x1dub, control.data(), controlD.data(), path.data(), numDisc);
 74 | 	printArray(path.data(), numControls*numDisc, DIMdubMain, std::cout);
 75 | 
 76 | 
 77 | 
 78 | 
 79 | 	std::cout << "*********** Beginning Dubins Aircraft Run (DIM = " << DIM << ", NUM = " << NUM << ") **********" << std::endl;
 80 | 
 81 | 	// check setup is 3D DI
 82 | 	if (DIM != 4) {
 83 | 		std::cout << "DIM must be 4 for Dubins airplane (x y z theta)" << std::endl;
 84 | 		return -1;
 85 | 	}
 86 | 
 87 | 	// check a file has been specific
 88 | 	if (argc != 2) {
 89 | 		std::cout << "Must specify an problem setup filename, i.e. $ ./dubins file.txt" << std::endl;
 90 | 		return -1;
 91 | 	}
 92 | 
 93 | 	int count = 0;
 94 | 	cudaGetDeviceCount(&count);
 95 | 	cudaError_t code;
 96 | 	int deviceNum = 1;
 97 | 	cudaSetDevice(deviceNum);
 98 | 	std::cout << "Number of CUDA devices = " << count << ", selecting " << deviceNum << std::endl;
 99 | 	code = cudaPeekAtLastError();
100 | 	if (cudaSuccess != code) { std::cout << "ERROR on selecting device: " << cudaGetErrorString(code) << std::endl; }
101 | 	
102 | 
103 | 	MotionPlanningProblem mpp;
104 | 	mpp.filename = argv[1];
105 | 	mpp.dimC = DIM;
106 | 	mpp.dimW = 3;
107 | 	mpp.numSamples = NUM;
108 | 	mpp.edgeDiscNum = edgeDiscNum;
109 | 	mpp.dt = dt;
110 | 	mpp.hi.resize(mpp.dimC);
111 | 	mpp.lo.resize(mpp.dimC);
112 | 
113 | 	for (int i = 0; i < DIM; ++i) {
114 | 		mpp.hi[i] = hi[i];
115 | 		mpp.lo[i] = lo[i];
116 | 	}
117 | 
118 | 	// init and goal states (must then connect to the final tree)
119 | 	std::vector<float> init(DIM, 0.1);
120 | 	std::vector<float> goal(DIM, 4.9);
121 | 	init[3] = M_PI/2;
122 | 	goal[3] = M_PI/2;
123 | 
124 | 	int goalIdx = NUM-1;
125 | 	int initIdx = 0;
126 | 
127 | 	int numObstacles = getObstaclesCount();
128 | 	std::vector<float> obstacles(numObstacles*2*DIM);
129 | 	generateObstacles(obstacles.data(), numObstacles*2*DIM);
130 | 
131 | 	std::ifstream file (mpp.filename);
132 | 	std::string readValue;
133 | 	if (file.good()) {
134 | 		// read in init
135 | 		for (int d = 0; d < DIM; ++d) {
136 | 			getline(file, readValue, ',');	
137 | 	        std::stringstream convertorInit(readValue);
138 | 	        convertorInit >> init[d];
139 | 		}
140 | 
141 | 		for (int d = 0; d < DIM; ++d) {
142 | 			getline(file, readValue, ',');	
143 | 	        std::stringstream convertorGoal(readValue);
144 | 	        convertorGoal >> goal[d];
145 | 		}
146 | 
147 | 		for (int d = 0; d < DIM; ++d) {
148 | 			getline(file, readValue, ',');	
149 | 	        std::stringstream convertorLo(readValue);
150 | 	        convertorLo >> lo[d];
151 | 		}
152 | 
153 | 		for (int d = 0; d < DIM; ++d) {
154 | 			getline(file, readValue, ',');	
155 | 	        std::stringstream convertorHi(readValue);
156 | 	        convertorHi >> hi[d];
157 | 		}
158 | 
159 | 		getline(file, readValue, ',');
160 | 		std::stringstream convertorNumObs(readValue);	
161 | 		convertorNumObs >> numObstacles;
162 | 
163 | 		obstacles.resize(numObstacles*DIM*2);
164 | 		for (int obs = 0; obs < numObstacles; ++obs) {
165 | 			for (int d = 0; d < DIM*2; ++d) {
166 | 				getline(file, readValue, ',');	
167 | 				std::stringstream convertorObs(readValue);
168 | 				convertorObs >> obstacles[obs*DIM*2 + d];
169 | 			}
170 | 		}
171 | 
172 | 		if (verbose) {
173 | 			std::cout << "***** Inputs from " << mpp.filename << " are: *****" << std::endl;
174 | 		}
175 | 		if (verbose) {
176 | 			std::cout << "init is: "; printArray(&init[0],1,DIM,std::cout);
177 | 		}
178 | 		if (verbose) {
179 | 			std::cout << "goal is: "; printArray(&goal[0],1,DIM,std::cout);
180 | 		}
181 | 		if (verbose) {
182 | 			std::cout << "lo is: "; printArray(&lo[0],1,DIM,std::cout);
183 | 		}
184 | 		if (verbose) {
185 | 			std::cout << "hi is: "; printArray(&hi[0],1,DIM,std::cout);
186 | 		}
187 | 		if (verbose) {
188 | 			std::cout << "obstacle count = " << numObstacles << std::endl;
189 | 		}
190 | 		if(verbose) {
191 | 			printArray(&obstacles[0],numObstacles,2*DIM,std::cout);
192 | 		}
193 | 	} else {
194 | 		std::cout << "didn't read in file, bad file!" << std::endl;
195 | 	}
196 | 
197 | 	std::cout << "--- Motion planning problem, " << mpp.filename << " ---" << std::endl;
198 | 	std::cout << "Sample count = " << mpp.numSamples << ", C-space dim = " << mpp.dimC << ", Workspace dim = " << mpp.dimW << std::endl;
199 | 	std::cout << "hi = ["; for (int i = 0; i < mpp.dimC; ++i) { std::cout << hi[i] << " "; } std::cout << "], ";
200 | 	std::cout << "lo = ["; for (int i = 0; i < mpp.dimC; ++i) { std::cout << lo[i] << " "; } std::cout << "]" << std::endl;
201 | 	std::cout << "edge discretizations " << mpp.edgeDiscNum << ", dt = " << mpp.dt << std::endl;
202 | 
203 | 	/*********************** create array to return debugging information ***********************/
204 | 	float *d_debugOutput;
205 | 	cudaMalloc(&d_debugOutput, sizeof(float)*NUM);
206 | 
207 | 	// ***************** setup data structures and struct
208 | 
209 | 
210 | 	// ***************** precomputation
211 | 
212 | 	std::vector<float> samplesAll (DIM*NUM);
213 | 	createSamplesHalton(0, samplesAll.data(), &(init[0]), &(goal[0]), lo, hi);
214 | 	thrust::device_vector<float> d_samples_thrust(DIM*NUM);
215 | 	float *d_samples = thrust::raw_pointer_cast(d_samples_thrust.data());
216 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_samples, samplesAll.data(), sizeof(float)*DIM*NUM, cudaMemcpyHostToDevice));
217 | 
218 | 	// calculate nn
219 | 	int nullControl[3]; // throw away values for computing the cost of each connection
220 | 	float nullControlD[3]; 
221 | 
222 | 	double t_calc10thStart = std::clock();
223 | 	std::vector<float> topts ((NUM-1)*(NUM));
224 | 	std::vector<float> copts ((NUM-1)*(NUM));
225 | 	float percentile = 0.05;
226 | 	int rnIdx = (NUM-1)*(NUM)*percentile; // 10th quartile and number of NN
227 | 	int numEdges = rnIdx;
228 | 	std::cout << "rnIdx is " << rnIdx << std::endl;
229 | 	std::cout << "This is a bit slow as it is currently implemented on CPU, can be sped up significantly with a simple GPU implementation" << std::endl;
230 | 	int idx = 0;
231 | 	std::vector<float> c2g (NUM);
232 | 	for (int i = 0; i < NUM; ++i) {
233 | 		for (int j = 0; j < NUM; ++j) {
234 | 			if (j == i)
235 | 				continue;
236 | 			topts[idx] = dubinsAirplaneCost(&(samplesAll[i*DIM]), &(samplesAll[j*DIM]), nullControl, nullControlD);
237 | 			copts[idx] = dubinsAirplaneCost(&(samplesAll[i*DIM]), &(samplesAll[j*DIM]), nullControl, nullControlD);
238 | 			idx++;
239 | 		}
240 | 		c2g[i] = dubinsAirplaneCost(&(samplesAll[i*DIM]), &(samplesAll[goalIdx*DIM]), nullControl, nullControlD);
241 | 	}
242 | 	std::vector<float> coptsSorted ((NUM-1)*(NUM));
243 | 	coptsSorted = copts;
244 | 	std::sort (coptsSorted.begin(), coptsSorted.end());
245 | 	float rn = coptsSorted[rnIdx];
246 | 	double t_calc10th = (std::clock() - t_calc10thStart) / (double) CLOCKS_PER_SEC;
247 | 	std::cout << percentile << "th percentile pre calc took: " << t_calc10th*ms << " ms for " << idx << " solves and cutoff is " 
248 | 		<< rn << " at " << rnIdx << std::endl;	
249 | 
250 | 	double t_2pbvpTestStart = std::clock();
251 | 	float x0[DIM], x1[DIM];
252 | 
253 | 	double t_discMotionsStart = std::clock();
254 | 	std::vector<int> nnIdxs(NUM*NUM,-3);
255 | 	int nnGoSizes[NUM];
256 | 	int nnComeSizes[NUM];
257 | 	for (int i = 0; i < NUM; ++i) {
258 | 		nnGoSizes[i] = 0;
259 | 		nnComeSizes[i] = 0;
260 | 	}
261 | 	std::vector<float> discMotions (numEdges*(numControls*numDisc)*DIM,0); // array of motions, but its a vector who idk for some reason it won't work as an array
262 | 	int nnIdx = 0; // index position in NN discretization array
263 | 	idx = 0; // index position in copts vector above
264 | 
265 | 	std::vector<float> coptsEdge (numEdges); // edge index accessed copts
266 | 	std::vector<float> toptsEdge (numEdges); // edge index accessed topts
267 | 	std::vector<int> controlEdge (numEdges*numControls); // edge index accessed controls
268 | 	std::vector<float> controlDEdge (numEdges*numControls); // edge index accessed control durations
269 | 	for (int i = 0; i < NUM; ++i) {
270 | 		for (int d = 0; d < DIM; ++d)
271 | 			x0[d] = samplesAll[d + DIM*i];
272 | 		for (int j = 0; j < NUM; ++j) {
273 | 			if (j == i)
274 | 				continue;
275 | 			if (copts[idx] < rn) {
276 | 				coptsEdge[nnIdx] = copts[idx];
277 | 				toptsEdge[nnIdx] = topts[idx];
278 | 				for (int d = 0; d < DIM; ++d)
279 | 					x1[d] = samplesAll[d + DIM*j];
280 | 				nnIdxs[j*NUM+i] = nnIdx; // look up for discrete motions from i -> j
281 | 				float tmpC = dubinsAirplaneCost(x0, x1, &(controlEdge[nnIdx*numControls]), 
282 | 					&(controlDEdge[nnIdx*numControls]));
283 | 				dubinsAirplanePath(x0, x1, 
284 | 					&(controlEdge[nnIdx*numControls]), 
285 | 					&(controlDEdge[nnIdx*numControls]), 
286 | 					&(discMotions[nnIdx*DIM*(numControls*numDisc)]), numDisc);
287 | 				nnGoSizes[i]++;
288 | 				nnComeSizes[j]++;
289 | 				nnIdx++;
290 | 			}
291 | 			idx++;
292 | 		}
293 | 	}
294 | 	double t_discMotions = (std::clock() - t_discMotionsStart) / (double) CLOCKS_PER_SEC;
295 | 	std::cout << "Discretizing motions took: " << t_discMotions*ms << " ms for " << nnIdx << " solves" << std::endl;
296 | 
297 | 	// printArray(&(discMotions[20000*DIM]), 200, DIM, std::cout);
298 | 	// printArray(&(nnGoSizes[0]), 1, 1000, std::cout);
299 | 
300 | 	float *d_toptsEdge;
301 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_toptsEdge, sizeof(float)*numEdges));
302 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_toptsEdge, toptsEdge.data(), sizeof(float)*numEdges, cudaMemcpyHostToDevice));
303 | 	float *d_coptsEdge;
304 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_coptsEdge, sizeof(float)*numEdges));
305 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_coptsEdge, coptsEdge.data(), sizeof(float)*numEdges, cudaMemcpyHostToDevice));
306 | 
307 | 	int maxNNSize = 0;
308 | 	for (int i = 0; i < NUM; ++i) {
309 | 		if (maxNNSize < nnGoSizes[i])
310 | 			maxNNSize = nnGoSizes[i];
311 | 		if (maxNNSize < nnComeSizes[i])
312 | 			maxNNSize = nnComeSizes[i];
313 | 	}
314 | 	std::cout << "max number of nn is " << maxNNSize << std::endl;
315 | 
316 | 	std::vector<float> distancesCome (NUM*maxNNSize, 0);
317 | 	std::vector<int> nnGoEdges (NUM*maxNNSize, -1); // edge gives indices (i,j) to check nnIdx to then find the discretized path
318 | 	std::vector<int> nnComeEdges (NUM*maxNNSize, -1); // edge gives indices (j,i) to check nnIdx to then find the discretized path
319 | 	std::vector<float> adjCosts (NUM*NUM,10000);
320 | 	std::vector<float> adjTimes (NUM*NUM,10000);
321 | 	idx = 0;
322 | 	for (int i = 0; i < NUM; ++i) {
323 | 		nnGoSizes[i] = 0; // clear nnSizes again
324 | 		nnComeSizes[i] = 0; // clear nnSizes again
325 | 	}
326 | 	for (int i = 0; i < NUM; ++i) {
327 | 		for (int j = 0; j < NUM; ++j) {
328 | 			if (j == i)
329 | 				continue;
330 | 			if (copts[idx] < rn) {
331 | 				nnGoEdges[i*maxNNSize + nnGoSizes[i]] = j; // edge from i to j (i -> j)
332 | 				nnComeEdges[j*maxNNSize + nnComeSizes[j]] = i;
333 | 				distancesCome[j*maxNNSize + nnComeSizes[j]] = copts[idx];
334 | 				nnGoSizes[i]++;
335 | 				nnComeSizes[j]++;
336 | 				adjCosts[i*NUM + j] = copts[idx]; // cost to go from i to j
337 | 				adjTimes[i*NUM + j] = topts[idx]; // time to go from i to j
338 | 			}
339 | 			idx++;
340 | 		}
341 | 	}	
342 | 
343 | 	// put NN onto device
344 | 	float *d_discMotions;
345 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_discMotions, sizeof(float)*numEdges*(numControls*numDisc)*DIM));
346 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_discMotions, &discMotions[0], sizeof(float)*numEdges*(numControls*numDisc)*DIM, cudaMemcpyHostToDevice));
347 | 	// std::cout << "**** disc motions = " << std::endl;
348 | 	// printArray(&discMotions[0], 30, DIM, std::cout);
349 | 
350 | 	int *d_nnIdxs;
351 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_nnIdxs, sizeof(int)*NUM*NUM));
352 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_nnIdxs, &(nnIdxs[0]), sizeof(int)*NUM*NUM, cudaMemcpyHostToDevice));
353 | 
354 | 	float *d_distancesCome;
355 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_distancesCome, sizeof(float)*NUM*maxNNSize));
356 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_distancesCome, &(distancesCome[0]), sizeof(float)*NUM*maxNNSize, cudaMemcpyHostToDevice));
357 | 
358 | 	int *d_nnGoEdges;
359 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_nnGoEdges, sizeof(int)*NUM*maxNNSize));
360 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_nnGoEdges, &(nnGoEdges[0]), sizeof(int)*NUM*maxNNSize, cudaMemcpyHostToDevice));
361 | 
362 | 	int *d_nnComeEdges;
363 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_nnComeEdges, sizeof(int)*NUM*maxNNSize));
364 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_nnComeEdges, &(nnComeEdges[0]), sizeof(int)*NUM*maxNNSize, cudaMemcpyHostToDevice));
365 | 
366 | 	float *d_costs;
367 | 	cudaMalloc(&d_costs, sizeof(float)*NUM);
368 | 	thrust::device_vector<int> d_edges(NUM);
369 | 	int* d_edges_ptr = thrust::raw_pointer_cast(d_edges.data());
370 | 
371 | 
372 | 	// ***************** read in online problem parameters from filename input
373 | 	// obstacles
374 | 	std::cout << "Obstacle set, count = " << numObstacles << ":" << std::endl;
375 | 	// printArray(obstacles.data(), numObstacles, 2*DIM, std::cout);
376 | 
377 | 	// load obstacles on device
378 | 	float *d_obstacles;
379 | 	CUDA_ERROR_CHECK(cudaMalloc(&d_obstacles, sizeof(float)*2*numObstacles*DIM));
380 | 	CUDA_ERROR_CHECK(cudaMemcpy(d_obstacles, obstacles.data(), sizeof(float)*2*numObstacles*DIM, cudaMemcpyHostToDevice));
381 | 
382 | 	// sample free	
383 | 	bool isFreeSamples[NUM];
384 | 	thrust::device_vector<bool> d_isFreeSamples_thrust(NUM);
385 | 	bool* d_isFreeSamples = thrust::raw_pointer_cast(d_isFreeSamples_thrust.data());
386 | 
387 | 	double t_sampleFreeStart = std::clock();
388 | 	const int blockSizeSF = 192;
389 | 	const int gridSizeSF = std::min((NUM + blockSizeSF - 1) / blockSizeSF, 2147483647);
390 | 	if (gridSizeSF == 2147483647)
391 | 		std::cout << "...... ERROR: increase grid size for sampleFree" << std::endl;
392 | 	sampleFree<<<gridSizeSF, blockSizeSF>>>(
393 | 		d_obstacles, numObstacles, d_samples, d_isFreeSamples, d_debugOutput);
394 | 	cudaDeviceSynchronize();
395 | 	code = cudaPeekAtLastError();
396 | 	if (cudaSuccess != code) { std::cout << "ERROR on freeEdges: " << cudaGetErrorString(code) << std::endl; }
397 | 
398 | 	double t_sampleFree = (std::clock() - t_sampleFreeStart) / (double) CLOCKS_PER_SEC;
399 | 	std::cout << "Sample free took: " << t_sampleFree << " s" << std::endl;
400 | 	CUDA_ERROR_CHECK(cudaMemcpy(isFreeSamples, d_isFreeSamples, sizeof(bool)*NUM, cudaMemcpyDeviceToHost));
401 | 
402 | 	// run GMT
403 | 	double t_gmtStart = std::clock();
404 | 	std::cout << "Running wavefront expansion GMT" << std::endl;
405 | 	GMTwavefront(&(init[0]), &(goal[0]), d_obstacles, numObstacles,
406 | 		d_distancesCome, d_nnGoEdges, d_nnComeEdges, maxNNSize, d_discMotions, d_nnIdxs,
407 | 		d_samples, NUM, d_isFreeSamples, rn, numDisc*numControls-1,
408 | 		d_costs, d_edges_ptr, initIdx, goalIdx) ;
409 | 
410 | 	double t_gmt = (std::clock() - t_gmtStart) / (double) CLOCKS_PER_SEC;
411 | 	std::cout << "******** GMT took: " << t_gmt << " s" << std::endl;
412 | 	float costGoal = 0;
413 | 	cudaMemcpy(&costGoal, d_costs+goalIdx, sizeof(float), cudaMemcpyDeviceToHost);
414 | 	std::cout << "Solution cost: " << costGoal << std::endl;
415 | 	
416 | 	// ***************** output results
417 | 	std::ofstream matlabData;
418 | 	matlabData.open ("matlabInflationData.txt");
419 | 	matlabData << "obstacles.data() = ["; 
420 | 	printArray(obstacles.data(), 2*numObstacles, DIM, matlabData); 
421 | 	matlabData << "];" << std::endl;
422 | 
423 | 	true && printSolution(NUM, d_samples, d_edges_ptr, d_costs);
424 | 	matlabData.close();
425 | 
426 | 	// ***************** PRM
427 | 	std::vector<float> costs(NUM,10000);
428 | 	
429 | 	double t_PRMStart = std::clock();
430 | 	std::cout << "Running PRM" << std::endl;
431 | 	PRM(&(init[0]), &(goal[0]), d_obstacles, numObstacles,
432 | 		adjCosts, nnGoEdges, nnComeEdges, maxNNSize, d_discMotions, nnIdxs,
433 | 		d_samples, NUM, d_isFreeSamples, rn, numDisc*numControls-1, numEdges,
434 | 		costs, d_edges_ptr, initIdx, goalIdx,
435 | 		c2g);
436 | 	double t_PRM = (std::clock() - t_PRMStart) / (double) CLOCKS_PER_SEC;
437 | 	std::cout << "******** PRM took: " << t_PRM << " s" << std::endl;	
438 | 
439 | 	// ***************** FMT
440 | 	double t_FMTStart = std::clock();
441 | 	std::cout << "Running FMT" << std::endl;
442 | 	// call FMT
443 | 	FMTdub(&(init[0]), &(goal[0]), obstacles.data(), numObstacles,
444 | 		adjCosts, nnGoEdges, nnComeEdges, maxNNSize, discMotions, nnIdxs,
445 | 		NUM, d_isFreeSamples, rn, numDisc*numControls-1, numEdges,
446 | 		costs, initIdx, goalIdx,
447 | 		samplesAll, adjTimes,
448 | 		numControls, controlEdge, controlDEdge);
449 | 	
450 | 	double t_FMT = (std::clock() - t_FMTStart) / (double) CLOCKS_PER_SEC;
451 | 	std::cout << "******** FMT took: " << t_FMT << " s" << std::endl;
452 | 
453 | 	// ***************** free memory
454 | 	cudaFree(d_obstacles);
455 | 	cudaFree(d_toptsEdge);
456 | 	cudaFree(d_coptsEdge);
457 | 	cudaFree(d_discMotions);
458 | 	cudaFree(d_nnIdxs);
459 | 	cudaFree(d_distancesCome);
460 | 	cudaFree(d_nnGoEdges);
461 | 	cudaFree(d_nnComeEdges);
462 | 	cudaFree(d_costs);
463 | }


--------------------------------------------------------------------------------
/gpu/GMT.cu:
--------------------------------------------------------------------------------
  1 | /*
  2 | Author: brian ichter
  3 | Runs the group marching tree algorithm.
  4 | */
  5 | #include "GMT.cuh"
  6 | 
  7 | const int numDiscInit = 8;
  8 | 
  9 | void GMT(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
 10 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
 11 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
 12 | 	float *d_costs, int *d_edges, int initial_idx, int goal_idx)
 13 | {
 14 | 	// only implementating for lambda = 0.5 for now
 15 | 	// numBuckets = 2;
 16 | 	// double t_gmtStart = std::clock();
 17 | 	// int maxItrs = 100; 
 18 | 	
 19 | 	// bool *d_wavefrontWas, *d_unvisited, *d_isCollision;
 20 | 	// float *d_costGoal;
 21 | 	// thrust::device_vector<float> d_debugOutput(samplesCount*numDisc);
 22 | 	// thrust::device_vector<bool> d_wavefrontNew(samplesCount);
 23 | 	// thrust::device_vector<int> d_wavefrontScanIdx(samplesCount);
 24 | 	// thrust::device_vector<int> d_wavefrontActiveIdx(samplesCount);
 25 | 
 26 | 	// thrust::device_vector<bool> d_wavefront(numBuckets*samplesCount);
 27 | 
 28 | 	// float *d_debugOutput_ptr = thrust::raw_pointer_cast(d_debugOutput.data());
 29 | 	// bool *d_wavefront_ptr = thrust::raw_pointer_cast(d_wavefront.data());
 30 | 	// bool *d_wavefrontNew_ptr = thrust::raw_pointer_cast(d_wavefrontNew.data());
 31 | 	// int *d_wavefrontScanIdx_ptr = thrust::raw_pointer_cast(d_wavefrontScanIdx.data());
 32 | 	// int *d_wavefrontActiveIdx_ptr = thrust::raw_pointer_cast(d_wavefrontActiveIdx.data());
 33 | 
 34 | 	// cudaMalloc(&d_wavefrontWas, sizeof(bool)*samplesCount);
 35 | 	// cudaMalloc(&d_unvisited, sizeof(bool)*samplesCount);
 36 | 	// cudaMalloc(&d_costGoal, sizeof(float));
 37 | 	// cudaMalloc(&d_isCollision, sizeof(bool)*numDisc*samplesCount);
 38 | 
 39 | 	// if (d_unvisited == NULL) {
 40 | 	// 	std::cout << "Allocation Failure" << std::endl;
 41 | 	// 	exit(1);
 42 | 	// }
 43 | 
 44 | 	// // setup array values
 45 | 	// const int blockSize = 128;
 46 | 	// const int gridSize = std::min((samplesCount + blockSize - 1) / blockSize, 2147483647);
 47 | 	// setupArrays_buck<<<gridSize, blockSize>>>(d_wavefront_ptr, d_wavefrontNew_ptr, 
 48 | 	// d_wavefrontWas, d_unvisited, d_isFreeSamples, d_costGoal, d_costs, d_edges, samplesCount);
 49 | }
 50 | 
 51 | // __global__ void setupArrays_buck(bool* wavefront, bool* wavefrontNew, bool* wavefrontWas, bool* unvisited, 
 52 | // 	bool *isFreeSamples, float *costGoal, float* costs, int* edges, int samplesCount)
 53 | // {
 54 | // 	int node = blockIdx.x * blockDim.x + threadIdx.x;
 55 | // 	if (node >= samplesCount) 
 56 | // 		return;
 57 | 
 58 | // 	unvisited[node] = isFreeSamples[node];
 59 | // 	wavefrontNew[node] = false;
 60 | // 	wavefrontWas[node] = !isFreeSamples[node];
 61 | // 	if (node == 0) {		
 62 | // 		wavefront[node] = true;
 63 | // 		*costGoal = 0;
 64 | // 	} else {
 65 | // 		wavefront[node] = false;
 66 | // 	}
 67 | // 	wavefront[node + samplesCount] = false; // second bucket starts empty
 68 | // 	costs[node] = 0;
 69 | // 	edges[node] = -1;
 70 | 
 71 | // 	if (!isFreeSamples[node]) {
 72 | // 		costs[node] = -11;
 73 | // 		edges[node] = -2;
 74 | // 	}
 75 | // }
 76 | 
 77 | 
 78 | // ***************** takes new initial state not currently in the tree
 79 | void GMTinit(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
 80 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
 81 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
 82 | 	float *d_costs, int *d_edges, int initial_idx, int goal_idx)
 83 | {
 84 | 	cudaError_t code;
 85 | 	double t_gmtStart = std::clock();
 86 | 	int maxItrs = 100; 
 87 | 	
 88 | 	bool *d_wavefrontWas, *d_unvisited, *d_isCollision;
 89 | 	float *d_costGoal;
 90 | 	thrust::device_vector<float> d_debugOutput(samplesCount*numDisc);
 91 | 	thrust::device_vector<bool> d_wavefront(samplesCount);
 92 | 	thrust::device_vector<bool> d_wavefrontNew(samplesCount);
 93 | 	thrust::device_vector<int> d_wavefrontScanIdx(samplesCount);
 94 | 	thrust::device_vector<int> d_wavefrontActiveIdx(samplesCount);
 95 | 
 96 | 	float *d_debugOutput_ptr = thrust::raw_pointer_cast(d_debugOutput.data());
 97 | 	bool *d_wavefront_ptr = thrust::raw_pointer_cast(d_wavefront.data());
 98 | 	bool *d_wavefrontNew_ptr = thrust::raw_pointer_cast(d_wavefrontNew.data());
 99 | 	int *d_wavefrontScanIdx_ptr = thrust::raw_pointer_cast(d_wavefrontScanIdx.data());
100 | 	int *d_wavefrontActiveIdx_ptr = thrust::raw_pointer_cast(d_wavefrontActiveIdx.data());
101 | 
102 | 	cudaMalloc(&d_wavefrontWas, sizeof(bool)*samplesCount);
103 | 	cudaMalloc(&d_unvisited, sizeof(bool)*samplesCount);
104 | 	cudaMalloc(&d_costGoal, sizeof(float));
105 | 	cudaMalloc(&d_isCollision, sizeof(bool)*numDisc*samplesCount);
106 | 
107 | 	if (d_unvisited == NULL) {
108 | 		std::cout << "Allocation Failure" << std::endl;
109 | 		exit(1);
110 | 	}
111 | 
112 | 	int *d_edges_backup;
113 | 	cudaMalloc(&d_edges_backup, sizeof(int)*samplesCount);
114 | 	cudaMemcpy(d_edges_backup, d_edges, sizeof(int)*samplesCount, cudaMemcpyDeviceToDevice);
115 | 
116 | 	// setup array values
117 | 	const int blockSize = 128;
118 | 	const int gridSize = std::min((samplesCount + blockSize - 1) / blockSize, 2147483647);
119 | 	setupArrays<<<gridSize, blockSize>>>(
120 | 		d_wavefront_ptr, d_wavefrontNew_ptr, 
121 | 		d_wavefrontWas, d_unvisited, d_isFreeSamples, d_costGoal, d_costs, d_edges, samplesCount);
122 | 
123 | 	// attach initial condition (referred to as node number -2)
124 | 	// std::cout << "init" << initial[0] << ", " << initial[1] << ", " << initial[2] << ", " << initial[3] << std::endl;
125 | 	float *d_initial;
126 | 	cudaMalloc(&d_initial, sizeof(float)*DIM);
127 | 	cudaMemcpy(d_initial, initial, sizeof(float)*DIM, cudaMemcpyHostToDevice);
128 | 	cudaDeviceSynchronize();
129 | 	// std::cout << "attaching wavefront to "<< initial[0] << ", " << initial[1] << ", " << initial[2] << ", " << initial[3] << std::endl;
130 | 	attachWavefront<<<gridSize,blockSize>>>(
131 | 		samplesCount, d_samples, d_initial, 
132 | 		r, goal_idx, d_wavefront_ptr, d_edges,
133 | 		obstaclesCount, d_obstacles, 
134 | 		d_costs, d_costGoal, d_debugOutput_ptr);
135 | 	cudaDeviceSynchronize();
136 | 	code = cudaPeekAtLastError();
137 | 	if (cudaSuccess != code) { std::cout << "ERROR on attachWavefront: " << cudaGetErrorString(code) << std::endl; }
138 | 	// std::cout << "wavefront attached "<< initial[0] << ", " << initial[1] << ", " << initial[2] << ", " << initial[3] << std::endl;
139 | 
140 | 	float costGoal = 0;
141 | 	int itrs = 0;
142 | 	int activeSize = 0;
143 | 	while (itrs < maxItrs && costGoal == 0)
144 | 	{ 
145 | 		++itrs;
146 | 
147 | 		// std::cout << "scanning" << std::endl;
148 | 
149 | 		// prefix sum to fill wavefrontScanIdx to find points in the current band of the wavefront
150 | 		thrust::exclusive_scan(d_wavefront.begin(), d_wavefront.end(), d_wavefrontScanIdx.begin());
151 | 		fillWavefront<<<gridSize, blockSize>>>(samplesCount,
152 | 			d_wavefrontActiveIdx_ptr, d_wavefrontScanIdx_ptr, d_wavefront_ptr);
153 | 		// std::cout << "scanned" << std::endl;
154 | 
155 | 		activeSize = d_wavefrontScanIdx[samplesCount-1];
156 | 		// increase by one if the last point in array is true because exclusive scan doesnt increment otherwise
157 | 		(d_wavefront[d_wavefront.size() - 1]) ? ++activeSize : 0;
158 | 
159 | 		// std::cout << "new DIM = " << DIM << " Itr " << itrs << " size " << activeSize << std::endl;
160 | 
161 | 		if (activeSize == 0) {
162 | 			// failed to connect to the tree (often this is from the initial condition, just return the same tree that came in)
163 | 			cudaMemcpy(d_edges, d_edges_backup, sizeof(int)*samplesCount, cudaMemcpyDeviceToDevice);
164 | 			return;
165 | 		}
166 | 
167 | 		const int blockSizeActive = 128;
168 | 		const int gridSizeActive = std::min((activeSize + blockSizeActive - 1) / blockSizeActive, 2147483647);
169 | 		
170 | 		// std::cout << "find wavefront (" << blockSizeActive << ", " gridSizeActive << ")" << std::endl;
171 | 		findWavefront<<<gridSizeActive, blockSizeActive>>>(
172 | 			d_unvisited, d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas, 
173 | 			d_nnGoEdges, nnSize, activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
174 | 
175 | 
176 | 		// prefix sum to fill wavefrontScanIdx to connect to the next wavefront
177 | 		thrust::exclusive_scan(d_wavefrontNew.begin(), d_wavefrontNew.end(), d_wavefrontScanIdx.begin());
178 | 		
179 | 		// to print thrust vector) thrust::copy(d_wavefrontScanIdx.begin(), d_wavefrontScanIdx.end(), std::ostream_iterator<int>(std::cout, " "));
180 | 
181 | 		fillWavefront<<<gridSize, blockSize>>>(
182 | 			samplesCount, d_wavefrontActiveIdx_ptr,
183 | 			d_wavefrontScanIdx_ptr, d_wavefrontNew_ptr);
184 | 
185 | 
186 | 		activeSize = d_wavefrontScanIdx[samplesCount-1]; 
187 | 		// increase by one if the last point in array is true because exclusive scan doesnt increment otherwise
188 | 		(d_wavefrontNew[d_wavefrontNew.size() - 1]) ?  ++activeSize : 0;
189 | 		if (activeSize == 0) // the next wavefront is empty (only valid for GMTwavefront)
190 | 			break;
191 | 
192 | 		// std::cout << "exp DIM = " << DIM << " Itr " << itrs << " size " << activeSize << std::endl;
193 | 
194 | 		const int blockSizeActiveExp = 128;
195 | 		const int gridSizeActiveExp = std::min((activeSize + blockSizeActiveExp - 1) / blockSizeActiveExp, 2147483647);
196 | 		findOptimalConnection<<<gridSizeActiveExp, blockSizeActiveExp>>>(
197 | 			d_wavefront_ptr, d_edges, d_costs, d_distancesCome, 
198 | 			d_nnComeEdges, nnSize, activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
199 | 
200 | 		const int blockSizeActiveVerify = 128;
201 | 		const int gridSizeActiveVerify = std::min((activeSize*numDisc + blockSizeActiveVerify - 1) / blockSizeActiveVerify, 2147483647);
202 | 		verifyExpansion<<<gridSizeActiveVerify, blockSizeActiveVerify>>>(
203 | 			obstaclesCount, d_wavefrontNew_ptr, d_edges,
204 | 			d_samples, d_obstacles, d_costs, 
205 | 			d_nnIdxs, d_discMotions, numDisc, d_isCollision,
206 | 			activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
207 | 
208 | 
209 | 		removeInvalidExpansions<<<gridSizeActiveExp, blockSizeActiveExp>>>(
210 | 			obstaclesCount, d_wavefrontNew_ptr, d_edges,
211 | 			d_samples, d_obstacles, d_costs, 
212 | 			d_nnIdxs, d_discMotions, numDisc, d_isCollision,
213 | 			activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
214 | 
215 | 		// std::cout << "debug output: " << std::endl;
216 | 		// thrust::copy(d_debugOutput.begin(), d_debugOutput.begin()+100, std::ostream_iterator<float>(std::cout, " "));
217 | 		updateWavefront<<<gridSize, blockSize>>>(
218 | 			samplesCount, goal_idx,
219 | 			d_unvisited, d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas,
220 | 			d_costs, d_costGoal, d_debugOutput_ptr);
221 | 
222 | 		// copy over the goal cost (if non zero, then a solution was found)
223 | 		cudaMemcpy(&costGoal, d_costGoal, sizeof(float), cudaMemcpyDeviceToHost);
224 | 	}
225 | 	double t_gmt = (std::clock() - t_gmtStart) / (double) CLOCKS_PER_SEC;
226 | 	// std::cout << "time inside GMT is " << t_gmt << std::endl;
227 | 		
228 | 	// free arrays
229 | 	cudaFree(d_wavefrontWas);
230 | 	cudaFree(d_unvisited);
231 | 	cudaFree(d_costGoal);
232 | 	cudaFree(d_isCollision);
233 | 	cudaFree(d_initial);
234 | }
235 | 
236 | // ***************** takes new initial and goal state not currently in the tree
237 | int GMTinitGoal(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
238 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
239 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
240 | 	float *d_costs, int *d_edges, int initial_idx)
241 | {
242 | 	int goal_idx = -10; // does not implement this style goal checking
243 | 
244 | 	cudaError_t code;
245 | 	double t_gmtStart = std::clock();
246 | 	int maxItrs = 100; 
247 | 	
248 | 	bool *d_wavefrontWas, *d_unvisited, *d_isCollision;
249 | 	float *d_costGoal;
250 | 	thrust::device_vector<float> d_debugOutput(samplesCount*numDisc);
251 | 	thrust::device_vector<bool> d_wavefront(samplesCount);
252 | 	thrust::device_vector<bool> d_wavefrontNew(samplesCount);
253 | 	thrust::device_vector<int> d_wavefrontScanIdx(samplesCount);
254 | 	thrust::device_vector<int> d_wavefrontActiveIdx(samplesCount);
255 | 
256 | 	float *d_debugOutput_ptr = thrust::raw_pointer_cast(d_debugOutput.data());
257 | 	bool *d_wavefront_ptr = thrust::raw_pointer_cast(d_wavefront.data());
258 | 	bool *d_wavefrontNew_ptr = thrust::raw_pointer_cast(d_wavefrontNew.data());
259 | 	int *d_wavefrontScanIdx_ptr = thrust::raw_pointer_cast(d_wavefrontScanIdx.data());
260 | 	int *d_wavefrontActiveIdx_ptr = thrust::raw_pointer_cast(d_wavefrontActiveIdx.data());
261 | 
262 | 	cudaMalloc(&d_wavefrontWas, sizeof(bool)*samplesCount);
263 | 	cudaMalloc(&d_unvisited, sizeof(bool)*samplesCount);
264 | 	cudaMalloc(&d_costGoal, sizeof(float));
265 | 	cudaMalloc(&d_isCollision, sizeof(bool)*numDisc*samplesCount);
266 | 
267 | 	if (d_unvisited == NULL) {
268 | 		std::cout << "Allocation Failure" << std::endl;
269 | 		exit(1);
270 | 	}
271 | 
272 | 	int *d_edges_backup;
273 | 	cudaMalloc(&d_edges_backup, sizeof(int)*samplesCount);
274 | 	cudaMemcpy(d_edges_backup, d_edges, sizeof(int)*samplesCount, cudaMemcpyDeviceToDevice);
275 | 
276 | 	// setup array values
277 | 	const int blockSize = 128;
278 | 	const int gridSize = std::min((samplesCount + blockSize - 1) / blockSize, 2147483647);
279 | 	setupArrays<<<gridSize, blockSize>>>(
280 | 		d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas, d_unvisited, d_isFreeSamples,
281 | 		d_costGoal, d_costs, d_edges, samplesCount);
282 | 
283 | 	// attach initial condition (referred to as node number -2)
284 | 	// std::cout << "init" << initial[0] << ", " << initial[1] << ", " << initial[2] << ", " << initial[3] << std::endl;
285 | 	float *d_initial;
286 | 	cudaMalloc(&d_initial, sizeof(float)*DIM);
287 | 	cudaMemcpy(d_initial, initial, sizeof(float)*DIM, cudaMemcpyHostToDevice);
288 | 	cudaDeviceSynchronize();
289 | 	// std::cout << "attaching wavefront to "<< initial[0] << ", " << initial[1] << ", " << initial[2] << ", " << initial[3] << std::endl;
290 | 	attachWavefront<<<gridSize,blockSize>>>(
291 | 		samplesCount, d_samples, d_initial, r, goal_idx, d_wavefront_ptr, d_edges,
292 | 		obstaclesCount, d_obstacles, d_costs, d_costGoal, d_debugOutput_ptr);
293 | 	cudaDeviceSynchronize();
294 | 	code = cudaPeekAtLastError();
295 | 	if (cudaSuccess != code) { std::cout << "ERROR on attachWavefront: " << cudaGetErrorString(code) << std::endl; }
296 | 	// std::cout << "wavefront attached "<< initial[0] << ", " << initial[1] << ", " << initial[2] << ", " << initial[3] << std::endl;
297 | 
298 | 	// setup goal condition
299 | 	bool *d_inGoal;
300 | 	cudaMalloc(&d_inGoal, sizeof(bool)*samplesCount);
301 | 	float *d_goal;
302 | 	cudaMalloc(&d_goal, sizeof(float)*DIM);
303 | 	cudaMemcpy(d_goal, goal, sizeof(float)*DIM, cudaMemcpyHostToDevice);
304 | 
305 | 	float goalD2 = 0.02; // define a ball around the goal region
306 | 	// TODO: verify initial condition isn't already in the goal region
307 | 	// TODO: check if initial attaching finds a solution
308 | 
309 | 	buildInGoal<<<gridSize,blockSize>>>(samplesCount, d_samples, d_goal, goalD2,
310 | 		d_inGoal, d_debugOutput_ptr);
311 | 	cudaDeviceSynchronize();
312 | 	code = cudaPeekAtLastError();
313 | 	if (cudaSuccess != code) { std::cout << "ERROR on buildInGoal: " << cudaGetErrorString(code) << std::endl; }
314 | 	
315 | 	bool inGoal[samplesCount];
316 | 	CUDA_ERROR_CHECK(cudaMemcpy(inGoal, d_inGoal, sizeof(bool)*samplesCount, cudaMemcpyDeviceToHost));
317 | 	bool wavefront[samplesCount];
318 | 
319 | 	float costGoal = 0;
320 | 	int itrs = 0;
321 | 	int activeSize = 0;
322 | 	while (itrs < maxItrs && costGoal == 0)
323 | 	{ 
324 | 		++itrs;
325 | 
326 | 		// prefix sum to fill wavefrontScanIdx to find points in the current band of the wavefront
327 | 		thrust::exclusive_scan(d_wavefront.begin(), d_wavefront.end(), d_wavefrontScanIdx.begin());
328 | 		fillWavefront<<<gridSize, blockSize>>>(samplesCount,
329 | 			d_wavefrontActiveIdx_ptr, d_wavefrontScanIdx_ptr, d_wavefront_ptr);
330 | 		// std::cout << "scanned" << std::endl;
331 | 
332 | 		activeSize = d_wavefrontScanIdx[samplesCount-1];
333 | 		// increase by one if the last point in array is true because exclusive scan doesnt increment otherwise
334 | 		(d_wavefront[d_wavefront.size() - 1]) ? ++activeSize : 0;
335 | 
336 | 		std::cout << "new DIM = " << DIM << " Itr " << itrs << " size " << activeSize << std::endl;
337 | 
338 | 		if (activeSize == 0) {
339 | 			// failed to connect to the tree (often this is from the initial condition, just return the same tree that came in)
340 | 			cudaMemcpy(d_edges, d_edges_backup, sizeof(int)*samplesCount, cudaMemcpyDeviceToDevice);
341 | 			return -1;
342 | 		}
343 | 
344 | 		const int blockSizeActive = 128;
345 | 		const int gridSizeActive = std::min((activeSize + blockSizeActive - 1) / blockSizeActive, 2147483647);
346 | 		
347 | 		// std::cout << "find wavefront (" << blockSizeActive << ", " gridSizeActive << ")" << std::endl;
348 | 		findWavefront<<<gridSizeActive, blockSizeActive>>>(
349 | 			d_unvisited, d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas, 
350 | 			d_nnGoEdges, nnSize, activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
351 | 
352 | 
353 | 		// prefix sum to fill wavefrontScanIdx to connect to the next wavefront
354 | 		thrust::exclusive_scan(d_wavefrontNew.begin(), d_wavefrontNew.end(), d_wavefrontScanIdx.begin());
355 | 		
356 | 		// to print thrust vector) thrust::copy(d_wavefrontScanIdx.begin(), d_wavefrontScanIdx.end(), std::ostream_iterator<int>(std::cout, " "));
357 | 
358 | 		fillWavefront<<<gridSize, blockSize>>>(
359 | 			samplesCount, d_wavefrontActiveIdx_ptr,
360 | 			d_wavefrontScanIdx_ptr, d_wavefrontNew_ptr);
361 | 
362 | 
363 | 		activeSize = d_wavefrontScanIdx[samplesCount-1]; 
364 | 		// increase by one if the last point in array is true because exclusive scan doesnt increment otherwise
365 | 		(d_wavefrontNew[d_wavefrontNew.size() - 1]) ?  ++activeSize : 0;
366 | 		if (activeSize == 0) // the next wavefront is empty (only valid for GMTwavefront)
367 | 			break;
368 | 
369 | 		// std::cout << "exp DIM = " << DIM << " Itr " << itrs << " size " << activeSize << std::endl;
370 | 
371 | 		const int blockSizeActiveExp = 128;
372 | 		const int gridSizeActiveExp = std::min((activeSize + blockSizeActiveExp - 1) / blockSizeActiveExp, 2147483647);
373 | 		findOptimalConnection<<<gridSizeActiveExp, blockSizeActiveExp>>>(
374 | 			d_wavefront_ptr, d_edges, d_costs, d_distancesCome, 
375 | 			d_nnComeEdges, nnSize, activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
376 | 
377 | 		const int blockSizeActiveVerify = 128;
378 | 		const int gridSizeActiveVerify = std::min((activeSize*numDisc + blockSizeActiveVerify - 1) / blockSizeActiveVerify, 2147483647);
379 | 		verifyExpansion<<<gridSizeActiveVerify, blockSizeActiveVerify>>>(
380 | 			obstaclesCount, d_wavefrontNew_ptr, d_edges,
381 | 			d_samples, d_obstacles, d_costs, 
382 | 			d_nnIdxs, d_discMotions, numDisc, d_isCollision,
383 | 			activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
384 | 
385 | 
386 | 		removeInvalidExpansions<<<gridSizeActiveExp, blockSizeActiveExp>>>(
387 | 			obstaclesCount, d_wavefrontNew_ptr, d_edges,
388 | 			d_samples, d_obstacles, d_costs, 
389 | 			d_nnIdxs, d_discMotions, numDisc, d_isCollision,
390 | 			activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
391 | 
392 | 		// std::cout << "debug output: " << std::endl;
393 | 		// thrust::copy(d_debugOutput.begin(), d_debugOutput.begin()+100, std::ostream_iterator<float>(std::cout, " "));
394 | 		updateWavefront<<<gridSize, blockSize>>>(
395 | 			samplesCount, goal_idx,
396 | 			d_unvisited, d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas,
397 | 			d_costs, d_costGoal, d_debugOutput_ptr);
398 | 
399 | 		// check if any nodes are in the goal region
400 | 		cudaMemcpy(wavefront, d_wavefront_ptr, sizeof(bool)*samplesCount, cudaMemcpyDeviceToHost);
401 | 		float costs[samplesCount];
402 | 		cudaMemcpy(costs, d_costs, sizeof(float)*samplesCount, cudaMemcpyDeviceToHost);
403 | 		for (int node = 0; node < samplesCount; ++node) {
404 | 			if (wavefront[node] && inGoal[node]) {
405 | 				if (costGoal == 0) {
406 | 					costGoal = costs[node];
407 | 					goal_idx = node;
408 | 				} else if (costGoal > costs[node]) {
409 | 					costGoal = costs[node];
410 | 					goal_idx = node;
411 | 				} 
412 | 				std::cout << "solved! with cost " << costGoal << " at node " << node << std::endl;
413 | 			}
414 | 		}
415 | 	}
416 | 	double t_gmt = (std::clock() - t_gmtStart) / (double) CLOCKS_PER_SEC;
417 | 	// std::cout << "time inside GMT is " << t_gmt << std::endl;
418 | 		
419 | 	// free arrays
420 | 	cudaFree(d_wavefrontWas);
421 | 	cudaFree(d_unvisited);
422 | 	cudaFree(d_costGoal);
423 | 	cudaFree(d_isCollision);
424 | 	cudaFree(d_initial);
425 | 	return goal_idx;
426 | }
427 | 
428 | // ***************** normal GMT with lambda = 1
429 | void GMTwavefront(float *initial, float *goal, float *d_obstacles, int obstaclesCount,
430 | 	float *d_distancesCome, int *d_nnGoEdges, int *d_nnComeEdges, int nnSize, float *d_discMotions, int *d_nnIdxs,
431 | 	float *d_samples, int samplesCount, bool *d_isFreeSamples, float r, int numDisc,
432 | 	float *d_costs, int *d_edges, int initial_idx, int goal_idx)
433 | {
434 | 	double t_gmtStart = std::clock();
435 | 	int maxItrs = 100; 
436 | 	
437 | 	bool *d_wavefrontWas, *d_unvisited, *d_isCollision;
438 | 	float *d_costGoal;
439 | 	thrust::device_vector<float> d_debugOutput(samplesCount*numDisc);
440 | 	thrust::device_vector<bool> d_wavefront(samplesCount);
441 | 	thrust::device_vector<bool> d_wavefrontNew(samplesCount);
442 | 	thrust::device_vector<int> d_wavefrontScanIdx(samplesCount);
443 | 	thrust::device_vector<int> d_wavefrontActiveIdx(samplesCount);
444 | 
445 | 	float *d_debugOutput_ptr = thrust::raw_pointer_cast(d_debugOutput.data());
446 | 	bool *d_wavefront_ptr = thrust::raw_pointer_cast(d_wavefront.data());
447 | 	bool *d_wavefrontNew_ptr = thrust::raw_pointer_cast(d_wavefrontNew.data());
448 | 	int *d_wavefrontScanIdx_ptr = thrust::raw_pointer_cast(d_wavefrontScanIdx.data());
449 | 	int *d_wavefrontActiveIdx_ptr = thrust::raw_pointer_cast(d_wavefrontActiveIdx.data());
450 | 
451 | 	cudaMalloc(&d_wavefrontWas, sizeof(bool)*samplesCount);
452 | 	cudaMalloc(&d_unvisited, sizeof(bool)*samplesCount);
453 | 	cudaMalloc(&d_costGoal, sizeof(float));
454 | 	cudaMalloc(&d_isCollision, sizeof(bool)*numDisc*samplesCount);
455 | 
456 | 	if (d_unvisited == NULL) {
457 | 		std::cout << "Allocation Failure" << std::endl;
458 | 		exit(1);
459 | 	}
460 | 
461 | 	// setup array values
462 | 	const int blockSize = 128;
463 | 	const int gridSize = std::min((samplesCount + blockSize - 1) / blockSize, 2147483647);
464 | 	setupArrays<<<gridSize, blockSize>>>(d_wavefront_ptr, d_wavefrontNew_ptr, 
465 | 		d_wavefrontWas, d_unvisited, d_isFreeSamples, d_costGoal, d_costs, d_edges, samplesCount);
466 | 	
467 | 	float costGoal = 0;
468 | 	int itrs = 0;
469 | 	int activeSize = 0;
470 | 	while (itrs < maxItrs && costGoal == 0)
471 | 	{ 
472 | 		++itrs;
473 | 
474 | 		// prefix sum to fill wavefrontScanIdx to find points in the current band of the wavefront
475 | 		thrust::exclusive_scan(d_wavefront.begin(), d_wavefront.end(), d_wavefrontScanIdx.begin());
476 | 		fillWavefront<<<gridSize, blockSize>>>(samplesCount,
477 | 			d_wavefrontActiveIdx_ptr, d_wavefrontScanIdx_ptr, d_wavefront_ptr);
478 | 
479 | 		activeSize = d_wavefrontScanIdx[samplesCount-1];
480 | 		// increase by one if the last point in array is true because exclusive scan doesnt increment otherwise
481 | 		(d_wavefront[d_wavefront.size() - 1]) ? ++activeSize : 0;
482 | 
483 | 		std::cout << "new DIM = " << DIM << " Itr " << itrs << " size " << activeSize << std::endl;
484 | 
485 | 		const int blockSizeActive = 128;
486 | 		const int gridSizeActive = std::min((activeSize + blockSizeActive - 1) / blockSizeActive, 2147483647);
487 | 		
488 | 		findWavefront<<<gridSizeActive, blockSizeActive>>>(
489 | 			d_unvisited, d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas, 
490 | 			d_nnGoEdges, nnSize, activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
491 | 
492 | 		// prefix sum to fill wavefrontScanIdx to connect to the next wavefront
493 | 		thrust::exclusive_scan(d_wavefrontNew.begin(), d_wavefrontNew.end(), d_wavefrontScanIdx.begin());
494 | 		
495 | 		// to print thrust vector) thrust::copy(d_wavefrontScanIdx.begin(), d_wavefrontScanIdx.end(), std::ostream_iterator<int>(std::cout, " "));
496 | 
497 | 		fillWavefront<<<gridSize, blockSize>>>(
498 | 			samplesCount, d_wavefrontActiveIdx_ptr,
499 | 			d_wavefrontScanIdx_ptr, d_wavefrontNew_ptr);
500 | 
501 | 		activeSize = d_wavefrontScanIdx[samplesCount-1]; 
502 | 		// increase by one if the last point in array is true because exclusive scan doesnt increment otherwise
503 | 		(d_wavefrontNew[d_wavefrontNew.size() - 1]) ?  ++activeSize : 0;
504 | 		if (activeSize == 0) // the next wavefront is empty (only valid for GMTwavefront)
505 | 			break;
506 | 
507 | 		std::cout << "exp DIM = " << DIM << " Itr " << itrs << " size " << activeSize << std::endl;
508 | 
509 | 		const int blockSizeActiveExp = 128;
510 | 		const int gridSizeActiveExp = std::min((activeSize + blockSizeActiveExp - 1) / blockSizeActiveExp, 2147483647);
511 | 		findOptimalConnection<<<gridSizeActiveExp, blockSizeActiveExp>>>(
512 | 			d_wavefront_ptr, d_edges, d_costs, d_distancesCome, 
513 | 			d_nnComeEdges, nnSize, activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
514 | 		const int blockSizeActiveVerify = 128;
515 | 		const int gridSizeActiveVerify = std::min((activeSize*numDisc + blockSizeActiveVerify - 1) / blockSizeActiveVerify, 2147483647);
516 | 		verifyExpansion<<<gridSizeActiveVerify, blockSizeActiveVerify>>>(
517 | 			obstaclesCount, d_wavefrontNew_ptr, d_edges,
518 | 			d_samples, d_obstacles, d_costs, 
519 | 			d_nnIdxs, d_discMotions, numDisc, d_isCollision,
520 | 			activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
521 | 		removeInvalidExpansions<<<gridSizeActiveExp, blockSizeActiveExp>>>(
522 | 			obstaclesCount, d_wavefrontNew_ptr, d_edges,
523 | 			d_samples, d_obstacles, d_costs, 
524 | 			d_nnIdxs, d_discMotions, numDisc, d_isCollision,
525 | 			activeSize, d_wavefrontActiveIdx_ptr, d_debugOutput_ptr);
526 | 		// std::cout << "debug output: " << std::endl;
527 | 		// thrust::copy(d_debugOutput.begin(), d_debugOutput.begin()+100, std::ostream_iterator<float>(std::cout, " "));
528 | 		updateWavefront<<<gridSize, blockSize>>>(
529 | 			samplesCount, goal_idx,
530 | 			d_unvisited, d_wavefront_ptr, d_wavefrontNew_ptr, d_wavefrontWas,
531 | 			d_costs, d_costGoal, d_debugOutput_ptr);
532 | 
533 | 		// copy over the goal cost (if non zero, then a solution was found)
534 | 		cudaMemcpy(&costGoal, d_costGoal, sizeof(float), cudaMemcpyDeviceToHost);
535 | 	}
536 | 	double t_gmt = (std::clock() - t_gmtStart) / (double) CLOCKS_PER_SEC;
537 | 	std::cout << "time inside GMT is " << t_gmt << std::endl;
538 | 		
539 | 	// free arrays
540 | 	cudaFree(d_wavefrontWas);
541 | 	cudaFree(d_unvisited);
542 | 	cudaFree(d_costGoal);
543 | 	cudaFree(d_isCollision);
544 | }
545 | 
546 | __global__ void setupArrays(bool* wavefront, bool* wavefrontNew, bool* wavefrontWas, bool* unvisited, 
547 | 	bool *isFreeSamples, float *costGoal, float* costs, int* edges, int samplesCount)
548 | {
549 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
550 | 	if (node >= samplesCount) 
551 | 		return;
552 | 
553 | 	unvisited[node] = isFreeSamples[node];
554 | 	wavefrontNew[node] = false;
555 | 	wavefrontWas[node] = !isFreeSamples[node];
556 | 	if (node == 0) {		
557 | 		wavefront[node] = true;
558 | 		*costGoal = 0;
559 | 	} else {
560 | 		wavefront[node] = false;
561 | 	}
562 | 	costs[node] = 0;
563 | 	edges[node] = -1;
564 | 
565 | 	if (!isFreeSamples[node]) {
566 | 		costs[node] = -11;
567 | 		edges[node] = -2;
568 | 	}
569 | }
570 | 
571 | __global__
572 | void findWavefront(bool *unvisited, bool *wavefront, bool *wavefrontNew, bool *wavefrontWas,
573 | 	int *nnGoEdges, int nnSize, int wavefrontSize, int* wavefrontActiveIdx, float* debugOutput)
574 | {
575 | 	int tid = blockIdx.x * blockDim.x + threadIdx.x;
576 | 	if (tid >= wavefrontSize)
577 | 		return;
578 | 
579 | 	int node = wavefrontActiveIdx[tid];
580 | 
581 | 	if (wavefront[node])
582 | 	{
583 | 		wavefrontWas[node] = true;
584 | 		for (int i = 0; i < nnSize; ++i) {
585 | 			int nnIdx = nnGoEdges[node*nnSize + i];
586 | 			if (nnIdx == -1)
587 | 				return;
588 | 			if (unvisited[nnIdx] && !wavefront[nnIdx]) {
589 | 				wavefrontNew[nnIdx] = true;
590 | 			}
591 | 		}
592 | 	}
593 | }
594 | 
595 | __global__
596 | void fillWavefront(int samplesCount, int *wavefrontActiveIdx, int *wavefrontScanIdx, bool* wavefront)
597 | {
598 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
599 | 	if (node >= samplesCount)
600 | 		return;
601 | 	if (!wavefront[node])
602 | 		return;
603 | 
604 | 	wavefrontActiveIdx[wavefrontScanIdx[node]] = node;
605 | }
606 | 
607 | __global__
608 | void findOptimalConnection(bool *wavefront, int* edges, float* costs, float *distancesCome, 
609 | 	int *nnComeEdges, int nnSize, int wavefrontSize, int* wavefrontActiveIdx, float* debugOutput)
610 | {
611 | 	int tid = blockIdx.x * blockDim.x + threadIdx.x;
612 | 	if (tid >= wavefrontSize)
613 | 		return;
614 | 
615 | 	int node = wavefrontActiveIdx[tid];
616 | 
617 | 	for (int i = 0; i < nnSize; ++i) {
618 | 		int nnIdx = nnComeEdges[node*nnSize + i];
619 | 		if (nnIdx == -1)
620 | 			break;
621 | 		if (wavefront[nnIdx]) {
622 | 			float costTmp = costs[nnIdx] + distancesCome[node*nnSize + i];
623 | 			if (costs[node] == 0 || costTmp < costs[node]) {
624 | 				costs[node] = costTmp;
625 | 				edges[node] = nnIdx;
626 | 			}
627 | 		}
628 | 	}
629 | } 
630 | 
631 | __global__
632 | void verifyExpansion(int obstaclesCount, bool *wavefrontNew, int* edges,
633 | 	float *samples, float* obstacles, float* costs,
634 | 	int *nnIdxs, float *discMotions, int numDisc, bool *isCollision,
635 | 	int wavefrontSize, int* wavefrontActiveIdx, float* debugOutput)
636 | {
637 | 	int tid = blockIdx.x * blockDim.x + threadIdx.x;
638 | 	if (tid >= wavefrontSize*numDisc)
639 | 		return;
640 | 	int nodeIdx = tid/numDisc;
641 | 	int discIdx = tid%numDisc;
642 | 	int node = wavefrontActiveIdx[nodeIdx];
643 | 	
644 | 	waypointCollisionCheck(node, edges[node], obstaclesCount, obstacles, 
645 | 			nnIdxs, discMotions, discIdx, numDisc, isCollision, tid, debugOutput);
646 | }
647 | 
648 | __global__
649 | void removeInvalidExpansions(int obstaclesCount, bool *wavefrontNew, int* edges,
650 | 	float *samples, float* obstacles, float* costs,
651 | 	int *nnIdxs, float *discMotions, int numDisc, bool *isCollision,
652 | 	int wavefrontSize, int* wavefrontActiveIdx, float* debugOutput)
653 | {
654 | 	int tid = blockIdx.x * blockDim.x + threadIdx.x;
655 | 	if (tid >= wavefrontSize)
656 | 		return;
657 | 	int node = wavefrontActiveIdx[tid];
658 | 
659 | 	bool notValid = isCollision[tid*numDisc];
660 | 	for (int i = 1; i < numDisc; ++i) 
661 | 		notValid = notValid || isCollision[tid*numDisc + i];
662 | 	if (notValid) {
663 | 		costs[node] = 0;
664 | 		edges[node] = -1;
665 | 		wavefrontNew[node] = false;
666 | 	}
667 | }
668 | 
669 | __global__
670 | void updateWavefront(int samplesCount, int goal_idx,
671 | 	bool *unvisited, bool *wavefront, bool *wavefrontNew, bool *wavefrontWas,
672 | 	float* costs, float* costGoal, float* debugOutput)
673 | {
674 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
675 | 
676 | 	if (node >= samplesCount)
677 | 		return;
678 | 	if (!unvisited[node])
679 | 		return;
680 | 
681 | 	if (wavefrontWas[node]) {
682 | 		wavefront[node] = false; // remove from the current wavefront
683 | 		unvisited[node] = false; // remove from unvisited
684 | 	} else if (wavefrontNew[node]) {
685 | 		wavefront[node] = true; // add to wavefront
686 | 		wavefrontNew[node] = false;
687 | 		if (node == goal_idx)
688 | 			*costGoal = costs[node];
689 | 	}
690 | }
691 | 
692 | __global__
693 | void attachWavefront(int samplesCount, float *samples, float *initial, 
694 | 	float r, int goal_idx, bool *wavefront, int *edges,
695 | 	int obstaclesCount, float *obstacles, 
696 | 	float* costs, float* costGoal, float* debugOutput)
697 | {
698 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
699 | 
700 | 	if (node >= samplesCount)
701 | 		return;
702 | 	if (node == 0) // undo setupArray init
703 | 		wavefront[node] = false;
704 | 
705 | 	float sample[DIM];
706 | 	for (int d = 0; d < DIM; ++d) 
707 | 		sample[d] = samples[node*DIM+d];
708 | 
709 | 	// check cost is below r
710 | 	float topt = toptBisection(initial, sample, 20);
711 | 	float copt = cost(topt, initial, sample);
712 | 	if (copt > r*2)
713 | 		return;
714 | 
715 | 	// if it is, check path is collision free
716 | 	float splitPath[DIM*(numDiscInit+1)];
717 | 	findDiscretizedPath(splitPath, initial, sample, numDiscInit);
718 | 
719 | 	float v[DIM], w[DIM];
720 | 	float bbMin[DIM], bbMax[DIM];
721 | 	bool motionValid = true;
722 | 	for (int i = 0; i < numDiscInit; ++i) {
723 | 		if (!motionValid)
724 | 			break;
725 | 		for (int d = 0; d < DIM; ++d) {
726 | 			v[d] = splitPath[i*DIM + d];
727 | 			w[d] = splitPath[i*DIM + d + DIM];
728 | 
729 | 			if (v[d] > w[d]) {
730 | 				bbMin[d] = w[d];
731 | 				bbMax[d] = v[d];
732 | 			} else {
733 | 				bbMin[d] = v[d];
734 | 				bbMax[d] = w[d];
735 | 			}
736 | 		}
737 | 		motionValid = motionValid && isMotionValid(v, w, bbMin, bbMax, obstaclesCount, obstacles, debugOutput);
738 | 	}
739 | 
740 | 	if (!motionValid)
741 | 		return;
742 | 
743 | 	// add the cost to costs, check if we have a goal connection, and add node to wavefront, update edges
744 | 	costs[node] = copt;
745 | 	wavefront[node] = true;
746 | 	edges[node] = -1; // -2 marks our starting point
747 | 
748 | 	if (node == goal_idx)
749 | 		costGoal[0] = copt;
750 | 
751 | }
752 | 
753 | __global__ 
754 | void buildInGoal(int samplesCount,float *samples, float *goal, float goalD2,
755 | 	bool *inGoal, float *debugOutput)
756 | {
757 | 	int node = blockIdx.x * blockDim.x + threadIdx.x;
758 | 
759 | 	if (node >= samplesCount)
760 | 		return;
761 | 
762 | 	float dist2 = 0;
763 | 	for (int d = 0; d < DIM/2; ++d)
764 | 		dist2 += (samples[node*DIM+d] - goal[d])*(samples[node*DIM+d] - goal[d]);
765 | 
766 | 	if (dist2 < goalD2)
767 | 		inGoal[node] = true;
768 | 	else 
769 | 		inGoal[node] = false;
770 | }


--------------------------------------------------------------------------------
/gpu/gates.txt:
--------------------------------------------------------------------------------
  1 | 0.5,0.05,0.5,0,0,0,
  2 | 0.8,0.8214,0.5,0,0,0,
  3 | 0,0,0,-1,-1,-1,
  4 | 1,1,1,1,1,1,
  5 | 496,
  6 | 0.125, 0.97205, 0.134, -3.1, -3.1, -3.1, 0.1559, 0.98607, 0.3184, 3.1, 3.1, 3.1,
  7 | 0.1041, 0.9579, 0.0124, -3.1, -3.1, -3.1, 0.13015, 0.97219, 0.3084, 3.1, 3.1, 3.1,
  8 | 0.13225, 0.94095, 0.1384, -3.1, -3.1, -3.1, 0.1588, 0.954, 0.3184, 3.1, 3.1, 3.1,
  9 | -0.0294, 0.93481, 0.0108, -3.1, -3.1, -3.1, -0.0005, 0.95031, 0.3156, 3.1, 3.1, 3.1,
 10 | -0.02915, 0.96157, 0.0104, -3.1, -3.1, -3.1, -0.0036, 0.97645, 0.3252, 3.1, 3.1, 3.1,
 11 | 0.0791, 0.92421, 0.1376, -3.1, -3.1, -3.1, 0.10525, 0.93717, 0.3164, 3.1, 3.1, 3.1,
 12 | -0.005, 0.9136, 0.0108, -3.1, -3.1, -3.1, 0.0309, 0.9259, 0.326, 3.1, 3.1, 3.1,
 13 | -0.02955, 0.91357, 0.0108, -3.1, -3.1, -3.1, -0.00075, 0.92676, 0.3224, 3.1, 3.1, 3.1,
 14 | 0.1684, 0.91557, 0.134, -3.1, -3.1, -3.1, 0.19975, 0.92926, 0.3164, 3.1, 3.1, 3.1,
 15 | 0.1989, 0.98898, 0.014, -3.1, -3.1, -3.1, 0.22885, 1.0027, 0.3136, 3.1, 3.1, 3.1,
 16 | -0.0305, 1.0166, -0.0004, -3.1, -3.1, -3.1, 0.2299, 1.0183, 1, 3.1, 3.1, 3.1,
 17 | -0.03145, 0.91317, 0.0104, -3.1, -3.1, -3.1, -0.02795, 1.0173, 1, 3.1, 3.1, 3.1,
 18 | -0.0296, 0.91283, 0.0108, -3.1, -3.1, -3.1, 0.22945, 0.91393, 1, 3.1, 3.1, 3.1,
 19 | 0.23805, 0.12817, -0.0016, -3.1, -3.1, -3.1, 0.23905, 0.14086, 0.8576, 3.1, 3.1, 3.1,
 20 | 0.23145, 0.14036, -0.002, -3.1, -3.1, -3.1, 0.23885, 0.14121, 0.8576, 3.1, 3.1, 3.1,
 21 | 0.69535, 0.54779, 0.012, -3.1, -3.1, -3.1, 0.71865, 0.57338, 0.2596, 3.1, 3.1, 3.1,
 22 | 0.7844, 0.7531, 0.02, -3.1, -3.1, -3.1, 0.79385, 0.77931, 0.8872, 3.1, 3.1, 3.1,
 23 | 0.23755, 0.12586, -0.0084, -3.1, -3.1, -3.1, 0.23905, 0.12852, 0.8564, 3.1, 3.1, 3.1,
 24 | 0.2307, 0.11069, -0.0088, -3.1, -3.1, -3.1, 0.2348, 0.12631, 0.8444, 3.1, 3.1, 3.1,
 25 | 0.44305, -0.002, -0.0184, -3.1, -3.1, -3.1, 0.7045, 0.0007381, 1, 3.1, 3.1, 3.1,
 26 | 0.5772, 0.00054762, -0.0184, -3.1, -3.1, -3.1, 0.5804, 0.12762, 1, 3.1, 3.1, 3.1,
 27 | 0.59025, 0.11629, -0.0184, -3.1, -3.1, -3.1, 0.7034, 0.11779, 0.664, 3.1, 3.1, 3.1,
 28 | 0.589, 0.00016667, -0.0184, -3.1, -3.1, -3.1, 0.59185, 0.11671, 1, 3.1, 3.1, 3.1,
 29 | 0.4418, -0.00042857, -0.0184, -3.1, -3.1, -3.1, 0.4449, 0.12762, 1, 3.1, 3.1, 3.1,
 30 | 0.7018, 0, -0.0184, -3.1, -3.1, -3.1, 0.70475, 0.11748, 1, 3.1, 3.1, 3.1,
 31 | 0.57995, 0.028048, -0.0184, -3.1, -3.1, -3.1, 0.5893, 0.0285, 0.5468, 3.1, 3.1, 3.1,
 32 | 0.5776, 0.55376, 0.0196, -3.1, -3.1, -3.1, 0.60605, 0.56964, 0.3116, 3.1, 3.1, 3.1,
 33 | 0.4065, 0.54821, 0.0128, -3.1, -3.1, -3.1, 0.43995, 0.56314, 0.3244, 3.1, 3.1, 3.1,
 34 | 0.38095, 0.5325, 0.0124, -3.1, -3.1, -3.1, 0.41375, 0.55038, 0.3252, 3.1, 3.1, 3.1,
 35 | 0.6633, 0.54733, 0.022, -3.1, -3.1, -3.1, 0.69545, 0.57433, 0.3024, 3.1, 3.1, 3.1,
 36 | 0.6096, 0.54952, 0.02, -3.1, -3.1, -3.1, 0.66755, 0.57179, 0.2484, 3.1, 3.1, 3.1,
 37 | 0.33745, 0.55424, 0.01, -3.1, -3.1, -3.1, 0.37485, 0.56648, 0.3276, 3.1, 3.1, 3.1,
 38 | 0.4184, 0.49483, 0.0128, -3.1, -3.1, -3.1, 0.46785, 0.5174, 0.342, 3.1, 3.1, 3.1,
 39 | 0.4274, 0.47821, 0.0132, -3.1, -3.1, -3.1, 0.47415, 0.4981, 0.2088, 3.1, 3.1, 3.1,
 40 | 0.4716, 0.47817, 0.0148, -3.1, -3.1, -3.1, 0.5151, 0.4985, 0.3392, 3.1, 3.1, 3.1,
 41 | 0.5138, 0.47812, 0.0164, -3.1, -3.1, -3.1, 0.5599, 0.499, 0.348, 3.1, 3.1, 3.1,
 42 | 0.55815, 0.47807, 0.0184, -3.1, -3.1, -3.1, 0.60165, 0.4986, 0.3436, 3.1, 3.1, 3.1,
 43 | 0.59905, 0.4961, 0.0196, -3.1, -3.1, -3.1, 0.6443, 0.5169, 0.3468, 3.1, 3.1, 3.1,
 44 | 0.60075, 0.47802, 0.0248, -3.1, -3.1, -3.1, 0.64315, 0.49802, 0.2104, 3.1, 3.1, 3.1,
 45 | 0.3401, 0.47631, 0.0076, -3.1, -3.1, -3.1, 0.68775, 0.4776, 1, 3.1, 3.1, 3.1,
 46 | 0.18005, 0.84093, 0.242, -3.1, -3.1, -3.1, 0.2256, 0.85098, 0.9256, 3.1, 3.1, 3.1,
 47 | 0.02055, 0.87336, 0.006, -3.1, -3.1, -3.1, 0.05005, 0.88829, 0.4176, 3.1, 3.1, 3.1,
 48 | 0.07025, 0.87314, 0.0048, -3.1, -3.1, -3.1, 0.10515, 0.88907, 0.3852, 3.1, 3.1, 3.1,
 49 | -0.01035, 0.8511, 0.0092, -3.1, -3.1, -3.1, 0.0172, 0.86933, 0.4172, 3.1, 3.1, 3.1,
 50 | -0.05055, 0.90479, 0.288, -3.1, -3.1, -3.1, -0.03185, 0.90998, 1, 3.1, 3.1, 3.1,
 51 | 0.0743, 0.84093, 0.0048, -3.1, -3.1, -3.1, 0.18295, 0.86186, 0.3496, 3.1, 3.1, 3.1,
 52 | -0.0587, 0.84083, 0.0036, -3.1, -3.1, -3.1, -0.0482, 0.90529, 1, 3.1, 3.1, 3.1,
 53 | -0.03295, 0.90917, 0.0048, -3.1, -3.1, -3.1, 0.22755, 0.9106, 1, 3.1, 3.1, 3.1,
 54 | -0.05075, 0.84029, 0.0048, -3.1, -3.1, -3.1, 0.20355, 0.8419, 1, 3.1, 3.1, 3.1,
 55 | 0.22695, 0.88288, 0.0284, -3.1, -3.1, -3.1, 0.22815, 0.91, 0.874, 3.1, 3.1, 3.1,
 56 | 0.1319, 0.8291, 0.016, -3.1, -3.1, -3.1, 0.18245, 0.837, 0.872, 3.1, 3.1, 3.1,
 57 | 0.0117, 0.80271, 0.016, -3.1, -3.1, -3.1, 0.0454, 0.8189, 0.428, 3.1, 3.1, 3.1,
 58 | 0.0881, 0.78824, 0.016, -3.1, -3.1, -3.1, 0.12135, 0.80393, 0.4848, 3.1, 3.1, 3.1,
 59 | 0.18575, 0.78733, 0.016, -3.1, -3.1, -3.1, 0.21465, 0.80217, 0.4192, 3.1, 3.1, 3.1,
 60 | 0.13995, 0.78693, 0.016, -3.1, -3.1, -3.1, 0.1741, 0.79824, 0.4736, 3.1, 3.1, 3.1,
 61 | 0.026, 0.78224, 0.016, -3.1, -3.1, -3.1, 0.0581, 0.79833, 0.4204, 3.1, 3.1, 3.1,
 62 | -0.05165, 0.76826, -0.0016, -3.1, -3.1, -3.1, -0.0309, 0.77414, 1, 3.1, 3.1, 3.1,
 63 | -0.03085, 0.76781, 0.0016, -3.1, -3.1, -3.1, 0.2276, 0.76867, 1, 3.1, 3.1, 3.1,
 64 | -0.05085, 0.83655, 0.016, -3.1, -3.1, -3.1, 0.18645, 0.83838, 1, 3.1, 3.1, 3.1,
 65 | 0.2265, 0.76869, 0.1356, -3.1, -3.1, -3.1, 0.22765, 0.79488, 0.8732, 3.1, 3.1, 3.1,
 66 | -0.0564, 0.77336, 0.296, -3.1, -3.1, -3.1, -0.04965, 0.83798, 1, 3.1, 3.1, 3.1,
 67 | 0.19535, 0.72736, 0.0064, -3.1, -3.1, -3.1, 0.22735, 0.73793, 0.2884, 3.1, 3.1, 3.1,
 68 | 0.1329, 0.69538, 0.0024, -3.1, -3.1, -3.1, 0.1852, 0.7065, 0.9912, 3.1, 3.1, 3.1,
 69 | 0.01685, 0.7341, 0.0028, -3.1, -3.1, -3.1, 0.0509, 0.75062, 0.3952, 3.1, 3.1, 3.1,
 70 | 0.0832, 0.73893, 0.0016, -3.1, -3.1, -3.1, 0.1172, 0.75343, 0.396, 3.1, 3.1, 3.1,
 71 | 0.14845, 0.7309, 0.0044, -3.1, -3.1, -3.1, 0.1817, 0.74826, 0.422, 3.1, 3.1, 3.1,
 72 | -0.02135, 0.71664, 0.0032, -3.1, -3.1, -3.1, 0.01125, 0.73524, 0.4004, 3.1, 3.1, 3.1,
 73 | -0.05165, 0.75981, 0.4744, -3.1, -3.1, -3.1, -0.0324, 0.76495, 1, 3.1, 3.1, 3.1,
 74 | -0.0542, 0.69555, 0.0036, -3.1, -3.1, -3.1, -0.04705, 0.75993, 1, 3.1, 3.1, 3.1,
 75 | -0.05075, 0.69529, 0.0016, -3.1, -3.1, -3.1, 0.1579, 0.69602, 1, 3.1, 3.1, 3.1,
 76 | -0.0327, 0.76429, 0.01, -3.1, -3.1, -3.1, 0.22705, 0.76517, 1, 3.1, 3.1, 3.1,
 77 | 0.2264, 0.73752, 0.2696, -3.1, -3.1, -3.1, 0.2278, 0.76445, 0.874, 3.1, 3.1, 3.1,
 78 | 0.1337, 0.63995, 0.0112, -3.1, -3.1, -3.1, 0.15395, 0.64398, 0.2748, 3.1, 3.1, 3.1,
 79 | 0.0444, 0.62564, 0.0104, -3.1, -3.1, -3.1, 0.064, 0.64202, 0.2796, 3.1, 3.1, 3.1,
 80 | 0.17855, 0.68338, 0.012, -3.1, -3.1, -3.1, 0.1916, 0.69186, 0.8744, 3.1, 3.1, 3.1,
 81 | 0.04435, 0.67486, 0.01, -3.1, -3.1, -3.1, 0.0784, 0.69195, 0.4136, 3.1, 3.1, 3.1,
 82 | 0.0735, 0.6751, 0.0104, -3.1, -3.1, -3.1, 0.1099, 0.69193, 0.4276, 3.1, 3.1, 3.1,
 83 | -0.01745, 0.66207, 0.0096, -3.1, -3.1, -3.1, 0.01095, 0.6795, 0.3932, 3.1, 3.1, 3.1,
 84 | 0.0092, 0.63524, 0.0096, -3.1, -3.1, -3.1, 0.0388, 0.64531, 0.3772, 3.1, 3.1, 3.1,
 85 | 0.0944, 0.63167, 0.0108, -3.1, -3.1, -3.1, 0.1286, 0.64676, 0.376, 3.1, 3.1, 3.1,
 86 | 0.15045, 0.63581, 0.0116, -3.1, -3.1, -3.1, 0.1806, 0.65086, 0.4504, 3.1, 3.1, 3.1,
 87 | 0.18695, 0.63831, 0.012, -3.1, -3.1, -3.1, 0.21655, 0.64595, 0.3228, 3.1, 3.1, 3.1,
 88 | -0.0521, 0.62279, 0.0372, -3.1, -3.1, -3.1, -0.0321, 0.6285, 1, 3.1, 3.1, 3.1,
 89 | -0.0522, 0.69188, 0.0096, -3.1, -3.1, -3.1, 0.1961, 0.69274, 1, 3.1, 3.1, 3.1,
 90 | -0.0332, 0.62271, 0.014, -3.1, -3.1, -3.1, 0.22585, 0.62362, 1, 3.1, 3.1, 3.1,
 91 | 0.22475, 0.62364, 0.0124, -3.1, -3.1, -3.1, 0.2261, 0.6499, 0.8636, 3.1, 3.1, 3.1,
 92 | -0.0576, 0.62852, 0.0088, -3.1, -3.1, -3.1, -0.04975, 0.69233, 1, 3.1, 3.1, 3.1,
 93 | 0.1762, 0.55121, 0.0168, -3.1, -3.1, -3.1, 0.18135, 0.55917, 0.87, 3.1, 3.1, 3.1,
 94 | -0.00335, 0.58607, 0.012, -3.1, -3.1, -3.1, 0.02945, 0.60107, 0.4792, 3.1, 3.1, 3.1,
 95 | 0.11855, 0.59338, 0.1308, -3.1, -3.1, -3.1, 0.1519, 0.60764, 0.374, 3.1, 3.1, 3.1,
 96 | 0.1415, 0.55121, 0.1524, -3.1, -3.1, -3.1, 0.17125, 0.56336, 0.364, 3.1, 3.1, 3.1,
 97 | 0.1131, 0.55336, 0.1416, -3.1, -3.1, -3.1, 0.14025, 0.5676, 0.366, 3.1, 3.1, 3.1,
 98 | -0.05205, 0.61417, 0.0096, -3.1, -3.1, -3.1, -0.03245, 0.61955, 1, 3.1, 3.1, 3.1,
 99 | -0.0531, 0.54998, 0.014, -3.1, -3.1, -3.1, -0.0508, 0.61443, 1, 3.1, 3.1, 3.1,
100 | 0.2245, 0.59176, 0.0072, -3.1, -3.1, -3.1, 0.22635, 0.6195, 0.8748, 3.1, 3.1, 3.1,
101 | -0.0324, 0.6185, 0.0056, -3.1, -3.1, -3.1, 0.2262, 0.61962, 1, 3.1, 3.1, 3.1,
102 | -0.0515, 0.5491, 0.0056, -3.1, -3.1, -3.1, 0.20125, 0.55062, 1, 3.1, 3.1, 3.1,
103 | 0.0944, 0.53881, 0.0068, -3.1, -3.1, -3.1, 0.18105, 0.54695, 0.2456, 3.1, 3.1, 3.1,
104 | 0.076, 0.48221, 0.008, -3.1, -3.1, -3.1, 0.10225, 0.49631, 0.3316, 3.1, 3.1, 3.1,
105 | 0.1111, 0.48631, 0.0364, -3.1, -3.1, -3.1, 0.1476, 0.50112, 0.3356, 3.1, 3.1, 3.1,
106 | 0.03235, 0.51095, 0.0128, -3.1, -3.1, -3.1, 0.06255, 0.52562, 0.3992, 3.1, 3.1, 3.1,
107 | -0.0534, 0.47762, 0.3836, -3.1, -3.1, -3.1, -0.03365, 0.48336, 1, 3.1, 3.1, 3.1,
108 | -0.03445, 0.47667, 0.008, -3.1, -3.1, -3.1, 0.22515, 0.47981, 1, 3.1, 3.1, 3.1,
109 | -0.06165, 0.4825, 0.3256, -3.1, -3.1, -3.1, -0.0518, 0.5469, 1, 3.1, 3.1, 3.1,
110 | 0.2244, 0.47729, 0.0052, -3.1, -3.1, -3.1, 0.22615, 0.50479, 0.8636, 3.1, 3.1, 3.1,
111 | -0.053, 0.54569, 0.066, -3.1, -3.1, -3.1, 0.20075, 0.54724, 1, 3.1, 3.1, 3.1,
112 | -0.05195, 0.40505, 0.4416, -3.1, -3.1, -3.1, -0.00725, 0.41357, 0.7864, 3.1, 3.1, 3.1,
113 | 0.17535, 0.40452, 0.0048, -3.1, -3.1, -3.1, 0.2003, 0.41493, 0.8836, 3.1, 3.1, 3.1,
114 | 0.05375, 0.43924, 0.01, -3.1, -3.1, -3.1, 0.08215, 0.45181, 0.3004, 3.1, 3.1, 3.1,
115 | 0.0018, 0.43033, 0.0108, -3.1, -3.1, -3.1, 0.04045, 0.4484, 0.3708, 3.1, 3.1, 3.1,
116 | 0.1944, 0.43543, 0.0028, -3.1, -3.1, -3.1, 0.22465, 0.44983, 0.1764, 3.1, 3.1, 3.1,
117 | 0.0728, 0.4106, 0.0084, -3.1, -3.1, -3.1, 0.10365, 0.42376, 0.178, 3.1, 3.1, 3.1,
118 | -0.05295, 0.46888, 0.3664, -3.1, -3.1, -3.1, -0.0339, 0.47421, 1, 3.1, 3.1, 3.1,
119 | 0.1011, 0.45352, 0.0068, -3.1, -3.1, -3.1, 0.2114, 0.47352, 0.328, 3.1, 3.1, 3.1,
120 | -0.0535, 0.4041, -0.0016, -3.1, -3.1, -3.1, 0.20115, 0.4056, 1, 3.1, 3.1, 3.1,
121 | -0.03435, 0.47333, 0.0404, -3.1, -3.1, -3.1, 0.2259, 0.47426, 0.8984, 3.1, 3.1, 3.1,
122 | 0.2249, 0.4465, 0.0128, -3.1, -3.1, -3.1, 0.226, 0.47331, 0.868, 3.1, 3.1, 3.1,
123 | -0.0551, 0.40505, 0.3044, -3.1, -3.1, -3.1, -0.05025, 0.46929, 1, 3.1, 3.1, 3.1,
124 | 0.173, 0.2596, 0.0036, -3.1, -3.1, -3.1, 0.2008, 0.272, 0.914, 3.1, 3.1, 3.1,
125 | 0.1032, 0.3811, 0.0056, -3.1, -3.1, -3.1, 0.12095, 0.38448, 0.2548, 3.1, 3.1, 3.1,
126 | 0.0018, 0.29907, 0.0004, -3.1, -3.1, -3.1, 0.0271, 0.3155, 0.3832, 3.1, 3.1, 3.1,
127 | 0.0284, 0.3416, 0.0044, -3.1, -3.1, -3.1, 0.0553, 0.35864, 0.3984, 3.1, 3.1, 3.1,
128 | -0.01315, 0.34219, 0.0016, -3.1, -3.1, -3.1, 0.01435, 0.35588, 0.4268, 3.1, 3.1, 3.1,
129 | 0.1394, 0.33017, 0.0016, -3.1, -3.1, -3.1, 0.1724, 0.34555, 0.3932, 3.1, 3.1, 3.1,
130 | 0.00465, 0.27379, 0, -3.1, -3.1, -3.1, 0.0342, 0.29093, 0.4016, 3.1, 3.1, 3.1,
131 | 0.08165, 0.27088, 0, -3.1, -3.1, -3.1, 0.11835, 0.2856, 0.416, 3.1, 3.1, 3.1,
132 | 0.04855, 0.30795, 0.0004, -3.1, -3.1, -3.1, 0.08405, 0.32512, 0.3904, 3.1, 3.1, 3.1,
133 | 0.1509, 0.3659, 0.0016, -3.1, -3.1, -3.1, 0.19075, 0.38088, 0.4008, 3.1, 3.1, 3.1,
134 | -0.0521, 0.34645, 0.1284, -3.1, -3.1, -3.1, -0.0271, 0.35943, 0.312, 3.1, 3.1, 3.1,
135 | -0.0286, 0.37507, 0.0012, -3.1, -3.1, -3.1, 0.0056, 0.39267, 0.434, 3.1, 3.1, 3.1,
136 | 0.11265, 0.36726, 0.0028, -3.1, -3.1, -3.1, 0.1474, 0.3841, 0.3936, 3.1, 3.1, 3.1,
137 | 0.0581, 0.36924, 0.0028, -3.1, -3.1, -3.1, 0.09205, 0.38824, 0.4384, 3.1, 3.1, 3.1,
138 | -0.05415, 0.32374, 0.2284, -3.1, -3.1, -3.1, -0.03405, 0.32836, 1, 3.1, 3.1, 3.1,
139 | -0.0536, 0.33202, 0.2232, -3.1, -3.1, -3.1, -0.0336, 0.33724, 1, 3.1, 3.1, 3.1,
140 | -0.0298, 0.3286, 0.3496, -3.1, -3.1, -3.1, -0.02815, 0.33183, 0.8492, 3.1, 3.1, 3.1,
141 | -0.05735, 0.25912, -0.0028, -3.1, -3.1, -3.1, -0.051, 0.32386, 1, 3.1, 3.1, 3.1,
142 | 0.1923, 0.32855, 0.0032, -3.1, -3.1, -3.1, 0.19735, 0.33183, 0.8508, 3.1, 3.1, 3.1,
143 | -0.0536, 0.25817, 0.0036, -3.1, -3.1, -3.1, 0.20025, 0.25988, 1, 3.1, 3.1, 3.1,
144 | 0.2241, 0.30121, 0.0064, -3.1, -3.1, -3.1, 0.22555, 0.32852, 0.868, 3.1, 3.1, 3.1,
145 | -0.0349, 0.32783, 0.3016, -3.1, -3.1, -3.1, 0.22495, 0.32876, 1, 3.1, 3.1, 3.1,
146 | -0.05365, 0.40086, 0.0052, -3.1, -3.1, -3.1, 0.2245, 0.40179, 1, 3.1, 3.1, 3.1,
147 | -0.05445, 0.33698, -0.0028, -3.1, -3.1, -3.1, -0.05175, 0.4011, 1, 3.1, 3.1, 3.1,
148 | -0.0356, 0.33164, 0.0028, -3.1, -3.1, -3.1, 0.22435, 0.33262, 1, 3.1, 3.1, 3.1,
149 | 0.13105, 0.23655, 0.0048, -3.1, -3.1, -3.1, 0.1551, 0.25514, 0.2516, 3.1, 3.1, 3.1,
150 | 0.0189, 0.23762, 0.0028, -3.1, -3.1, -3.1, 0.03855, 0.25143, 0.2404, 3.1, 3.1, 3.1,
151 | 0.1303, 0.13374, 0.0112, -3.1, -3.1, -3.1, 0.1524, 0.14583, 0.2672, 3.1, 3.1, 3.1,
152 | 0.1074, 0.13226, 0.012, -3.1, -3.1, -3.1, 0.1313, 0.134, 0.2468, 3.1, 3.1, 3.1,
153 | -0.0527, 0.15455, 0.0016, -3.1, -3.1, -3.1, -0.02915, 0.17655, 0.294, 3.1, 3.1, 3.1,
154 | -0.0358, 0.13995, -0.0056, -3.1, -3.1, -3.1, 5e-05, 0.16105, 0.4152, 3.1, 3.1, 3.1,
155 | 0.1909, 0.13076, 0.0184, -3.1, -3.1, -3.1, 0.22265, 0.14433, 0.4464, 3.1, 3.1, 3.1,
156 | 0.15745, 0.19079, -0.0016, -3.1, -3.1, -3.1, 0.19305, 0.20702, 0.3976, 3.1, 3.1, 3.1,
157 | -0.0318, 0.2274, -0.0056, -3.1, -3.1, -3.1, 0.0145, 0.24405, 0.3664, 3.1, 3.1, 3.1,
158 | 0.0063, 0.13364, -0.0048, -3.1, -3.1, -3.1, 0.0369, 0.14619, 0.362, 3.1, 3.1, 3.1,
159 | 0.0359, 0.12883, -0.0044, -3.1, -3.1, -3.1, 0.064, 0.14669, 0.3916, 3.1, 3.1, 3.1,
160 | 0.07375, 0.12962, -0.0036, -3.1, -3.1, -3.1, 0.10565, 0.14483, 0.3888, 3.1, 3.1, 3.1,
161 | -0.0124, 0.19143, 0.0024, -3.1, -3.1, -3.1, 0.01835, 0.20602, 0.3304, 3.1, 3.1, 3.1,
162 | 0.0553, 0.19517, 0.0036, -3.1, -3.1, -3.1, 0.0922, 0.20655, 0.362, 3.1, 3.1, 3.1,
163 | 0.073, 0.2341, 0.004, -3.1, -3.1, -3.1, 0.115, 0.24874, 0.3008, 3.1, 3.1, 3.1,
164 | -0.0534, 0.1865, 0.1, -3.1, -3.1, -3.1, -0.03445, 0.19207, 1, 3.1, 3.1, 3.1,
165 | -0.0539, 0.17733, 0.2764, -3.1, -3.1, -3.1, -0.03305, 0.18295, 1, 3.1, 3.1, 3.1,
166 | -0.0538, 0.25395, -0.006, -3.1, -3.1, -3.1, 0.2242, 0.25707, 1, 3.1, 3.1, 3.1,
167 | -0.0347, 0.1824, 0.2752, -3.1, -3.1, -3.1, -0.0299, 0.18302, 0.888, 3.1, 3.1, 3.1,
168 | -0.03505, 0.18614, 0.2552, -3.1, -3.1, -3.1, 0.22265, 0.1885, 1, 3.1, 3.1, 3.1,
169 | -0.03005, 0.1825, 0.2988, -3.1, -3.1, -3.1, -0.02815, 0.18629, 0.888, 3.1, 3.1, 3.1,
170 | -0.03345, 0.17819, 0.8868, -3.1, -3.1, -3.1, 0.19945, 0.17955, 0.998, 3.1, 3.1, 3.1,
171 | -0.0551, 0.11379, 0.0016, -3.1, -3.1, -3.1, -0.0419, 0.17764, 1, 3.1, 3.1, 3.1,
172 | -0.05415, 0.19171, 0.0016, -3.1, -3.1, -3.1, -0.05175, 0.2556, 1, 3.1, 3.1, 3.1,
173 | -0.0527, 0.11324, 0.0016, -3.1, -3.1, -3.1, 0.22285, 0.11531, 1, 3.1, 3.1, 3.1,
174 | 0.19325, 0.18171, 0.14, -3.1, -3.1, -3.1, 0.22225, 0.1835, 0.8892, 3.1, 3.1, 3.1,
175 | 0.19315, 0.1834, 0.0064, -3.1, -3.1, -3.1, 0.1938, 0.18631, 0.838, 3.1, 3.1, 3.1,
176 | -0.029, 0.18183, 0.842, -3.1, -3.1, -3.1, 0.19935, 0.1835, 0.888, 3.1, 3.1, 3.1,
177 | -0.05205, 0.024071, -0.0068, -3.1, -3.1, -3.1, -0.01405, 0.033548, 0.2464, 3.1, 3.1, 3.1,
178 | -0.02415, 0.099548, -0.0068, -3.1, -3.1, -3.1, -0.0152, 0.10988, 0.2244, 3.1, 3.1, 3.1,
179 | 0.096, 0.008381, 0.0232, -3.1, -3.1, -3.1, 0.1159, 0.014071, 0.2596, 3.1, 3.1, 3.1,
180 | -0.0306, -0.0019524, -0.0076, -3.1, -3.1, -3.1, -0.003, 0.016, 0.2616, 3.1, 3.1, 3.1,
181 | 0.09745, 0.05569, -0.014, -3.1, -3.1, -3.1, 0.11865, 0.079143, 0.8572, 3.1, 3.1, 3.1,
182 | -0.01685, 0.041, -0.0096, -3.1, -3.1, -3.1, 0.0133, 0.05581, 0.392, 3.1, 3.1, 3.1,
183 | -0.0385, 0.078381, -0.0072, -3.1, -3.1, -3.1, -0.0046, 0.0915, 0.356, 3.1, 3.1, 3.1,
184 | 0.0148, 0.011048, -0.012, -3.1, -3.1, -3.1, 0.046, 0.028619, 0.3268, 3.1, 3.1, 3.1,
185 | 0.05535, 0.012833, -0.012, -3.1, -3.1, -3.1, 0.09165, 0.026381, 0.4172, 3.1, 3.1, 3.1,
186 | 0.0659, 0.066667, -0.0124, -3.1, -3.1, -3.1, 0.0994, 0.08431, 0.312, 3.1, 3.1, 3.1,
187 | -0.05365, -0.0079762, 0.2512, -3.1, -3.1, -3.1, -0.03385, 0.00047619, 1, 3.1, 3.1, 3.1,
188 | -0.03535, -0.0091667, -0.012, -3.1, -3.1, -3.1, 0.11895, -0.0079524, 1, 3.1, 3.1, 3.1,
189 | -0.05365, 0.10948, -0.012, -3.1, -3.1, -3.1, 0.1119, 0.11114, 1, 3.1, 3.1, 3.1,
190 | -0.05585, -2.381e-05, 0.096, -3.1, -3.1, -3.1, -0.05185, 0.11055, 1, 3.1, 3.1, 3.1,
191 | 0.1158, -0.0082381, -0.0132, -3.1, -3.1, -3.1, 0.1194, 0.087429, 1, 3.1, 3.1, 3.1,
192 | 0.12637, 0.008631, 0.2212, -3.1, -3.1, -3.1, 0.14642, 0.032536, 0.9832, 3.1, 3.1, 3.1,
193 | 0.14183, 0.033321, -0.002, -3.1, -3.1, -3.1, 0.16448, 0.042964, 0.2524, 3.1, 3.1, 3.1,
194 | 0.20027, 0.0085595, -0.0056, -3.1, -3.1, -3.1, 0.21937, 0.010821, 0.2556, 3.1, 3.1, 3.1,
195 | 0.20008, 0.032131, -0.006, -3.1, -3.1, -3.1, 0.23072, 0.04756, 0.3664, 3.1, 3.1, 3.1,
196 | 0.15433, 0.051964, -0.0044, -3.1, -3.1, -3.1, 0.18743, 0.067607, 0.3896, 3.1, 3.1, 3.1,
197 | 0.22358, 0.0084643, -0.004, -3.1, -3.1, -3.1, 0.26078, 0.024607, 0.3752, 3.1, 3.1, 3.1,
198 | 0.12507, -0.0097738, -0.0092, -3.1, -3.1, -3.1, 0.12782, 0.076036, 1, 3.1, 3.1, 3.1,
199 | 0.12602, 0.075488, -0.0056, -3.1, -3.1, -3.1, 0.21532, 0.076917, 0.8656, 3.1, 3.1, 3.1,
200 | 0.12788, -0.011845, 0.0244, -3.1, -3.1, -3.1, 0.29008, -0.00875, 1, 3.1, 3.1, 3.1,
201 | 0.3207, 0.048548, 0, -3.1, -3.1, -3.1, 0.33885, 0.072524, 0.8752, 3.1, 3.1, 3.1,
202 | 0.3815, 0.10669, -0.0032, -3.1, -3.1, -3.1, 0.42635, 0.11576, 0.8632, 3.1, 3.1, 3.1,
203 | 0.33685, 0.10657, -0.0012, -3.1, -3.1, -3.1, 0.38145, 0.1161, 0.858, 3.1, 3.1, 3.1,
204 | 0.39895, 0.041262, 0.1412, -3.1, -3.1, -3.1, 0.42625, 0.05381, 0.3048, 3.1, 3.1, 3.1,
205 | 0.37265, 0.0094286, -0.002, -3.1, -3.1, -3.1, 0.4081, 0.027143, 0.3876, 3.1, 3.1, 3.1,
206 | 0.2971, 0.071143, 0.0008, -3.1, -3.1, -3.1, 0.3207, 0.080905, 0.8976, 3.1, 3.1, 3.1,
207 | 0.3195, -0.013524, 0.1416, -3.1, -3.1, -3.1, 0.32335, 0.061071, 0.9076, 3.1, 3.1, 3.1,
208 | 0.42315, -0.012762, -0.0032, -3.1, -3.1, -3.1, 0.4281, 0.11443, 1, 3.1, 3.1, 3.1,
209 | 0.32355, -0.0094048, 0.9948, -3.1, -3.1, -3.1, 0.4253, -0.007119, 1, 3.1, 3.1, 3.1,
210 | 0.2969, 0.080381, 0.0016, -3.1, -3.1, -3.1, 0.2984, 0.090071, 0.8652, 3.1, 3.1, 3.1,
211 | 0.32185, 0.11552, 0.998, -3.1, -3.1, -3.1, 0.4254, 0.1164, 1, 3.1, 3.1, 3.1,
212 | 0.42565, -0.00080952, 0.9988, -3.1, -3.1, -3.1, 0.4284, 0.11426, 1, 3.1, 3.1, 3.1,
213 | 0.37605, 0.28676, 0.0056, -3.1, -3.1, -3.1, 0.4145, 0.30293, 0.4952, 3.1, 3.1, 3.1,
214 | 0.33, 0.3226, 0.2972, -3.1, -3.1, -3.1, 0.50935, 0.32317, 0.8664, 3.1, 3.1, 3.1,
215 | 0.50855, 0.25833, 0.0056, -3.1, -3.1, -3.1, 0.51085, 0.32281, 1, 3.1, 3.1, 3.1,
216 | 0.33825, 0.25824, 0.0056, -3.1, -3.1, -3.1, 0.5085, 0.26017, 1, 3.1, 3.1, 3.1,
217 | 0.32935, 0.29462, 0.0056, -3.1, -3.1, -3.1, 0.3306, 0.32257, 0.8648, 3.1, 3.1, 3.1,
218 | 0.59925, 0.25852, 0.1348, -3.1, -3.1, -3.1, 0.62775, 0.2716, 0.3244, 3.1, 3.1, 3.1,
219 | 0.55505, 0.25876, 0.006, -3.1, -3.1, -3.1, 0.58805, 0.27683, 0.4108, 3.1, 3.1, 3.1,
220 | 0.5529, 0.28679, 0.0048, -3.1, -3.1, -3.1, 0.58395, 0.29574, 0.372, 3.1, 3.1, 3.1,
221 | 0.6184, 0.28317, 0.0048, -3.1, -3.1, -3.1, 0.6516, 0.30002, 0.4136, 3.1, 3.1, 3.1,
222 | 0.51505, 0.32181, 0.0008, -3.1, -3.1, -3.1, 0.69445, 0.32302, 0.8708, 3.1, 3.1, 3.1,
223 | 0.5149, 0.25719, -0.004, -3.1, -3.1, -3.1, 0.68545, 0.25976, 1, 3.1, 3.1, 3.1,
224 | 0.6923, 0.29419, 0.0212, -3.1, -3.1, -3.1, 0.6949, 0.32245, 0.8616, 3.1, 3.1, 3.1,
225 | 0.5142, 0.25843, 0.2832, -3.1, -3.1, -3.1, 0.5169, 0.32226, 1, 3.1, 3.1, 3.1,
226 | 0.34165, 0.46152, 0.004, -3.1, -3.1, -3.1, 0.3879, 0.4729, 1, 3.1, 3.1, 3.1,
227 | 0.43105, 0.43457, 0.0092, -3.1, -3.1, -3.1, 0.45535, 0.44431, 0.408, 3.1, 3.1, 3.1,
228 | 0.3639, 0.42583, 0.0028, -3.1, -3.1, -3.1, 0.4079, 0.44319, 0.4484, 3.1, 3.1, 3.1,
229 | 0.46425, 0.45114, 0.0128, -3.1, -3.1, -3.1, 0.48555, 0.4656, 0.4208, 3.1, 3.1, 3.1,
230 | 0.3775, 0.4724, 0.0048, -3.1, -3.1, -3.1, 0.5098, 0.47414, 1, 3.1, 3.1, 3.1,
231 | 0.50575, 0.40731, 0.0152, -3.1, -3.1, -3.1, 0.51145, 0.47331, 1, 3.1, 3.1, 3.1,
232 | 0.3309, 0.40712, 0.21, -3.1, -3.1, -3.1, 0.50855, 0.40793, 1, 3.1, 3.1, 3.1,
233 | 0.33045, 0.4079, 0.0068, -3.1, -3.1, -3.1, 0.33135, 0.42724, 0.8604, 3.1, 3.1, 3.1,
234 | 0.4505, 0.35079, 0.0072, -3.1, -3.1, -3.1, 0.47535, 0.36593, 0.3864, 3.1, 3.1, 3.1,
235 | 0.4314, 0.38757, 0.0072, -3.1, -3.1, -3.1, 0.46085, 0.40371, 0.4276, 3.1, 3.1, 3.1,
236 | 0.4496, 0.37674, 0.0068, -3.1, -3.1, -3.1, 0.4829, 0.39317, 0.4096, 3.1, 3.1, 3.1,
237 | 0.35695, 0.37576, 0.0088, -3.1, -3.1, -3.1, 0.38695, 0.39129, 0.3816, 3.1, 3.1, 3.1,
238 | 0.33055, 0.40352, -0.0032, -3.1, -3.1, -3.1, 0.50995, 0.40483, 1, 3.1, 3.1, 3.1,
239 | 0.5086, 0.3369, 0.0172, -3.1, -3.1, -3.1, 0.5106, 0.40386, 1, 3.1, 3.1, 3.1,
240 | 0.33905, 0.33657, 0.0012, -3.1, -3.1, -3.1, 0.50855, 0.33814, 1, 3.1, 3.1, 3.1,
241 | 0.3302, 0.3784, 0.1396, -3.1, -3.1, -3.1, 0.33165, 0.4035, 0.8716, 3.1, 3.1, 3.1,
242 | 0.64625, 0.39538, 0.004, -3.1, -3.1, -3.1, 0.69425, 0.40388, 0.898, 3.1, 3.1, 3.1,
243 | 0.60185, 0.33726, 0.0056, -3.1, -3.1, -3.1, 0.6551, 0.3469, 0.9976, 3.1, 3.1, 3.1,
244 | 0.56185, 0.34295, 0.008, -3.1, -3.1, -3.1, 0.5929, 0.36021, 0.3312, 3.1, 3.1, 3.1,
245 | 0.5524, 0.36074, 0.0084, -3.1, -3.1, -3.1, 0.58815, 0.37655, 0.3852, 3.1, 3.1, 3.1,
246 | 0.5157, 0.40343, 0.0088, -3.1, -3.1, -3.1, 0.68555, 0.40462, 1, 3.1, 3.1, 3.1,
247 | 0.51545, 0.33671, 0.0112, -3.1, -3.1, -3.1, 0.51755, 0.40362, 1, 3.1, 3.1, 3.1,
248 | 0.5176, 0.33655, 0.0012, -3.1, -3.1, -3.1, 0.67925, 0.33774, 1, 3.1, 3.1, 3.1,
249 | 0.6927, 0.37736, 0.0052, -3.1, -3.1, -3.1, 0.6947, 0.39619, 0.874, 3.1, 3.1, 3.1,
250 | 0.57395, 0.43626, 0.0068, -3.1, -3.1, -3.1, 0.6033, 0.45133, 0.412, 3.1, 3.1, 3.1,
251 | 0.5516, 0.40786, 0.0064, -3.1, -3.1, -3.1, 0.5894, 0.42317, 0.4104, 3.1, 3.1, 3.1,
252 | 0.51565, 0.47293, 0.0084, -3.1, -3.1, -3.1, 0.68635, 0.47376, 1, 3.1, 3.1, 3.1,
253 | 0.51535, 0.40733, 0.0352, -3.1, -3.1, -3.1, 0.5171, 0.47333, 1, 3.1, 3.1, 3.1,
254 | 0.5166, 0.40717, 0.0064, -3.1, -3.1, -3.1, 0.69455, 0.40826, 1, 3.1, 3.1, 3.1,
255 | 0.68975, 0.40781, 0.0092, -3.1, -3.1, -3.1, 0.6949, 0.42707, 0.8704, 3.1, 3.1, 3.1,
256 | 0.38755, 0.76774, 0.008, -3.1, -3.1, -3.1, 0.43525, 0.77564, 1, 3.1, 3.1, 3.1,
257 | 0.34275, 0.76705, 0.008, -3.1, -3.1, -3.1, 0.3886, 0.77726, 1, 3.1, 3.1, 3.1,
258 | 0.4275, 0.79812, 0.008, -3.1, -3.1, -3.1, 0.4547, 0.80952, 0.4244, 3.1, 3.1, 3.1,
259 | 0.3426, 0.81979, 0.008, -3.1, -3.1, -3.1, 0.38185, 0.83355, 0.4064, 3.1, 3.1, 3.1,
260 | 0.333, 0.83257, 0.0296, -3.1, -3.1, -3.1, 0.5112, 0.83402, 1, 3.1, 3.1, 3.1,
261 | 0.51015, 0.76755, 0.006, -3.1, -3.1, -3.1, 0.512, 0.83352, 1, 3.1, 3.1, 3.1,
262 | 0.42745, 0.76721, 0.008, -3.1, -3.1, -3.1, 0.511, 0.76819, 1, 3.1, 3.1, 3.1,
263 | 0.3327, 0.8141, 0.176, -3.1, -3.1, -3.1, 0.33415, 0.83357, 0.8736, 3.1, 3.1, 3.1,
264 | 0.6418, 0.77052, 0.0216, -3.1, -3.1, -3.1, 0.6872, 0.77848, 0.9844, 3.1, 3.1, 3.1,
265 | 0.59425, 0.7679, 0.0204, -3.1, -3.1, -3.1, 0.64175, 0.77602, 1, 3.1, 3.1, 3.1,
266 | 0.582, 0.79164, 0.02, -3.1, -3.1, -3.1, 0.6108, 0.80576, 0.372, 3.1, 3.1, 3.1,
267 | 0.60455, 0.80895, 0.0204, -3.1, -3.1, -3.1, 0.63765, 0.82581, 0.4024, 3.1, 3.1, 3.1,
268 | 0.51565, 0.76714, 0.0188, -3.1, -3.1, -3.1, 0.51815, 0.83324, 1, 3.1, 3.1, 3.1,
269 | 0.5169, 0.76707, 0.0196, -3.1, -3.1, -3.1, 0.59645, 0.76895, 1, 3.1, 3.1, 3.1,
270 | 0.5162, 0.83257, 0.0208, -3.1, -3.1, -3.1, 0.69565, 0.83445, 1, 3.1, 3.1, 3.1,
271 | 0.6947, 0.81445, 0.2336, -3.1, -3.1, -3.1, 0.69595, 0.8336, 0.8792, 3.1, 3.1, 3.1,
272 | 0.64565, 0.89455, 0.0156, -3.1, -3.1, -3.1, 0.6527, 0.9036, 0.8836, 3.1, 3.1, 3.1,
273 | 0.51805, 0.83669, 0.0076, -3.1, -3.1, -3.1, 0.5655, 0.84533, 0.9928, 3.1, 3.1, 3.1,
274 | 0.5309, 0.88945, 0.0092, -3.1, -3.1, -3.1, 0.55845, 0.90274, 0.3184, 3.1, 3.1, 3.1,
275 | 0.52055, 0.8701, 0.0076, -3.1, -3.1, -3.1, 0.5572, 0.88769, 0.3824, 3.1, 3.1, 3.1,
276 | 0.63725, 0.83948, 0.1484, -3.1, -3.1, -3.1, 0.66255, 0.85455, 0.358, 3.1, 3.1, 3.1,
277 | 0.51595, 0.83745, 0.0072, -3.1, -3.1, -3.1, 0.52115, 0.90233, 1, 3.1, 3.1, 3.1,
278 | 0.5164, 0.90324, 0.0148, -3.1, -3.1, -3.1, 0.6868, 0.90462, 1, 3.1, 3.1, 3.1,
279 | 0.5455, 0.83614, 0.0076, -3.1, -3.1, -3.1, 0.69585, 0.8376, 1, 3.1, 3.1, 3.1,
280 | 0.69335, 0.8375, 0.0224, -3.1, -3.1, -3.1, 0.69635, 0.86305, 0.886, 3.1, 3.1, 3.1,
281 | 0.36795, 0.89231, 0.0164, -3.1, -3.1, -3.1, 0.4181, 0.9035, 0.5604, 3.1, 3.1, 3.1,
282 | 0.334, 0.83705, 0.3076, -3.1, -3.1, -3.1, 0.38215, 0.85117, 0.8816, 3.1, 3.1, 3.1,
283 | 0.4169, 0.85624, 0.1976, -3.1, -3.1, -3.1, 0.44725, 0.87226, 0.3756, 3.1, 3.1, 3.1,
284 | 0.33475, 0.90376, 0.1364, -3.1, -3.1, -3.1, 0.51105, 0.90469, 1, 3.1, 3.1, 3.1,
285 | 0.3429, 0.83686, 0.3076, -3.1, -3.1, -3.1, 0.5124, 0.83795, 1, 3.1, 3.1, 3.1,
286 | 0.51, 0.83695, 0.2572, -3.1, -3.1, -3.1, 0.5125, 0.90455, 1, 3.1, 3.1, 3.1,
287 | 0.33315, 0.84438, 0.0152, -3.1, -3.1, -3.1, 0.33655, 0.8635, 0.8756, 3.1, 3.1, 3.1,
288 | 0.8417, 0.98688, -0.008, -3.1, -3.1, -3.1, 0.848, 0.99393, 0.8608, 3.1, 3.1, 3.1,
289 | 1.0373, 0.98771, -0.008, -3.1, -3.1, -3.1, 1.0422, 0.9989, 0.2584, 3.1, 3.1, 3.1,
290 | 0.87135, 1.0363, -0.008, -3.1, -3.1, -3.1, 0.893, 1.0447, 0.2564, 3.1, 3.1, 3.1,
291 | 0.9366, 0.9859, -0.008, -3.1, -3.1, -3.1, 0.98735, 0.99436, 0.9808, 3.1, 3.1, 3.1,
292 | 0.9255, 1.0137, -0.008, -3.1, -3.1, -3.1, 0.96, 1.03, 0.3848, 3.1, 3.1, 3.1,
293 | 0.99735, 1.0076, -0.008, -3.1, -3.1, -3.1, 1.0305, 1.0245, 0.3876, 3.1, 3.1, 3.1,
294 | 0.8285, 1.0252, -0.008, -3.1, -3.1, -3.1, 0.86065, 1.0418, 0.3932, 3.1, 3.1, 3.1,
295 | 0.80085, 1.0538, -0.008, -3.1, -3.1, -3.1, 1.0801, 1.0551, 1, 3.1, 3.1, 3.1,
296 | 1.0764, 0.98567, 0.2796, -3.1, -3.1, -3.1, 1.0816, 1.0542, 1, 3.1, 3.1, 3.1,
297 | 0.8242, 0.98555, -0.008, -3.1, -3.1, -3.1, 1.0796, 0.98731, 1, 3.1, 3.1, 3.1,
298 | 0.80025, 1.0281, 0.1464, -3.1, -3.1, -3.1, 0.80145, 1.0537, 0.8524, 3.1, 3.1, 3.1,
299 | 0.9775, 0.97398, 0.0024, -3.1, -3.1, -3.1, 1.0277, 0.98305, 0.854, 3.1, 3.1, 3.1,
300 | 0.80325, 0.97052, -0.0028, -3.1, -3.1, -3.1, 0.8481, 0.98221, 0.8788, 3.1, 3.1, 3.1,
301 | 0.96465, 0.91429, 0.0016, -3.1, -3.1, -3.1, 0.98425, 0.93226, 0.2544, 3.1, 3.1, 3.1,
302 | 1.0006, 0.94833, 0.0028, -3.1, -3.1, -3.1, 1.0365, 0.9656, 0.3956, 3.1, 3.1, 3.1,
303 | 0.8545, 0.93083, -0.0024, -3.1, -3.1, -3.1, 0.88565, 0.94833, 0.3552, 3.1, 3.1, 3.1,
304 | 0.91865, 0.93198, -0.0004, -3.1, -3.1, -3.1, 0.953, 0.94812, 0.4012, 3.1, 3.1, 3.1,
305 | 0.8482, 0.96931, -0.002, -3.1, -3.1, -3.1, 0.8771, 0.98224, 0.3092, 3.1, 3.1, 3.1,
306 | 1.0607, 0.91338, 0.296, -3.1, -3.1, -3.1, 1.0805, 0.91898, 1, 3.1, 3.1, 3.1,
307 | 0.8253, 0.98221, -0.0004, -3.1, -3.1, -3.1, 1.0806, 0.98357, 1, 3.1, 3.1, 3.1,
308 | 1.0789, 0.91845, 0.2996, -3.1, -3.1, -3.1, 1.0825, 0.98264, 1, 3.1, 3.1, 3.1,
309 | 0.80295, 0.9124, -0.0032, -3.1, -3.1, -3.1, 1.0608, 0.91436, 1, 3.1, 3.1, 3.1,
310 | 0.80105, 0.91307, 0.108, -3.1, -3.1, -3.1, 0.8039, 0.94045, 0.864, 3.1, 3.1, 3.1,
311 | 0.98745, 0.89024, 0.0232, -3.1, -3.1, -3.1, 1.0074, 0.89102, 0.2604, 3.1, 3.1, 3.1,
312 | 0.84575, 0.84088, 0.2816, -3.1, -3.1, -3.1, 0.8492, 0.84836, 0.8852, 3.1, 3.1, 3.1,
313 | 0.807, 0.89036, 0.0108, -3.1, -3.1, -3.1, 0.82475, 0.9089, 0.2644, 3.1, 3.1, 3.1,
314 | 0.93775, 0.84033, 0.0196, -3.1, -3.1, -3.1, 0.98865, 0.84838, 1, 3.1, 3.1, 3.1,
315 | 1.001, 0.84076, 0.0256, -3.1, -3.1, -3.1, 1.0301, 0.8526, 0.328, 3.1, 3.1, 3.1,
316 | 1.0073, 0.85976, 0.0248, -3.1, -3.1, -3.1, 1.0404, 0.87395, 0.4064, 3.1, 3.1, 3.1,
317 | 0.92955, 0.88198, 0.0192, -3.1, -3.1, -3.1, 0.9663, 0.89933, 0.3616, 3.1, 3.1, 3.1,
318 | 1.0608, 0.90362, 0.3552, -3.1, -3.1, -3.1, 1.0816, 0.90931, 1, 3.1, 3.1, 3.1,
319 | 0.8264, 0.83983, 0.0256, -3.1, -3.1, -3.1, 1.0814, 0.84071, 1, 3.1, 3.1, 3.1,
320 | 0.8022, 0.90898, 0.0116, -3.1, -3.1, -3.1, 1.0632, 0.91002, 1, 3.1, 3.1, 3.1,
321 | 1.0788, 0.84043, 0.3268, -3.1, -3.1, -3.1, 1.0833, 0.90467, 1, 3.1, 3.1, 3.1,
322 | 0.80165, 0.88221, 0.0868, -3.1, -3.1, -3.1, 0.80325, 0.90955, 0.8784, 3.1, 3.1, 3.1,
323 | 0.93855, 0.5496, 0.0096, -3.1, -3.1, -3.1, 0.9869, 0.55945, 0.9996, 3.1, 3.1, 3.1,
324 | 0.8339, 0.55005, 0.0984, -3.1, -3.1, -3.1, 0.8425, 0.5601, 0.8676, 3.1, 3.1, 3.1,
325 | 0.9247, 0.57295, 0.0112, -3.1, -3.1, -3.1, 0.9544, 0.58788, 0.4028, 3.1, 3.1, 3.1,
326 | 0.955, 0.58362, 0.01, -3.1, -3.1, -3.1, 0.98415, 0.59924, 0.4028, 3.1, 3.1, 3.1,
327 | 0.8682, 0.56033, 0.0132, -3.1, -3.1, -3.1, 0.89945, 0.57402, 0.3676, 3.1, 3.1, 3.1,
328 | 0.83105, 0.58419, 0.0144, -3.1, -3.1, -3.1, 0.86675, 0.59924, 0.4004, 3.1, 3.1, 3.1,
329 | 0.986, 0.5911, 0.0084, -3.1, -3.1, -3.1, 1.0191, 0.6064, 0.406, 3.1, 3.1, 3.1,
330 | 1.0597, 0.61386, 0.298, -3.1, -3.1, -3.1, 1.0796, 0.61924, 1, 3.1, 3.1, 3.1,
331 | 0.8011, 0.59176, 0.1436, -3.1, -3.1, -3.1, 0.80235, 0.61917, 0.8736, 3.1, 3.1, 3.1,
332 | 1.0788, 0.54971, 0.0064, -3.1, -3.1, -3.1, 1.082, 0.6145, 1, 3.1, 3.1, 3.1,
333 | 0.82565, 0.5495, 0.0448, -3.1, -3.1, -3.1, 1.0799, 0.5501, 1, 3.1, 3.1, 3.1,
334 | 0.80125, 0.6184, 0.0076, -3.1, -3.1, -3.1, 1.0613, 0.61993, 1, 3.1, 3.1, 3.1,
335 | 0.8481, 0.68255, 0.0084, -3.1, -3.1, -3.1, 0.89625, 0.69167, 0.8648, 3.1, 3.1, 3.1,
336 | 0.8262, 0.68207, 0.0088, -3.1, -3.1, -3.1, 0.84805, 0.68769, 0.8556, 3.1, 3.1, 3.1,
337 | 0.8951, 0.67955, 0.0068, -3.1, -3.1, -3.1, 0.9424, 0.691, 0.5468, 3.1, 3.1, 3.1,
338 | 0.96475, 0.67643, 0.0052, -3.1, -3.1, -3.1, 0.99715, 0.69119, 0.3824, 3.1, 3.1, 3.1,
339 | 1.0056, 0.64667, 0.1536, -3.1, -3.1, -3.1, 1.0346, 0.65967, 0.362, 3.1, 3.1, 3.1,
340 | 0.86195, 0.6681, 0.008, -3.1, -3.1, -3.1, 0.8928, 0.68395, 0.436, 3.1, 3.1, 3.1,
341 | 0.8861, 0.63702, 0.124, -3.1, -3.1, -3.1, 0.9146, 0.64919, 0.3608, 3.1, 3.1, 3.1,
342 | 1.0602, 0.62338, 0.2972, -3.1, -3.1, -3.1, 1.0804, 0.62821, 1, 3.1, 3.1, 3.1,
343 | 0.8019, 0.62233, 0.0076, -3.1, -3.1, -3.1, 1.0619, 0.62305, 1, 3.1, 3.1, 3.1,
344 | 1.0783, 0.62824, 0.0032, -3.1, -3.1, -3.1, 1.0864, 0.69183, 1, 3.1, 3.1, 3.1,
345 | 0.8273, 0.69055, 0.0064, -3.1, -3.1, -3.1, 1.0799, 0.69214, 1, 3.1, 3.1, 3.1,
346 | 0.80125, 0.62305, 0.306, -3.1, -3.1, -3.1, 0.8025, 0.6494, 0.8724, 3.1, 3.1, 3.1,
347 | 0.80255, 0.74412, 0.0152, -3.1, -3.1, -3.1, 0.82625, 0.76362, 0.5584, 3.1, 3.1, 3.1,
348 | 0.89355, 0.69514, 0.0152, -3.1, -3.1, -3.1, 0.9398, 0.70402, 0.99, 3.1, 3.1, 3.1,
349 | 0.8216, 0.6996, 0.0152, -3.1, -3.1, -3.1, 0.84715, 0.70755, 0.9544, 3.1, 3.1, 3.1,
350 | 0.8472, 0.69526, 0.0152, -3.1, -3.1, -3.1, 0.8935, 0.7076, 0.99, 3.1, 3.1, 3.1,
351 | 0.93985, 0.69519, 0.0152, -3.1, -3.1, -3.1, 0.9878, 0.70419, 1, 3.1, 3.1, 3.1,
352 | 0.96765, 0.71567, 0.0152, -3.1, -3.1, -3.1, 1.0018, 0.73202, 0.41, 3.1, 3.1, 3.1,
353 | 1.0047, 0.70048, 0.0152, -3.1, -3.1, -3.1, 1.034, 0.71474, 0.4324, 3.1, 3.1, 3.1,
354 | 0.8291, 0.74652, 0.1544, -3.1, -3.1, -3.1, 0.85865, 0.75943, 0.3228, 3.1, 3.1, 3.1,
355 | 0.9522, 0.70167, 0.1504, -3.1, -3.1, -3.1, 0.98205, 0.7156, 0.3184, 3.1, 3.1, 3.1,
356 | 0.916, 0.70271, 0.0156, -3.1, -3.1, -3.1, 0.945, 0.71617, 0.3288, 3.1, 3.1, 3.1,
357 | 0.8617, 0.72814, 0.0652, -3.1, -3.1, -3.1, 0.8919, 0.74071, 0.3196, 3.1, 3.1, 3.1,
358 | 1.0608, 0.75879, 0.2972, -3.1, -3.1, -3.1, 1.0797, 0.76443, 1, 3.1, 3.1, 3.1,
359 | 1.0781, 0.69502, 0.2968, -3.1, -3.1, -3.1, 1.0829, 0.75948, 1, 3.1, 3.1, 3.1,
360 | 0.98495, 0.69464, 0.0152, -3.1, -3.1, -3.1, 1.0802, 0.69557, 1, 3.1, 3.1, 3.1,
361 | 0.8027, 0.76364, 0.0184, -3.1, -3.1, -3.1, 1.0613, 0.76469, 1, 3.1, 3.1, 3.1,
362 | 0.8017, 0.73724, 0.326, -3.1, -3.1, -3.1, 0.8029, 0.76362, 0.8768, 3.1, 3.1, 3.1,
363 | 0.89415, 0.82921, 0.0176, -3.1, -3.1, -3.1, 0.9425, 0.83683, 0.87, 3.1, 3.1, 3.1,
364 | 0.84705, 0.82871, 0.0176, -3.1, -3.1, -3.1, 0.8941, 0.83724, 0.87, 3.1, 3.1, 3.1,
365 | 0.8061, 0.77714, 0.0176, -3.1, -3.1, -3.1, 0.82975, 0.78655, 0.2836, 3.1, 3.1, 3.1,
366 | 1.0405, 0.82538, 0.0176, -3.1, -3.1, -3.1, 1.0674, 0.83464, 0.258, 3.1, 3.1, 3.1,
367 | 0.87495, 0.81036, 0.0176, -3.1, -3.1, -3.1, 0.90725, 0.82688, 0.4348, 3.1, 3.1, 3.1,
368 | 0.9167, 0.79595, 0.0176, -3.1, -3.1, -3.1, 0.9508, 0.81202, 0.392, 3.1, 3.1, 3.1,
369 | 0.99815, 0.81193, 0.0176, -3.1, -3.1, -3.1, 1.0322, 0.82807, 0.3872, 3.1, 3.1, 3.1,
370 | 0.9586, 0.82102, 0.0176, -3.1, -3.1, -3.1, 1.0007, 0.83681, 0.3392, 3.1, 3.1, 3.1,
371 | 1.0613, 0.76833, 0.0452, -3.1, -3.1, -3.1, 1.0821, 0.7736, 1, 3.1, 3.1, 3.1,
372 | 0.82565, 0.835, 0.0176, -3.1, -3.1, -3.1, 1.0804, 0.838, 1, 3.1, 3.1, 3.1,
373 | 0.80075, 0.76695, 0.0176, -3.1, -3.1, -3.1, 1.0645, 0.76831, 1, 3.1, 3.1, 3.1,
374 | 1.0778, 0.7736, 0.0176, -3.1, -3.1, -3.1, 1.0825, 0.83712, 1, 3.1, 3.1, 3.1,
375 | 0.80025, 0.76805, 0.0176, -3.1, -3.1, -3.1, 0.80405, 0.79512, 0.8832, 3.1, 3.1, 3.1,
376 | 0.8448, 0.53779, 0.012, -3.1, -3.1, -3.1, 0.84815, 0.54533, 0.8568, 3.1, 3.1, 3.1,
377 | 0.94915, 0.50388, 0.012, -3.1, -3.1, -3.1, 0.9746, 0.52002, 0.394, 3.1, 3.1, 3.1,
378 | 0.90965, 0.49774, 0.012, -3.1, -3.1, -3.1, 0.9436, 0.51324, 0.4244, 3.1, 3.1, 3.1,
379 | 0.9721, 0.53129, 0.012, -3.1, -3.1, -3.1, 1.002, 0.54545, 0.4396, 3.1, 3.1, 3.1,
380 | 1.0071, 0.52443, 0.012, -3.1, -3.1, -3.1, 1.0332, 0.53895, 0.3144, 3.1, 3.1, 3.1,
381 | 1.0605, 0.477, 0.2952, -3.1, -3.1, -3.1, 1.0799, 0.48233, 1, 3.1, 3.1, 3.1,
382 | 0.82515, 0.54533, 0.012, -3.1, -3.1, -3.1, 1.0793, 0.54671, 1, 3.1, 3.1, 3.1,
383 | 0.8021, 0.47636, 0.012, -3.1, -3.1, -3.1, 1.0615, 0.47755, 1, 3.1, 3.1, 3.1,
384 | 1.0771, 0.48167, 0.012, -3.1, -3.1, -3.1, 1.0832, 0.54595, 1, 3.1, 3.1, 3.1,
385 | 0.80075, 0.47698, 0.012, -3.1, -3.1, -3.1, 0.80265, 0.50379, 0.8724, 3.1, 3.1, 3.1,
386 | 0.87538, 0.0075119, -0.008, -3.1, -3.1, -3.1, 0.89702, 0.031083, 0.9556, 3.1, 3.1, 3.1,
387 | 0.87872, 0.031083, 0.042, -3.1, -3.1, -3.1, 0.89782, 0.052226, 0.9568, 3.1, 3.1, 3.1,
388 | 0.87152, 0.053464, -0.008, -3.1, -3.1, -3.1, 0.88308, 0.075012, 0.5336, 3.1, 3.1, 3.1,
389 | 0.81538, 0.063083, 0.1376, -3.1, -3.1, -3.1, 0.84132, 0.076083, 0.3076, 3.1, 3.1, 3.1,
390 | 0.78097, 0.01425, -0.008, -3.1, -3.1, -3.1, 0.81603, 0.030298, 0.3932, 3.1, 3.1, 3.1,
391 | 0.81653, 0.074917, -0.0076, -3.1, -3.1, -3.1, 0.89787, 0.07625, 0.858, 3.1, 3.1, 3.1,
392 | 0.89667, -0.010202, 0.3768, -3.1, -3.1, -3.1, 0.89902, 0.074988, 1, 3.1, 3.1, 3.1,
393 | 0.73467, -0.01044, 0.2984, -3.1, -3.1, -3.1, 0.89708, -0.0086548, 1, 3.1, 3.1, 3.1,
394 | 0.73372, -0.0082976, 0.2472, -3.1, -3.1, -3.1, 0.73998, 0.065964, 1, 3.1, 3.1, 3.1,
395 | 1.052, 0.050881, -0.0136, -3.1, -3.1, -3.1, 1.0749, 0.074595, 0.2796, 3.1, 3.1, 3.1,
396 | 0.91065, 0.072214, -0.0072, -3.1, -3.1, -3.1, 0.94425, 0.082071, 0.2416, 3.1, 3.1, 3.1,
397 | 0.95945, 0.055452, -0.0092, -3.1, -3.1, -3.1, 0.9986, 0.074524, 0.388, 3.1, 3.1, 3.1,
398 | 0.93145, 0.022357, -0.0084, -3.1, -3.1, -3.1, 0.96835, 0.037571, 0.396, 3.1, 3.1, 3.1,
399 | 0.99465, 0.031071, -0.0112, -3.1, -3.1, -3.1, 1.0311, 0.047476, 0.3544, 3.1, 3.1, 3.1,
400 | 1.0222, 0.048452, -0.0124, -3.1, -3.1, -3.1, 1.0552, 0.063452, 0.3836, 3.1, 3.1, 3.1,
401 | 1.0173, 0.011619, -0.0124, -3.1, -3.1, -3.1, 1.0513, 0.029405, 0.3652, 3.1, 3.1, 3.1,
402 | 1.0153, 0.08031, -0.0124, -3.1, -3.1, -3.1, 1.0487, 0.096571, 0.3492, 3.1, 3.1, 3.1,
403 | 1.0572, -0.0095952, -0.0088, -3.1, -3.1, -3.1, 1.0774, -0.0002619, 1, 3.1, 3.1, 3.1,
404 | 1.0748, -0.00057143, -0.0136, -3.1, -3.1, -3.1, 1.0777, 0.11043, 1, 3.1, 3.1, 3.1,
405 | 0.9037, -0.0099762, -0.01, -3.1, -3.1, -3.1, 1.0593, -0.0089048, 1, 3.1, 3.1, 3.1,
406 | 0.9036, -0.0099286, 0.168, -3.1, -3.1, -3.1, 0.90585, 0.085214, 1, 3.1, 3.1, 3.1,
407 | 0.92785, 0.10981, -0.0064, -3.1, -3.1, -3.1, 1.0768, 0.11062, 1, 3.1, 3.1, 3.1,
408 | 0.91615, 0.11426, -0.008, -3.1, -3.1, -3.1, 0.9362, 0.13319, 0.2516, 3.1, 3.1, 3.1,
409 | 0.9762, 0.13064, -0.0072, -3.1, -3.1, -3.1, 1.0078, 0.14571, 0.3276, 3.1, 3.1, 3.1,
410 | 0.8529, 0.13271, -0.0068, -3.1, -3.1, -3.1, 0.8901, 0.15243, 0.4192, 3.1, 3.1, 3.1,
411 | 0.9445, 0.14207, -0.0064, -3.1, -3.1, -3.1, 0.9812, 0.15824, 0.338, 3.1, 3.1, 3.1,
412 | 0.9314, 0.13298, -0.0068, -3.1, -3.1, -3.1, 0.96535, 0.1479, 0.4028, 3.1, 3.1, 3.1,
413 | 1.0568, 0.17721, 0.172, -3.1, -3.1, -3.1, 1.0768, 0.18198, 1, 3.1, 3.1, 3.1,
414 | 0.79975, 0.11293, -0.0084, -3.1, -3.1, -3.1, 1.0775, 0.11426, 1, 3.1, 3.1, 3.1,
415 | 0.83275, 0.18198, -0.0036, -3.1, -3.1, -3.1, 1.0595, 0.18324, 1, 3.1, 3.1, 3.1,
416 | 1.0755, 0.11338, 0.0156, -3.1, -3.1, -3.1, 1.079, 0.17769, 1, 3.1, 3.1, 3.1,
417 | 0.8016, 0.24502, 0.0016, -3.1, -3.1, -3.1, 0.84575, 0.25545, 0.854, 3.1, 3.1, 3.1,
418 | 0.8458, 0.24502, 0.0008, -3.1, -3.1, -3.1, 0.8948, 0.2556, 0.8648, 3.1, 3.1, 3.1,
419 | 0.93445, 0.18969, -0.0004, -3.1, -3.1, -3.1, 0.9531, 0.20464, 0.2608, 3.1, 3.1, 3.1,
420 | 0.98585, 0.1974, -0.0012, -3.1, -3.1, -3.1, 1.0066, 0.2055, 0.2596, 3.1, 3.1, 3.1,
421 | 0.9851, 0.21462, -0.0012, -3.1, -3.1, -3.1, 1.0177, 0.2295, 0.314, 3.1, 3.1, 3.1,
422 | 0.84585, 0.20536, 0.0012, -3.1, -3.1, -3.1, 0.87895, 0.22129, 0.3624, 3.1, 3.1, 3.1,
423 | 0.95425, 0.19738, -0.0008, -3.1, -3.1, -3.1, 0.98695, 0.2076, 0.3324, 3.1, 3.1, 3.1,
424 | 0.89095, 0.19948, 0.0004, -3.1, -3.1, -3.1, 0.92425, 0.21507, 0.4128, 3.1, 3.1, 3.1,
425 | 1.0589, 0.18631, 0.426, -3.1, -3.1, -3.1, 1.0767, 0.19105, 1, 3.1, 3.1, 3.1,
426 | 0.79995, 0.18579, 0.2836, -3.1, -3.1, -3.1, 1.0604, 0.18679, 1, 3.1, 3.1, 3.1,
427 | 0.79925, 0.18662, 0.2972, -3.1, -3.1, -3.1, 0.80135, 0.21376, 0.8656, 3.1, 3.1, 3.1,
428 | 1.0758, 0.19081, 0.05, -3.1, -3.1, -3.1, 1.0816, 0.25588, 1, 3.1, 3.1, 3.1,
429 | 0.88385, 0.25502, -0.0012, -3.1, -3.1, -3.1, 1.0778, 0.25593, 0.8952, 3.1, 3.1, 3.1,
430 | 0.9383, 0.25888, 0.0468, -3.1, -3.1, -3.1, 0.98665, 0.2676, 1, 3.1, 3.1, 3.1,
431 | 0.8249, 0.25898, 0.004, -3.1, -3.1, -3.1, 0.8472, 0.26731, 0.934, 3.1, 3.1, 3.1,
432 | 0.9867, 0.25926, 0.0744, -3.1, -3.1, -3.1, 1.0333, 0.26905, 1, 3.1, 3.1, 3.1,
433 | 0.91465, 0.25888, 0.0036, -3.1, -3.1, -3.1, 0.9409, 0.27081, 0.3292, 3.1, 3.1, 3.1,
434 | 0.83635, 0.29114, 0.0024, -3.1, -3.1, -3.1, 0.8737, 0.30702, 0.3956, 3.1, 3.1, 3.1,
435 | 0.93245, 0.28248, 0.004, -3.1, -3.1, -3.1, 0.9663, 0.3001, 0.4208, 3.1, 3.1, 3.1,
436 | 1.005, 0.28843, 0.0052, -3.1, -3.1, -3.1, 1.0402, 0.30276, 0.342, 3.1, 3.1, 3.1,
437 | 1.0586, 0.32271, 0.2956, -3.1, -3.1, -3.1, 1.0788, 0.32812, 1, 3.1, 3.1, 3.1,
438 | 0.79955, 0.30114, 0.002, -3.1, -3.1, -3.1, 0.80125, 0.32807, 0.8696, 3.1, 3.1, 3.1,
439 | 1.0764, 0.25883, 0.2944, -3.1, -3.1, -3.1, 1.0801, 0.32314, 1, 3.1, 3.1, 3.1,
440 | 0.82435, 0.25824, 0.0028, -3.1, -3.1, -3.1, 1.0781, 0.26012, 1, 3.1, 3.1, 3.1,
441 | 0.8001, 0.32752, 0.0024, -3.1, -3.1, -3.1, 1.0596, 0.3284, 1, 3.1, 3.1, 3.1,
442 | 0.93535, 0.39048, 0.012, -3.1, -3.1, -3.1, 0.98645, 0.40074, 0.8668, 3.1, 3.1, 3.1,
443 | 0.80575, 0.33412, 0.0096, -3.1, -3.1, -3.1, 0.8269, 0.35179, 0.2652, 3.1, 3.1, 3.1,
444 | 0.9632, 0.33212, 0.0124, -3.1, -3.1, -3.1, 0.9842, 0.35031, 0.2684, 3.1, 3.1, 3.1,
445 | 0.89395, 0.33219, 0.0112, -3.1, -3.1, -3.1, 0.91675, 0.35055, 0.2708, 3.1, 3.1, 3.1,
446 | 0.8121, 0.39217, 0.01, -3.1, -3.1, -3.1, 0.8504, 0.40069, 0.8924, 3.1, 3.1, 3.1,
447 | 0.92625, 0.35338, 0.0116, -3.1, -3.1, -3.1, 0.9596, 0.37119, 0.3096, 3.1, 3.1, 3.1,
448 | 0.88655, 0.3555, 0.0108, -3.1, -3.1, -3.1, 0.9196, 0.37133, 0.3868, 3.1, 3.1, 3.1,
449 | 0.8557, 0.36345, 0.0104, -3.1, -3.1, -3.1, 0.8861, 0.37855, 0.3648, 3.1, 3.1, 3.1,
450 | 1.0168, 0.38007, 0.0132, -3.1, -3.1, -3.1, 1.0462, 0.3941, 0.426, 3.1, 3.1, 3.1,
451 | 0.9898, 0.34567, 0.0128, -3.1, -3.1, -3.1, 1.0236, 0.36102, 0.4032, 3.1, 3.1, 3.1,
452 | 1.0593, 0.33157, 0.2932, -3.1, -3.1, -3.1, 1.0802, 0.33676, 1, 3.1, 3.1, 3.1,
453 | 0.82535, 0.40002, 0.0108, -3.1, -3.1, -3.1, 1.0783, 0.40138, 1, 3.1, 3.1, 3.1,
454 | 0.80075, 0.33095, 0.01, -3.1, -3.1, -3.1, 1.0619, 0.33219, 1, 3.1, 3.1, 3.1,
455 | 1.076, 0.33662, 0.0144, -3.1, -3.1, -3.1, 1.0822, 0.40136, 1, 3.1, 3.1, 3.1,
456 | 0.8004, 0.33186, 0.0728, -3.1, -3.1, -3.1, 0.80115, 0.35871, 0.8728, 3.1, 3.1, 3.1,
457 | 0.8872, 0.45481, 0.0064, -3.1, -3.1, -3.1, 0.91, 0.46012, 0.2584, 3.1, 3.1, 3.1,
458 | 0.80475, 0.40526, 0.0104, -3.1, -3.1, -3.1, 0.84885, 0.41312, 0.9012, 3.1, 3.1, 3.1,
459 | 0.85655, 0.41514, 0.0068, -3.1, -3.1, -3.1, 0.89145, 0.4324, 0.438, 3.1, 3.1, 3.1,
460 | 0.9915, 0.42683, 0.0044, -3.1, -3.1, -3.1, 1.0282, 0.44433, 0.344, 3.1, 3.1, 3.1,
461 | 0.96135, 0.4401, 0.0048, -3.1, -3.1, -3.1, 0.9975, 0.45769, 0.3972, 3.1, 3.1, 3.1,
462 | 0.91, 0.4441, 0.006, -3.1, -3.1, -3.1, 0.9442, 0.45917, 0.4236, 3.1, 3.1, 3.1,
463 | 0.8443, 0.44995, 0.0072, -3.1, -3.1, -3.1, 0.8736, 0.46286, 0.3448, 3.1, 3.1, 3.1,
464 | 1.0595, 0.46795, 0.0212, -3.1, -3.1, -3.1, 1.0798, 0.47355, 1, 3.1, 3.1, 3.1,
465 | 0.8245, 0.40374, 0.0048, -3.1, -3.1, -3.1, 1.0797, 0.40483, 1, 3.1, 3.1, 3.1,
466 | 0.80035, 0.44614, 0.0184, -3.1, -3.1, -3.1, 0.80235, 0.47362, 0.872, 3.1, 3.1, 3.1,
467 | 1.0741, 0.40438, 0.0036, -3.1, -3.1, -3.1, 1.0834, 0.4686, 1, 3.1, 3.1, 3.1,
468 | 0.80085, 0.47286, 0.0076, -3.1, -3.1, -3.1, 1.0611, 0.47419, 1, 3.1, 3.1, 3.1,
469 | 0.37485, 0.64857, 0.0168, -3.1, -3.1, -3.1, 0.40755, 0.6646, 0.348, 3.1, 3.1, 3.1,
470 | 0.5327, 0.72531, 0.0168, -3.1, -3.1, -3.1, 0.56745, 0.74095, 0.3448, 3.1, 3.1, 3.1,
471 | 0.5672, 0.72617, 0.0168, -3.1, -3.1, -3.1, 0.59845, 0.74088, 0.34, 3.1, 3.1, 3.1,
472 | 0.40385, 0.71907, 0.0168, -3.1, -3.1, -3.1, 0.4361, 0.73443, 0.344, 3.1, 3.1, 3.1,
473 | 0.43405, 0.71793, 0.0168, -3.1, -3.1, -3.1, 0.4656, 0.73171, 0.3448, 3.1, 3.1, 3.1,
474 | 0.4639, 0.72031, 0.0168, -3.1, -3.1, -3.1, 0.4958, 0.73452, 0.346, 3.1, 3.1, 3.1,
475 | 0.40535, 0.64907, 0.0168, -3.1, -3.1, -3.1, 0.4398, 0.66407, 0.346, 3.1, 3.1, 3.1,
476 | 0.43735, 0.65045, 0.0168, -3.1, -3.1, -3.1, 0.4702, 0.66521, 0.3476, 3.1, 3.1, 3.1,
477 | 0.5802, 0.6541, 0.0168, -3.1, -3.1, -3.1, 0.61965, 0.67231, 0.34, 3.1, 3.1, 3.1,
478 | 0.6054, 0.66038, 0.0168, -3.1, -3.1, -3.1, 0.6449, 0.67926, 0.3448, 3.1, 3.1, 3.1,
479 | 0.6311, 0.66769, 0.0168, -3.1, -3.1, -3.1, 0.66835, 0.6856, 0.3448, 3.1, 3.1, 3.1,
480 | 0.35795, 0.5921, 0.0168, -3.1, -3.1, -3.1, 0.39005, 0.6091, 0.3896, 3.1, 3.1, 3.1,
481 | 0.35865, 0.57633, 0.0168, -3.1, -3.1, -3.1, 0.38865, 0.59452, 0.4256, 3.1, 3.1, 3.1,
482 | 0.61035, 0.74, 0.0168, -3.1, -3.1, -3.1, 0.6462, 0.75519, 0.3436, 3.1, 3.1, 3.1,
483 | 0.33065, 0.75388, 0.0168, -3.1, -3.1, -3.1, 0.3426, 0.76445, 1, 3.1, 3.1, 3.1,
484 | 0.33065, 0.60826, 0.0168, -3.1, -3.1, -3.1, 0.3418, 0.63312, 1, 3.1, 3.1, 3.1,
485 | 0.68535, 0.60805, 0.0168, -3.1, -3.1, -3.1, 0.69545, 0.63331, 1, 3.1, 3.1, 3.1,
486 | 0.6871, 0.75329, 0.0168, -3.1, -3.1, -3.1, 0.6954, 0.76474, 1, 3.1, 3.1, 3.1,
487 | 0.38365, 0.57495, 0.0168, -3.1, -3.1, -3.1, 0.6768, 0.5805, 0.9024, 3.1, 3.1, 3.1,
488 | 0.34265, 0.76357, 0.0168, -3.1, -3.1, -3.1, 0.68765, 0.765, 1, 3.1, 3.1, 3.1,
489 | 0.5312, 0.91895, 0.5628, -3.1, -3.1, -3.1, 0.71015, 0.92848, 0.7688, 3.1, 3.1, 3.1,
490 | 0.51785, 0.92631, 0.3392, -3.1, -3.1, -3.1, 0.53445, 0.98171, 0.752, 3.1, 3.1, 3.1,
491 | 0.5165, 0.98152, -0.0056, -3.1, -3.1, -3.1, 0.68655, 0.98264, 1, 3.1, 3.1, 3.1,
492 | 0.51545, 0.91826, 0.3672, -3.1, -3.1, -3.1, 0.51825, 0.9825, 1, 3.1, 3.1, 3.1,
493 | 0.5177, 0.91779, -0.0056, -3.1, -3.1, -3.1, 0.7107, 0.91943, 0.8596, 3.1, 3.1, 3.1,
494 | 0.7099, 0.91869, -0.0004, -3.1, -3.1, -3.1, 0.7114, 0.95783, 0.8688, 3.1, 3.1, 3.1,
495 | 0.3189, 0.91876, 0.0012, -3.1, -3.1, -3.1, 0.49605, 0.91957, 0.8596, 3.1, 3.1, 3.1,
496 | 0.34325, 0.98157, 0.0004, -3.1, -3.1, -3.1, 0.5123, 0.9824, 1, 3.1, 3.1, 3.1,
497 | 0.51065, 0.93031, 0.3516, -3.1, -3.1, -3.1, 0.51255, 0.98169, 1, 3.1, 3.1, 3.1,
498 | 0.3183, 0.9191, 0.0036, -3.1, -3.1, -3.1, 0.31965, 0.95676, 0.8708, 3.1, 3.1, 3.1,
499 | 0.3385, 0.19048, 0, -3.1, -3.1, -3.1, 0.68545, 0.19286, 1, 3.1, 3.1, 3.1,
500 | -0.1, -0.047619, 0, -3.1, -3.1, -3.1, 1.125, 1.0714, 0.004, 3.1, 3.1, 3.1,
501 | -0.1, -0.047619, 1, -3.1, -3.1, -3.1, 1.125, 1.0714, 1.004, 3.1, 3.1, 3.1,
502 | 
503 | the format is:
504 | 	initial position
505 | 	goal position
506 | 	lower bound on state space
507 | 	upper bound on state space
508 | 	number of obstacles
509 | 	obstacles


--------------------------------------------------------------------------------