int recursiveMove(int i, int j, int m, int n, vector<vector<int> > &obstacleGrid)
{
if (i >= m || j >= n)
return 0;
if (obstacleGrid[i][j] == 1)
return 0;
if (i == (m-1) && j == (n-1))
return 1;
return recursiveMove(i+1, j, m, n, obstacleGrid) + recursiveMove(i, j+1, m, n, obstacleGrid);
}
/* This fuction traveres the table and add vint and time to the trajectory. */
int recursiveMove(std::vector<classPoint2T> &traj, int t, int d, int vend_index, int v0_index, std::vector<std::vector<fSet> >& G, std::vector<std::vector<int> > &T, std::vector<std::vector<int> > &D ){
// std::cout<<"t and d: "<<t<< " - "<<d<<std::endl;
if(t <= 0.1 && d <= 0.1){
// std::cout<<" End recursive move!"<<std::endl;
return 1;
}
// print T
std::cout<<"T table in recursive:"<<std::endl;
for(int i=0; i<26; i++){
for(int j = 0; j<26; j++)
std::cout<<T[i][j]<<"\t";
std::cout<<"\n";
}
for(int vint_index=0; vint_index<M; vint_index++){
int delta_d = d - D[v0_index][vint_index];
if(delta_d >=0){
int delta_t = t - T[v0_index][vint_index];
classPoint2T point(delta_t, vend_index);
if(delta_t>=0 && point.inSet(G[vint_index][delta_d]) >=0){
/* add point to the trajectory */
std::cout<<"v0_index: "<<v0_index<<" vint_index: "<<vint_index<<" T[][]: "<<T[v0_index][vint_index]<<" D[][]: "<<D[v0_index][vint_index]<< " t:" <<t<<" d:"<<d<<std::endl;
classPoint2T intPoint(T[v0_index][vint_index] + traj.back().time, vint_index);
traj.push_back(intPoint);
// std::cout<<"\t recursiveMove: obtained an intermediate point:"<< intPoint.time <<"-"<<intPoint.vel<<std::endl;
// continue recursiveMove() with new ending time, ending distance and starting velocity
recursiveMove(traj, delta_t, delta_d, vend_index, vint_index, G, T, D);
break;
}
}
}
}
int uniquePathsWithObstacles(vector<vector<int> > &obstacleGrid) {
// Start typing your C/C++ solution below
// DO NOT write int main() function
int m = obstacleGrid.size();
if (m == 0)
return 0;
int n = obstacleGrid[0].size();
if (n == 0)
return 0;
return recursiveMove(0, 0, m, n, obstacleGrid);
}
//*******************************************************//
int findTrajectory(std::vector<std::vector<fSet> >& G, int &targetDistance, int v0_index, classPoint2T &point, std::vector<std::vector<int> > &T, std::vector<std::vector<int> > &D){
/* This function find out the trajectory of a feasible point */
// Initialize a trajectory vector
std::vector<classPoint2T> traj;
// std::cout<<"Size of G table:"<<M<<" x "<<N<<std::endl;
int t = point.time;
int d = targetDistance;
int vend_index = point.vel;
// print T
std::cout<<"T table in findTrajectory:"<<std::endl;
for(int i=0; i<M; i++){
for(int j = 0; j<M; j++)
std::cout<<T[i][j]<<"\t";
std::cout<<"\n";
}
// Add starting point to the trajectory
classPoint2T startingPoint(0,v0_index);
traj.push_back(startingPoint);
/* Now travere the table and add vint and time to the trajectory. */
if(recursiveMove(traj, t, d, vend_index, v0_index, G, T, D) ) {
// std::cout<<"\t Successfully retrieve a plan"<<std::endl;
}
/* // if starting time is not 0, donot return the obtained plan
if(traj[0].time >= 1) {
std::cout<<"\t But starting time is not zero."<<std::endl;
return -1;
} */
std::cout<<"\tplan obtained with arrival point ";
point.print();
std::cout<<" is:" <<std::endl;
std::cout<<"[["<<targetDistance<<","<<v0_index<<","<<point.vel<<","<<point.time<<"],"<<std::endl;
std::cout<<"[";
for(unsigned int i =0;i<traj.size();++i){
traj[i].print();
if(i<traj.size() - 1)
std::cout<<",";
}
std::cout<<"]\n"<<std::endl;
return 1;
}
