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