bool AStar::judgeSurround(const ASCOORD& coord,const ASCOORD& parentCoord,int G) { if(!isInList(m_closeList,coord) && !m_mark.isMask(coord._y,coord._x)) { StepData* pSD = findFromList(m_openList,coord); if (pSD && pSD->_g > G) { pSD->_g = G; pSD->_parentCoord = parentCoord; } else if(!pSD) { StepData newSD(coord); newSD._g = G; newSD._h = computeH(coord); newSD._parentCoord = parentCoord; m_openList.push_back(newSD); if(newSD._coord == m_start) return true; } } return false; }
// Use RANSAC to compute H without openCV function Mat Assignment2::computeRANSAC(Assignment2 &m2) { vector< DMatch > matches = this->FindMatchesEuclidian(m2); // find matches based on euclidian distances // Create vectors for storing Points vector<Point2f> query(matches.size()); vector<Point2f> train(matches.size()); //cout << matches.size() << " " << keypoints.size() << endl; // Assign points to that declared array for(unsigned int i=0;i<(matches.size());i++) { query[i]=this->keypoints[matches[i].queryIdx].pt; train[i]=m2.keypoints[matches[i].trainIdx].pt; } // define parameters for RANSAC algorithm int iterations=0; double inlierThresh=0.90; double inlierPerc=0; Mat H; Mat Hbest; double inlierbest=0; // do while loop for 3000 iterations do { int N=matches.size(); int M=4; vector<Point2f> queryH(M); // take 4 points vector<Point2f> trainH(M); vector<int> randVecId=randomGenerate(N,M); for(int i=0;i<M;i++) { queryH[i]=query[randVecId[i]]; // take 4 points randomly trainH[i]=train[randVecId[i]]; } H = computeH(queryH, trainH); // Compute H based on the that 4 points double inliners = computeInliers(train, query, H, 3); // calculate inliners for that H if(inliners>inlierbest) { inlierbest=inliners; Hbest=H.clone(); } inlierPerc=inliners/(double)N; iterations++; } while(iterations<3000);//&&inlierPerc<inlierThresh); normalizeH(Hbest); // normalize Hbest return Hbest; }
bool AStar::ComputeRoute() { m_openList.clear(); m_closeList.clear(); StepData sd(m_target); sd._g = 0; sd._h = computeH(m_target); m_openList.push_back(sd); while (!m_openList.empty()) { popBestStep(&sd); m_closeList.push_back(sd); ASCOORD coord; for(int i = 0 ; i<m_numSurround ; ++i) { coord = sd._coord; coord += m_surround[i]; if(judgeSurround(coord,sd._coord,sd._g+m_gAdd[i])) return true; } } return false; }
void main(int argc, char *argv[]){ int t; char fname1[100], fname2[100]; int m = sprintf(fname1,"phi"); int n = sprintf(fname2,"velocities"); MPI_Init(&argc,&argv); MPI_Comm_size(MPI_COMM_WORLD,&numtasks); MPI_Comm_rank(MPI_COMM_WORLD,&taskid); numworkers = numtasks - 1; allocate_memory_phasefields(MESHX); #ifdef FLUID allocate_memory_FluidVariables(MESHX,pmesh); #endif if(taskid == MASTER) { initialize_phasefields(); #ifdef FLUID initialize_velocities(MESHX); #endif mpi_distribute(MESHX); for (t = 0; t <= phi_timesteps; t++){ #ifdef FLUID gauss_siedel(); #endif if (t%save_phi == 0) { receivefrmworker(phi_old); receivefrmworker(u_old); receivefrmworker(v_old); write2file( t, MESHX, phi_old, fname1); write2file1( t, MESHX, u_old, v_old, fname2); } } }else { mpi_distribute(MESHX); for (t = 0; t <= phi_timesteps; t++){ mpiexchange(taskid, phi_old, MESHX); mpiexchange(taskid, mu_old, MESHX); boundary_mpi(taskid, phi_old, MESHX); boundary_mpi(taskid, mu_old, MESHX); laplacian(phi_old, lap_phi, MESHX); laplacian(mu_old, lap_mu, MESHX); #ifdef FLUID computeH(u_old,v_old,Hx,Hy); RHS_fn(Hx,Hy,rhs_fn,MESHX, pmesh); LHS_fn(MESHX, pmesh); boundary_pressure(taskid); gauss_siedel(); ns_solver(start, end, phi_old); update_velocities(MESHX); #endif solverloop(); phi_update(); if (t%save_phi == 0) { sendtomaster(taskid, phi_old); } } } }
/** The signature of this function matches KTR_callback in knitro.h. * Only "hessian" or "hessVector" is modified. */ int callbackEvalHess (const int evalRequestCode, const int n, const int m, const int nnzJ, const int nnzH, const double * const x, const double * const lambda, double * const obj, double * const c, double * const objGrad, double * const jac, double * const hessian, double * const hessVector, void * userParams) { /*---- IN THIS EXAMPLE, CALL THE ROUTINES IN problemDef.h. */ switch (evalRequestCode) { case KTR_RC_EVALH: computeH (x, 1.0, lambda, hessian); break; case KTR_RC_EVALH_NO_F: computeH (x, 0.0, lambda, hessian); break; case KTR_RC_EVALHV: computeHV (x, 1.0, lambda, hessVector); break; case KTR_RC_EVALHV_NO_F: computeHV (x, 0.0, lambda, hessVector); break; default: printf ("*** callbackEvalHess incorrectly called with eval code %d\n", evalRequestCode); return( -1 ); } return( 0 ); }
//if cell is an obstacle or is already on closedList ignore it. Otherwise add cell to openList. void evalCell(int x, int y){ if((!isOnClosed(x, y)) && (!isOnOpen(x, y)) && (mapLayout[x][y] != 'x')){ openList[openLength].g = (closedList[closedLength - 1].g + 1); openList[openLength].h = computeH(x, y); openList[openLength].f = openList[openLength].h + openList[openLength].g; openList[openLength].x = x; openList[openLength].y = y; openList[openLength].pIdx = closedLength-1; openLength++; } return; }
fluid_solver(){ int i,j,z,x; double dp_dx,dp_dy,du_dt,dv_dt; double phi; boundary_fluid(); computeH(u_old,v_old,Hx,Hy); RHS_fn(Hx,Hy,rhs_fn,MESHX); LHS_fn(); boundary_pressure(); #ifdef GS_MPI gs_mpi(); #endif #ifdef GS_UNI Gauss_siedel(P, rhs_fn, a_x, a_y); #endif for(i=1; i<MESHX-1; i++){ for(j=1; j<MESHX-1; j++){ z = i*MESHX +j; x = (i-1)*(pmesh)+ (j-1); phi = phi_old[z]; dp_dx = 0.5*inv_deltax*(P[x+1]+P[x+pmesh+1]-P[x+pmesh]-P[x]); dp_dy = 0.5*inv_deltax*(P[x+pmesh]+P[x+pmesh+1]-P[x+1]-P[x]); du_dt = Hx[z] - dp_dx*(1.0-phi);// - Mu*2.757*phi*phi*u_str[z]*inv_deltax2; dv_dt = Hy[z] - dp_dy*(1.0-phi);// - Mu*2.757*phi*phi*v_str[z]*inv_deltax2; u_now[z] = u_str[z] + deltat * du_dt; v_now[z] = v_str[z] + deltat * dv_dt; } } V_update(MESHX); boundary_fluid(); }
PortalRoute * PathPlanner::computeRoute( unsigned int startID, unsigned int endID, float minWidth ) { const size_t N = _navMesh->getNodeCount(); #ifdef _OPENMP // Assuming that threadNum \in [0, omp_get_max_threads() ) const unsigned int threadNum = omp_get_thread_num(); AStarMinHeap heap( _HEAP + threadNum * N, _DATA + threadNum * DATA_SIZE, _STATE + threadNum * STATE_SIZE, _PATH + threadNum * N, N ); #else AStarMinHeap heap( _HEAP, _DATA, _STATE, _PATH, N ); #endif const Vector2 goalPos( _navMesh->getNode( endID ).getCenter() ); heap.g( startID, 0 ); heap.h( startID, computeH( startID, goalPos ) ); heap.f( startID, heap.h( startID ) ); heap.push( startID ); bool found = false; while ( !heap.empty() ) { unsigned int x = heap.pop(); //std::cout << "\tTesting node " << x << "\n"; if ( x == endID ) { found = true; break; } NavMeshNode & node = _navMesh->_nodes[ x ]; //std::cout << "\t\tNeighbors:\n"; for ( size_t e = 0; e < node._edgeCount; ++e ) { NavMeshEdge * edge = node._edges[ e ]; unsigned int y = edge->getOtherByID( x )->_id; //std::cout << "\t\t\t" << y << "\n"; if ( heap.isVisited( y ) ) continue; float distance = edge->getNodeDistance( minWidth ); if ( distance < 0.f ) continue; float tempG = heap.g( x ) + distance; //std::cout << "\t\t\t\ttemp G: " << tempG << ", old g = " << heap.g(y) << "\n"; bool isOld = true; if ( ! heap.isInHeap( y ) ) { heap.h( y, computeH( y, goalPos ) ); isOld = false; } if ( tempG < heap.g( y ) ) { heap.setReachedFrom( y, x ); heap.g( y, tempG ); heap.f( y, tempG + heap.h( y ) ); //std::cout << "\t\t\t\tIMPROVED: g = " << heap.g(y) << ", f = " << heap.f(y) << "\n"; //std::cout << "\t\t\t\t" << y << " came from " << x << "\n"; } if ( ! heap.isInHeap( y ) ) { heap.push( y ); //std::cout << "\t\t\t\tAdding to open with h = " << heap.h(y) << "\n"; } } } // reconstruct the path if ( !found ) { std::stringstream ss; ss << "Trying to find a path from " << startID << " to " << endID << ". A* finished without a route!"; throw PathPlannerException( ss.str() ); } // Create the list of nodes through which I must pass std::list< unsigned int > path; unsigned int curr = endID; while ( curr != startID ) { path.push_front( curr ); curr = heap.getReachedFrom( curr ); } path.push_front( startID ); #ifdef _WIN32 // Visual studio 2005 compiler is giving an erroneous warning // It feels that: // unsigned int prev = *itr; // Is trying to cast a value of type size_t into a value of type unsigned int // which, if true, would possibly lose data. However, this is simply incorrect, // as the iterator is to a list of unsigned ints. // // Because the code is correct, this is the only way to silence the stupid warning. #pragma warning( disable : 4267 ) #endif // Now construct the path std::list< unsigned int >::const_iterator itr = path.begin(); unsigned int prev = *itr; NavMeshNode * prevNode = &_navMesh->_nodes[ prev ]; ++itr; PortalRoute * route = new PortalRoute( startID, endID ); route->_bestSmallest = minWidth; for ( ; itr != path.end(); ++itr ) { unsigned int id = *itr; NavMeshEdge * edge = prevNode->getConnection( id ); route->appendWayPortal( edge, prevNode->getID() ); prevNode = &_navMesh->_nodes[ id ]; } #ifdef _WIN32 #pragma warning( default : 4267 ) #endif cacheRoute( startID, endID, route ); return route; }
RoadMapPath * Graph::getPath( size_t startID, size_t endID ) { const size_t N = _vCount; #ifdef _OPENMP // Assuming that threadNum \in [0, omp_get_max_threads() ) const unsigned int threadNum = omp_get_thread_num(); AStarMinHeap heap( _HEAP + threadNum * N, _DATA + threadNum * DATA_SIZE, _STATE + threadNum * STATE_SIZE, _PATH + threadNum * N, N ); #else AStarMinHeap heap( _HEAP, _DATA, _STATE, _PATH, N ); #endif const Vector2 goalPos( _vertices[ endID ].getPosition() ); heap.g( (unsigned int)startID, 0 ); heap.h( (unsigned int)startID, computeH( startID, goalPos ) ); heap.f( (unsigned int)startID, heap.h( (unsigned int)startID ) ); heap.push( (unsigned int)startID ); bool found = false; while ( !heap.empty() ) { unsigned int x = heap.pop(); if ( x == endID ) { found = true; break; } GraphVertex & vert = _vertices[ x ]; const size_t E_COUNT = vert.getEdgeCount(); for ( size_t n = 0; n < E_COUNT; ++n ) { const GraphVertex * nbr = vert.getNeighbor( n ); size_t y = nbr->getID(); if ( heap.isVisited( (unsigned int)y ) ) continue; float distance = vert.getDistance( n ); float tempG = heap.g( x ) + distance; bool inHeap = heap.isInHeap( (unsigned int)y ); if ( ! inHeap ) { heap.h( (unsigned int)y, computeH( y, goalPos ) ); } if ( tempG < heap.g( (unsigned int)y ) ) { heap.setReachedFrom( (unsigned int)y, (unsigned int)x ); heap.g( (unsigned int)y, tempG ); heap.f( (unsigned int)y, tempG + heap.h( (unsigned int)y ) ); } if ( ! inHeap ) { heap.push( (unsigned int)y ); } } } if ( !found ) { logger << Logger::ERR_MSG << "Was unable to find a path from " << startID << " to " << endID << "\n"; return 0x0; } // Count the number of nodes in the path size_t wayCount = 1; // for the startID size_t next = endID; while ( next != startID ) { ++wayCount; next = heap.getReachedFrom( (unsigned int)next ); } RoadMapPath * path = new RoadMapPath( wayCount ); next = endID; for ( size_t i = wayCount; i > 0; --i ) { path->setWayPoint( i - 1, _vertices[ next ].getPosition() ); next = heap.getReachedFrom( (unsigned int)next ); } return path; }