コード例 #1
0
ファイル: PathFinder.cpp プロジェクト: highjin/ProjectArthas
bool PathFinder::findWay(Tag nextCheckTag)
{
	std::priority_queue<Tag, std::vector<Tag>, Compare> openTags;
	int checkResult = FAIL;
	cocos2d::Point checkingPos;
	m_CurTag = nextCheckTag;

	for(int dir = DIR_UP; dir < DIR_MAX; ++dir)
	{
		checkingPos = findNeighbor(dir);
		checkResult |= checkPos(checkingPos, &openTags);
		if(checkResult & FIND)
		{
			return true;
		}
	}

	if(checkResult & CHECKING)
	{
		while(!openTags.empty())
		{
   			Tag nextTag = openTags.top();
			if(findWay(nextTag))
			{
				m_Path.push(cocos2d::Point(nextTag.m_X, nextTag.m_Y));
				return true;
			}
			openTags.pop();
		}
	}
	return false;
}
コード例 #2
0
ファイル: the-maze.cpp プロジェクト: BriteSun/LeetCode-1
    bool hasPath(vector<vector<int>>& maze, vector<int>& start, vector<int>& destination) {
        static const vector<vector<int>> dirs = {{-1, 0}, {0, 1}, {0, -1}, {1, 0}};
        queue<node> q;
        unordered_set<int> visited;
        q.emplace(0, start);

        while (!q.empty()) {
            int dist = 0;
            vector<int> node;
            tie(dist, node) = q.front();
            q.pop();
            if (visited.count(hash(maze, node))) {
                continue;
            }
            if (node[0] == destination[0] &&
                node[1] == destination[1]) {
                return true;
            }

            visited.emplace(hash(maze, node));
            for (const auto& dir : dirs) {
                int neighbor_dist = 0;
                vector<int> neighbor;
                tie(neighbor_dist, neighbor) = findNeighbor(maze, node, dir);
                q.emplace(dist + neighbor_dist, neighbor);
            }
        }

        return false;
    }
コード例 #3
0
ファイル: rfx.c プロジェクト: michirod/cgr-jni
PsmAddress	postProbeEvent(IonNode *node, Embargo *embargo)
{
	PsmPartition	ionwm = getIonwm();
	PsmAddress	addr;
	IonProbe	*probe;
	IonVdb		*ionvdb;
	IonNeighbor	*neighbor;
	PsmAddress	nextElt;
	unsigned int	rtlt;		/*	Round-trip light time.	*/
	int		interval = 6;	/*	Minimum 6-sec interval.	*/
	PsmAddress	elt;
	IonProbe	*pr;

	CHKZERO(node);
	CHKZERO(embargo);
	addr = psm_zalloc(ionwm, sizeof(IonProbe));
	if (addr == 0)
	{
		putErrmsg("Can't create probe event.", NULL);
		return 0;
	}

	probe = (IonProbe *) psp(ionwm, addr);
	CHKZERO(probe);
	probe->time = getUTCTime();
	probe->destNodeNbr = node->nodeNbr;
	probe->neighborNodeNbr = embargo->nodeNbr;

	/*	Schedule next probe of this embargoed neighbor for the
	 *	time that is the current time plus 2x the round-trip
	 *	light time from the local node to the neighbor (but
	 *	at least 6 seconds).					*/
	 
	ionvdb = getIonVdb();
	neighbor = findNeighbor(ionvdb, embargo->nodeNbr, &nextElt);
	if (neighbor)
	{
		rtlt = (neighbor->owltOutbound + neighbor->owltInbound) << 1;
		if (rtlt > interval)
		{
			interval = rtlt;
		}
	}

	probe->time += interval;
	for (elt = sm_list_last(ionwm, ionvdb->probes); elt;
			elt = sm_list_prev(ionwm, elt))
	{
		pr = (IonProbe *) psp(ionwm, sm_list_data(ionwm, elt));
		CHKZERO(pr);
		if (pr->time <= probe->time)
		{
			return sm_list_insert_after(ionwm, elt, addr);
		}
	}

	return sm_list_insert_first(ionwm, ionvdb->probes, addr);
}
コード例 #4
0
ファイル: control.cpp プロジェクト: Mr-Phoebe/QT-learning
Control::Control(QWidget *parent) :
    QDialog(parent),
    ui(new Ui::Control)
{
    ui->setupUi(this);
    setWindowTitle("GSM管理界面");
    out = new Dataout(0,"0",this);
    in = new DataIn();
    ui->stackedWidget->setCurrentIndex(0);
    ui->stackedWidget_2->setCurrentIndex(0);
    QRegExp regExp("0?[.][0-9]+$");
    ui->lineEdit->setValidator(new QRegExpValidator(regExp,this));
    initTime();
    addBTSname();
    addCellID();
    connect(ui->confirm,SIGNAL(clicked()),this,SLOT(data_in()));
    connect(ui->confirm_2,SIGNAL(clicked()),this,SLOT(data_out()));
    connect(ui->pushButton,SIGNAL(clicked()),this,SLOT(page1()));
    connect(ui->pushButton_2,SIGNAL(clicked()),this,SLOT(page2()));
    connect(ui->confirm_3,SIGNAL(clicked()),this,SLOT(queryBTS()));
    connect(ui->pushButton_3,SIGNAL(clicked()),this,SLOT(page3()));
    connect(ui->pushButton_4,SIGNAL(clicked()),this,SLOT(page4()));
    connect(ui->confirm_4,SIGNAL(clicked()),this,SLOT(queryCell()));
    connect(ui->pushButton_6,SIGNAL(clicked()),this,SLOT(bulkInsert()));
    connect(ui->pushButton_5,SIGNAL(clicked()),this,SLOT(page5()));
    connect(ui->pushButton_9,SIGNAL(clicked()),this,SLOT(reCalculateDatas()));
    connect(ui->pushButton_7,SIGNAL(clicked()),this,SLOT(page6()));
    connect(ui->pushButton_11,SIGNAL(clicked()),this,SLOT(printNeighbor()));
    connect(ui->pushButton_8,SIGNAL(clicked()),this,SLOT(page7()));
    connect(ui->pushButton_10,SIGNAL(clicked()),this,SLOT(findNeighbor()));
    connect(ui->pushButton_12,SIGNAL(clicked()),this,SLOT(findCellInfo()));


    connect(ui->pushButton_13,SIGNAL(clicked()),this,SLOT(page13()));
    connect(ui->pushButton_14,SIGNAL(clicked()),this,SLOT(page14()));
    connect(ui->pushButton_15,SIGNAL(clicked()),this,SLOT(page15()));
    connect(ui->pushButton_16,SIGNAL(clicked()),this,SLOT(page16()));
    connect(ui->pushButton_17,SIGNAL(clicked()),this,SLOT(page17()));
    connect(ui->pushButton_18,SIGNAL(clicked()),this,SLOT(page18()));

    connect(ui->pushButton_22,SIGNAL(clicked()),this,SLOT(action22()));
    connect(ui->pushButton_23,SIGNAL(clicked()),this,SLOT(action23()));
}
コード例 #5
0
void SetIntervalReg::contract(IntervalVector * box,int type) {
	vector<SetNodeReg*> neigh;
	findNeighbor(*box,&neigh);
	vector<IntervalVector> neighbox;
	double min,max;
	while(!neigh.empty()) {
		if(neigh.back()->status == type) {neighbox.push_back(findNodeBox(neigh.back()));}
		neigh.pop_back();
	}
	if(neighbox.empty()) {
		*box = IntervalVector::empty(box->size());
		return;
	}
	for(int i = 0;i<box->size();i++) {
		double min((*box)[i].ub()),max((*box)[i].lb());
		for(int j =0;j<neighbox.size();j++) {
			min = (min<neighbox.at(j)[i].lb()? min : neighbox.at(j)[i].lb());
			max = (max>neighbox.at(j)[i].ub()? max : neighbox.at(j)[i].ub());
		}
		if(min>(*box)[i].lb()) {(*box)[i] = Interval(min,(*box)[i].ub());}
		if(max<(*box)[i].ub()) {(*box)[i] = Interval((*box)[i].lb(),max);}
	}
}
コード例 #6
0
void
mexFunction(int nout, mxArray *out[],
            int nin, const mxArray *in[])
{
  enum { X=0,Y,I,iwXp,iwYp } ;
  enum { wI=0, wIx, wIy } ;

  int M, N, Mp, Np, ip, jp ;
  double
    *X_pt,
    *Y_pt,
    *I_pt,
    *iwXp_pt,
    *iwYp_pt,
    *wI_pt,
    *wIx_pt   = 0,
    *wIy_pt   = 0 ;

  double Xmin, Xmax, Ymin, Ymax ;
  const double NaN = mxGetNaN() ;

  /* -----------------------------------------------------------------
   *                                               Check the arguments
   * -------------------------------------------------------------- */
  if (nin < 5) {
    vlmxError (vlmxErrNotEnoughInputArguments, NULL) ;
  }
  if (nin > 5) {
    vlmxError (vlmxErrTooManyOutputArguments, NULL) ;
  }
  if (nout > 3) {
    vlmxError (vlmxErrTooManyOutputArguments, NULL) ;
  }

  if (! vlmxIsPlainMatrix(in[I], -1, -1)) {
    vlmxError (vlmxErrInvalidArgument, "I is not a plain matrix.") ;
  }

  if (! vlmxIsPlainMatrix(in[iwXp], -1, -1)) {
    vlmxError(vlmxErrInvalidArgument, "iwXp is not a plain matrix.") ;
  }

  M = getM(I) ;
  N = getN(I) ;
  Mp = getM(iwXp) ;
  Np = getN(iwXp) ;

  if(!vlmxIsPlainMatrix(in[iwYp], Mp, Np)) {
    vlmxError(vlmxErrInvalidArgument,
              "iwXp is not a plain matrix of the same idmension of iwYp.") ;
  }

  if(!vlmxIsPlainVector(in[X],N) || !vlmxIsPlainVector(in[Y],M)) {
    vlmxError(vlmxErrInvalidArgument,
              "X and Y are not plain vectors with a length equal to the"
	      " number of columns and rows of I.") ;
  }

  X_pt = getPr(X);
  Y_pt = getPr(Y) ;
  I_pt = getPr(I) ;
  iwXp_pt = getPr(iwXp) ;
  iwYp_pt = getPr(iwYp) ;

  /* Allocate the result. */
  out[wI] = mxCreateDoubleMatrix(Mp, Np, mxREAL) ;
  wI_pt = mxGetPr(out[wI]) ;

  if (nout > 1) {
    out[wIx] = mxCreateDoubleMatrix(Mp, Np, mxREAL) ;
    out[wIy] = mxCreateDoubleMatrix(Mp, Np, mxREAL) ;
    wIx_pt = mxGetPr (out[wIx]) ;
    wIy_pt = mxGetPr (out[wIy]) ;
  }

  /* -----------------------------------------------------------------
   *                                                        Do the job
   * -------------------------------------------------------------- */
  Xmin = X_pt [0] ;
  Xmax = X_pt [N - 1] ;
  Ymin = Y_pt [0] ;
  Ymax = Y_pt [M - 1] ;

  if (nout <= 1) {

    /* optimized for only image output */
    for(jp = 0 ; jp < Np ; ++jp) {
      for(ip = 0 ; ip < Mp ; ++ip) {
	/* Search for the four neighbors of the backprojected point. */
	double x = *iwXp_pt++ ;
	double y = *iwYp_pt++ ;
	double z = NaN ;

	/* This messy code allows the identity transformation
	 * to be processed as expected. */
	if(x >= Xmin && x <= Xmax &&
	   y >= Ymin && y <= Ymax) {
	  int j = findNeighbor(x, X_pt, N) ;
	  int i = findNeighbor(y, Y_pt, M) ;
	  double* pt  = I_pt + j*M + i ;

	  /* Weights. */
	  double x0 = X_pt[j] ;
	  double x1 = (j < N-1) ? X_pt[j+1] : x0 + 1;
	  double y0 = Y_pt[i] ;
	  double y1 = (i < M-1) ? Y_pt[i+1] : y0 + 1;
	  double wx = (x-x0)/(x1-x0) ;
	  double wy = (y-y0)/(y1-y0) ;

	  /* Load all possible neighbors. */
	  double z00 = 0.0 ;
	  double z10 = 0.0 ;
	  double z01 = 0.0 ;
	  double z11 = 0.0 ;

	  if(j > -1) {
	    if(i > -1 ) z00 = *pt ;
	    pt++ ;
	    if(i < M-1) z10 = *pt ;
	  } else {
	    pt++ ;
	  }

	  pt += M - 1;

	  if(j < N-1) {
	    if(i > -1 ) z01 = *pt ;
	    pt++ ;
	    if(i < M-1) z11 = *pt ;
	  }

	  /* Bilinear interpolation. */
	  z =
	    (1 - wy) * ((1-wx) * z00 + wx * z01) +
	    (    wy) * ((1-wx) * z10 + wx * z11) ;
	}

	*(wI_pt + jp*Mp + ip) = z ;
      }
    }
  }

  /* do also the derivatives */
  else {

    /* optimized for only image output */
    for(jp = 0 ; jp < Np ; ++jp) {
      for(ip = 0 ; ip < Mp ; ++ip) {
	/* Search for the four neighbors of the backprojected point. */
	double x = *iwXp_pt++ ;
	double y = *iwYp_pt++ ;
	double z = NaN, zx = NaN, zy = NaN ;

	/* This messy code allows the identity transformation
	 * to be processed as expected. */
	if(x >= Xmin && x <= Xmax &&
	   y >= Ymin && y <= Ymax) {
	  int j = findNeighbor(x, X_pt, N) ;
	  int i = findNeighbor(y, Y_pt, M) ;
	  double* pt  = I_pt + j*M + i ;

	  /* Weights. */
	  double x0 = X_pt[j] ;
	  double x1 = X_pt[j+1] ;
	  double y0 = Y_pt[i] ;
	  double y1 = Y_pt[i+1] ;
	  double wx = (x-x0)/(x1-x0) ;
	  double wy = (y-y0)/(y1-y0) ;

	  /* Load all possible neighbors. */
	  double z00 = 0.0 ;
	  double z10 = 0.0 ;
	  double z01 = 0.0 ;
	  double z11 = 0.0 ;

	  if(j > -1) {
	    if(i > -1 ) z00 = *pt ;
	    pt++ ;
	    if(i < M-1) z10 = *pt ;
	  } else {
	    pt++ ;
	  }

	  pt += M - 1;

	  if(j < N-1) {
	    if(i > -1 ) z01 = *pt ;
	    pt++ ;
	    if(i < M-1) z11 = *pt ;
	  }

	  /* Bilinear interpolation. */
	  z =
	    (1-wy)*( (1-wx) * z00 + wx * z01) +
	    wy*( (1-wx) * z10 + wx * z11) ;

	  zx =
	    (1-wy) * (z01 - z00) +
	       wy  * (z11 - z10) ;

	  zy =
	    (1-wx) * (z10 - z00) +
	       wx  * (z11 - z01) ;
	}

	*(wI_pt  + jp*Mp + ip) = z ;
	*(wIx_pt + jp*Mp + ip) = zx ;
	*(wIy_pt + jp*Mp + ip) = zy ;
      }
    }
  }
}
コード例 #7
0
ファイル: bpclock.c プロジェクト: b/ION
static int	adjustThrottles()
{
	PsmPartition	ionwm = getIonwm();
	IonVdb		*ionvdb = getIonVdb();
	BpVdb		*bpvdb = getBpVdb();
	PsmAddress	elt;
	VOutduct	*outduct;
	unsigned long	nodeNbr;
	IonNeighbor	*neighbor;
	PsmAddress	nextElt;
	int		delta;
	VInduct		*induct;

	/*	Only the LTP induct and outduct throttles can be
	 *	dynamically adjusted in response to changes in data
	 *	rate between the local node and its neighbors, because
	 *	(currently) there is no mechanism for mapping neighbor
	 *	node number to duct name for any other CL protocol.
	 *	For LTP, duct name is LTP engine number which, by
	 *	convention, is identical to BP node number.  For all
	 *	other CL protocols, duct nominal data rate is initially
	 *	set to the protocol's configured nominal data rate and
	 *	is never subsequently modified.
	 *	
	 *	So, first we find the LTP induct if any.		*/

	for (elt = sm_list_first(ionwm, bpvdb->inducts); elt;
			elt = sm_list_next(ionwm, elt))
	{
		induct = (VInduct *) psp(ionwm, sm_list_data(ionwm, elt));
		if (strcmp(induct->protocolName, "ltp") == 0)
		{
			break;	/*	Found the LTP induct.		*/
		}
	}

	if (elt == 0)		/*	No LTP induct; nothing to do.	*/
	{
		return 0;
	}

	/*	Now update all LTP outducts, and the induct as well,
	 *	inferring the existence of Neighbors in the process.	*/

	for (elt = sm_list_first(ionwm, bpvdb->outducts); elt;
			elt = sm_list_next(ionwm, elt))
	{
		outduct = (VOutduct *) psp(ionwm, sm_list_data(ionwm, elt));
		if (strcmp(outduct->protocolName, "ltp") != 0)
		{
			continue;
		}

		nodeNbr = atol(outduct->ductName);
		neighbor = findNeighbor(ionvdb, nodeNbr, &nextElt);
		if (neighbor == NULL)
		{
			neighbor = addNeighbor(ionvdb, nodeNbr, nextElt);
			if (neighbor == NULL)
			{
				putErrmsg("Can't adjust outduct throttle.",
						NULL);
				return -1;
			}
		}

		if (neighbor->xmitRate != neighbor->prevXmitRate)
		{
#ifndef ION_NOSTATS
			if (neighbor->nodeNbr != getOwnNodeNbr())
			{
			/*	We report and clear transmission
			 *	statistics as necessary.  NOTE that
			 *	this procedure is based on the
			 *	assumption that the local node is
			 *	in LTP transmission contact with
			 *	AT MOST ONE neighbor at any time.
			 *	For more complex topologies it will
			 *	need to be redesigned.			*/

				if (neighbor->xmitRate == 0)
				{
					/*	End of xmit contact.	*/
					reportAllStateStats();
					clearAllStateStats();
				}
				else if (neighbor->prevXmitRate == 0)
				{
					/*	Start of xmit contact.	*/
					reportAllStateStats();
					clearAllStateStats();
				}
			}
#endif
			outduct->xmitThrottle.nominalRate = neighbor->xmitRate;
			neighbor->prevXmitRate = neighbor->xmitRate;
		}

		/*	Note that the LTP induct is aggregate; the
		 *	duct's nominal rate is the sum of the rates
		 *	at which all neighbors are expected to be
		 *	transmitting to the local node at any given
		 *	moment.  So we must add the change in rate
		 *	for each known neighbor to the aggregate
		 *	nominal reception rate for the induct.		*/

		if (neighbor->recvRate != neighbor->prevRecvRate)
		{
#ifndef ION_NOSTATS
			if (neighbor->nodeNbr != getOwnNodeNbr())
			{
			/*	We report and clear reception
			 *	statistics as necessary.  NOTE that
			 *	this procedure is based on the
			 *	assumption that the local node is
			 *	in LTP reception contact with
			 *	AT MOST ONE neighbor at any time.
			 *	For more complex topologies it will
			 *	need to be redesigned.			*/

				if (neighbor->recvRate == 0)
				{
					/*	End of recv contact.	*/
					reportAllStateStats();
					clearAllStateStats();
				}
				else if (neighbor->prevRecvRate == 0)
				{
					/*	Start of recv contact.	*/
					reportAllStateStats();
					clearAllStateStats();
				}
			}
#endif
			delta = neighbor->recvRate - neighbor->prevRecvRate;
			induct->acqThrottle.nominalRate += delta;
			neighbor->prevRecvRate = neighbor->recvRate;
		}
	}

	return 0;
}
コード例 #8
0
ファイル: ltpclock.c プロジェクト: b/ION
static int	manageLinks(Sdr sdr, time_t currentTime)
{
	PsmPartition	ionwm = getIonwm();
	LtpVdb		*ltpvdb = getLtpVdb();
	IonVdb		*ionvdb = getIonVdb();
	PsmAddress	elt;
	LtpVspan	*vspan;
	Object		obj;
	LtpSpan		span;
	IonNeighbor	*neighbor;
	PsmAddress	nextElt;
	unsigned long	priorXmitRate;

	sdr_begin_xn(sdr);
	for (elt = sm_list_first(ionwm, ltpvdb->spans); elt;
			elt = sm_list_next(ionwm, elt))
	{
		vspan = (LtpVspan *) psp(ionwm, sm_list_data(ionwm, elt));

		/*	Finish aggregation as necessary.		*/

		obj = sdr_list_data(sdr, vspan->spanElt);
		sdr_stage(sdr, (char *) &span, obj, sizeof(LtpSpan));
		if (span.lengthOfBufferedBlock > 0)
		{
			span.ageOfBufferedBlock++;
			sdr_write(sdr, obj, (char *) &span, sizeof(LtpSpan));
			if (span.ageOfBufferedBlock >= span.aggrTimeLimit)
			{
				sm_SemGive(vspan->bufFullSemaphore);
			}
		}

		/*	Find Neighbor object encapsulating the current
		 *	known state of this LTP engine.			*/

		neighbor = findNeighbor(ionvdb, vspan->engineId, &nextElt);
		if (neighbor == NULL)
		{
			neighbor = addNeighbor(ionvdb, vspan->engineId,
					nextElt);
			if (neighbor == NULL)
			{
				putErrmsg("Can't update span.", NULL);
				return -1;
			}
		}

		if (neighbor->xmitRate == 0)
		{
			if (vspan->localXmitRate > 0)
			{
				vspan->localXmitRate = 0;
				ltpStopXmit(vspan);
			}
		}
		else
		{
			if (vspan->localXmitRate == 0)
			{
				vspan->localXmitRate = neighbor->xmitRate;
				ltpStartXmit(vspan);
			}
		}

		if (neighbor->fireRate == 0)
		{
			if (vspan->remoteXmitRate > 0)
			{
				priorXmitRate = vspan->remoteXmitRate;
				vspan->remoteXmitRate = 0;
				if (ltpSuspendTimers(vspan, elt, currentTime,
						priorXmitRate))
				{
					putErrmsg("Can't manage links.", NULL);
					return -1;
				}
			}
		}
		else
		{
			if (vspan->remoteXmitRate == 0)
			{
				vspan->remoteXmitRate = neighbor->fireRate;
				if (ltpResumeTimers(vspan, elt, currentTime,
						vspan->remoteXmitRate))
				{
					putErrmsg("Can't manage links.", NULL);
					return -1;
				}
			}
		}

		if (neighbor->recvRate == 0)
		{
			vspan->receptionRate = 0;
		}
		else
		{
			vspan->receptionRate = neighbor->recvRate;
		}

		if (neighbor->owltInbound != vspan->owltInbound)
		{
			vspan->owltInbound = neighbor->owltInbound;
		}

		if (neighbor->owltOutbound != vspan->owltOutbound)
		{
			vspan->owltOutbound = neighbor->owltOutbound;
		}
	}

	if (sdr_end_xn(sdr) < 0)
	{
		putErrmsg("ltpclock failed managing links.", NULL);
		return -1;
	}

	return 0;
}
コード例 #9
0
ファイル: AStarPlanner.cpp プロジェクト: kl414/SteerLite
 bool AStarPlanner::computePath(std::vector<Util::Point>& agent_path,  Util::Point start, Util::Point goal, SteerLib::GridDatabase2D * _gSpatialDatabase, bool append_to_path)
 {
     gSpatialDatabase = _gSpatialDatabase;
     
     //TODO
     std::cout<<"\nIn A*\n";
     //coordinate for start & goal in the cell
     int startIndex = gSpatialDatabase->getCellIndexFromLocation(start);
     gSpatialDatabase->getLocationFromIndex(startIndex, start);
     int goalIndex = gSpatialDatabase->getCellIndexFromLocation(goal);
     gSpatialDatabase->getLocationFromIndex(goalIndex, goal);
     
     std::vector<AStarPlannerNode> closedset;
     std::vector<AStarPlannerNode> openset;
     std::vector<AStarPlannerNode> neighbors;
     std::map<AStarPlannerNode, AStarPlannerNode> came;
     
     AStarPlannerNode startNode = AStarPlannerNode(start, 0, 0, NULL);
     startNode.startNode = 1;
     startNode.g = 0;
     startNode.f = startNode.g + W * heuristic(start, goal);
     
     openset.push_back(startNode);
     
     AStarPlannerNode curr;
     int count = 0;
     
     while(!openset.empty()){
         count++;
         //get Lowest F
         int index = 0;
         curr = openset[0];
         for(int i = 0; i < openset.size(); i++){
             if( openset[i] < curr){
                 curr = openset[i];
                 index = i;
             }
             
             if( openset[i].f == curr.f){
                 if(openset[i].g > curr.g){
                     curr = openset[i];
                     index = i;
                 }
             }
             
         }
         if(curr.point == goal){
             agent_path.push_back(curr.point);
             while(curr.startNode != 1){
                 for(AStarPlannerNode node: closedset){
                     if(node.point.x == curr.parentx && node.point.z == curr.parenty){
                         curr = node;
                         agent_path.push_back(curr.point);
                         break;
                     }
                 }
             }
             std::cout << "path:" << agent_path.size() <<std::endl;
             std::cout << "expanded node:" << closedset.size()+1 <<std::endl;
             std::reverse(agent_path.begin(), agent_path.end());
             return true;
         }
         
         openset.erase(openset.begin() + index);
         closedset.push_back(curr);
         
         neighbors = findNeighbor(curr.point, gSpatialDatabase);
         for(AStarPlannerNode neighbor: neighbors){
             if(closed(neighbor, closedset)){
                 continue;
             }
             
             double temp_g = curr.g + euclidean(curr.point, neighbor.point);
             
             
             
             if(temp_g < neighbor.g){
                 neighbor.g = temp_g;
                 neighbor.f = neighbor.g + W * heuristic(neighbor.point, goal);
                 neighbor.parentx = curr.point.x;
                 neighbor.parenty = curr.point.z;
                 if(!opened(neighbor, openset)){
                     openset.push_back(neighbor);
                 }
             }
         }
     }
     return false;
 }
コード例 #10
0
std::unique_ptr<StepGenerator> NnRolloutFactory::createGenerator(SearchStatus &status,
        HistoryEntry const *entry, State const */*state*/, HistoricalData const */*data*/) {
    // Find a neighbor, and use it to make a new generator.
    BeliefNode *neighbor = findNeighbor(entry->getAssociatedBeliefNode());
    return std::make_unique<NnRolloutGenerator>(status, solver_, neighbor);
}
コード例 #11
0
void
mexFunction(int nout, mxArray *out[], 
            int nin, const mxArray *in[])
{
  enum { X=0,Y,I,iwXp,iwYp } ;
  enum { wI=0, wIx, wIy } ;

  int M, N, Mp, Np, ip, jp ;
  double 
    *X_pt, 
    *Y_pt, 
    *I_pt, 
    *iwXp_pt, 
    *iwYp_pt, 
    *wI_pt, 
    *wIx_pt   = 0,
    *wIy_pt   = 0 ;

  double Xmin, Xmax, Ymin, Ymax ;
  const double NaN = mxGetNaN() ;
  
  /* -----------------------------------------------------------------
   *                                               Check the arguments
   * -------------------------------------------------------------- */
  if(nin != 5) {
    mexErrMsgTxt("Five input argumets required.") ;
  }

  if (nout > 3) {
    mexErrMsgTxt("Too many output arguments") ;
  }
  
  if(!uIsRealMatrix(in[I], -1, -1)) {
    mexErrMsgTxt("I must be a real matrix of class DOUBLE") ;
  }
  
  if(!uIsRealMatrix(in[iwXp], -1, -1)) {
    mexErrMsgTxt("iwXp must be a real matrix") ;
  }
  
  M = getM(I) ;
  N = getN(I) ;
  Mp = getM(iwXp) ;
  Np = getN(iwXp) ;

  if(!uIsRealMatrix(in[iwYp], Mp, Np)) {
      mexErrMsgTxt
	  ("iwXp and iwYp must be a real matrices of the same dimension") ;
  }

  if(!uIsRealVector(in[X],N) || !uIsRealVector(in[Y],M)) {
      mexErrMsgTxt
	  ("X and Y must be vectors of the same dimensions "
	   "of the columns/rows of I, respectivelye") ;
  }

  X_pt = getPr(X); 
  Y_pt = getPr(Y) ;
  I_pt = getPr(I) ;
  iwXp_pt = getPr(iwXp) ;
  iwYp_pt = getPr(iwYp) ;

  /* Allocate the result. */
  out[wI] = mxCreateDoubleMatrix(Mp, Np, mxREAL) ;
  wI_pt = mxGetPr(out[wI]) ;

  if (nout > 1) {
    out[wIx] = mxCreateDoubleMatrix(Mp, Np, mxREAL) ;
    out[wIy] = mxCreateDoubleMatrix(Mp, Np, mxREAL) ;
    wIx_pt = mxGetPr (out[wIx]) ;
    wIy_pt = mxGetPr (out[wIy]) ;
  }

  /* -----------------------------------------------------------------
   *                                                        Do the job
   * -------------------------------------------------------------- */
  Xmin = X_pt [0] ;
  Xmax = X_pt [N - 1] ;
  Ymin = Y_pt [0] ;
  Ymax = Y_pt [M - 1] ;

  if (nout <= 1) {

    /* optimized for only image output */
    for(jp = 0 ; jp < Np ; ++jp) {
      for(ip = 0 ; ip < Mp ; ++ip) {    
	/* Search for the four neighbors of the backprojected point. */     
	double x = *iwXp_pt++ ;
	double y = *iwYp_pt++ ;
	double z = NaN ;
	
	/* This messy code allows the identity transformation
	 * to be processed as expected. */
	if(x >= Xmin && x <= Xmax &&
	   y >= Ymin && y <= Ymax) {
	  int j = findNeighbor(x, X_pt, N) ;
	  int i = findNeighbor(y, Y_pt, M) ;
	  double* pt  = I_pt + j*M + i ;
	  
	  /* Weights. */
	  double x0 = X_pt[j] ;
	  double x1 = X_pt[j+1] ;
	  double y0 = Y_pt[i] ;
	  double y1 = Y_pt[i+1] ;
	  double wx = (x-x0)/(x1-x0) ;
	  double wy = (y-y0)/(y1-y0) ;
	  
	  /* Load all possible neighbors. */
	  double z00 = 0.0 ;
	  double z10 = 0.0 ;
	  double z01 = 0.0 ;
	  double z11 = 0.0 ;

	  if(j > -1) {
	    if(i > -1 ) z00 = *pt ;
	    pt++ ;
	    if(i < M-1) z10 = *pt ;
	  } else {
	    pt++ ;
	  }
	  
	  pt += M - 1;
	  
	  if(j < N-1) {
	    if(i > -1 ) z01 = *pt ; 
	    pt++ ;
	    if(i < M-1) z11 = *pt ;
	  }       
	  
	  /* Bilinear interpolation. */
	  z = 
	    (1 - wy) * ((1-wx) * z00 + wx * z01) +
	    (    wy) * ((1-wx) * z10 + wx * z11) ; 
	}
	
	*(wI_pt + jp*Mp + ip) = z ;
      }
    }
  }
  
  /* do also the derivatives */
  else {

    /* optimized for only image output */
    for(jp = 0 ; jp < Np ; ++jp) {
      for(ip = 0 ; ip < Mp ; ++ip) {    
	/* Search for the four neighbors of the backprojected point. */     
	double x = *iwXp_pt++ ;
	double y = *iwYp_pt++ ;
	double z = NaN, zx = NaN, zy = NaN ;
	
	/* This messy code allows the identity transformation
	 * to be processed as expected. */
	if(x >= Xmin && x <= Xmax &&
	   y >= Ymin && y <= Ymax) {
	  int j = findNeighbor(x, X_pt, N) ;
	  int i = findNeighbor(y, Y_pt, M) ;
	  double* pt  = I_pt + j*M + i ;
	  
	  /* Weights. */
	  double x0 = X_pt[j] ;
	  double x1 = X_pt[j+1] ;
	  double y0 = Y_pt[i] ;
	  double y1 = Y_pt[i+1] ;
	  double wx = (x-x0)/(x1-x0) ;
	  double wy = (y-y0)/(y1-y0) ;
	  
	  /* Load all possible neighbors. */
	  double z00 = 0.0 ;
	  double z10 = 0.0 ;
	  double z01 = 0.0 ;
	  double z11 = 0.0 ;
	  
	  if(j > -1) {
	    if(i > -1 ) z00 = *pt ;
	    pt++ ;
	    if(i < M-1) z10 = *pt ;
	  } else {
	    pt++ ;
	  }
	  
	  pt += M - 1;
	  
	  if(j < N-1) {
	    if(i > -1 ) z01 = *pt ; 
	    pt++ ;
	    if(i < M-1) z11 = *pt ;
	  }       
	  
	  /* Bilinear interpolation. */
	  z = 
	    (1-wy)*( (1-wx) * z00 + wx * z01) +
	    wy*( (1-wx) * z10 + wx * z11) ;
	  
	  zx = 
	    (1-wy) * (z01 - z00) +
	       wy  * (z11 - z10) ;

	  zy = 
	    (1-wx) * (z10 - z00) +
	       wx  * (z11 - z01) ;
	}
	
	*(wI_pt  + jp*Mp + ip) = z ;
	*(wIx_pt + jp*Mp + ip) = zx ;
	*(wIy_pt + jp*Mp + ip) = zy ;
      }
    }
  }
}
コード例 #12
0
ファイル: rfx.c プロジェクト: michirod/cgr-jni
static void	deleteContact(PsmAddress cxaddr)
{
	Sdr		sdr = getIonsdr();
	PsmPartition	ionwm = getIonwm();
	IonVdb 		*vdb = getIonVdb();
	time_t		currentTime = getUTCTime();
	IonCXref	*cxref;
	Object		obj;
	IonEvent	event;
	IonNeighbor	*neighbor;
	PsmAddress	nextElt;

	cxref = (IonCXref *) psp(ionwm, cxaddr);


	/*	Delete contact events from timeline.			*/

	event.ref = cxaddr;
	if (cxref->startXmit)
	{
		event.time = cxref->startXmit;
		event.type = IonStartXmit;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	if (cxref->stopXmit)
	{
		event.time = cxref->stopXmit;
		event.type = IonStopXmit;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	if (cxref->startFire)
	{
		event.time = cxref->startFire;
		event.type = IonStartFire;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	if (cxref->stopFire)
	{
		event.time = cxref->stopFire;
		event.type = IonStopFire;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	if (cxref->startRecv)
	{
		event.time = cxref->startRecv;
		event.type = IonStartRecv;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	if (cxref->stopRecv)
	{
		event.time = cxref->stopRecv;
		event.type = IonStopRecv;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	if (cxref->purgeTime)
	{
		event.time = cxref->purgeTime;
		event.type = IonPurgeContact;
		sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
				&event, rfx_erase_data, NULL);
	}

	/*	Apply to current state as necessary.			*/

	if (currentTime >= cxref->startXmit && currentTime <= cxref->stopXmit)
	{
		neighbor = findNeighbor(vdb, cxref->toNode, &nextElt);
		if (neighbor)
		{
			neighbor->xmitRate = 0;
		}
	}

	if (currentTime >= cxref->startFire && currentTime <= cxref->stopFire)
	{
		neighbor = findNeighbor(vdb, cxref->fromNode, &nextElt);
		if (neighbor)
		{
			neighbor->fireRate = 0;
		}
	}

	if (currentTime >= cxref->startRecv && currentTime <= cxref->stopRecv)
	{
		neighbor = findNeighbor(vdb, cxref->fromNode, &nextElt);
		if (neighbor)
		{
			neighbor->recvRate = 0;
		}
	}

	/*	Delete contact from index.				*/

	if (cxref->toTime > currentTime)	/*	Affects routes.	*/
	{
		vdb->lastEditTime = currentTime;
	}

	sm_rbt_delete(ionwm, vdb->contactIndex, rfx_order_contacts, cxref,
			rfx_erase_data, NULL);

	/*	Delete contact from non-volatile database.		*/

	obj = sdr_list_data(sdr, cxref->contactElt);
	sdr_list_delete(sdr, cxref->contactElt, NULL, NULL);
	sdr_free(sdr, obj);
}
コード例 #13
0
ファイル: rfx.c プロジェクト: michirod/cgr-jni
static void	deleteRange(PsmAddress rxaddr, int conditional)
{
	Sdr		sdr = getIonsdr();
	PsmPartition	ionwm = getIonwm();
	IonVdb 		*vdb = getIonVdb();
	time_t		currentTime = getUTCTime();
	IonRXref	*rxref;
	Object		obj;
	IonEvent	event;
	IonNeighbor	*neighbor;
	PsmAddress	nextElt;

	rxref = (IonRXref *) psp(ionwm, rxaddr);

	/*	Delete range from non-volatile database.		*/

	if (rxref->rangeElt)		/*	An asserted range.	*/
	{
		if (conditional)	/*	Delete only if imputed.	*/
		{
			return;		/*	Retain asserted range.	*/
		}

		/*	Unconditional deletion; remove range from DB.	*/

		obj = sdr_list_data(sdr, rxref->rangeElt);
		sdr_free(sdr, obj);
		sdr_list_delete(sdr, rxref->rangeElt, NULL, NULL);
	}

	/*	Delete range events from timeline.			*/

	event.ref = rxaddr;
	event.time = rxref->fromTime;
	if (rxref->rangeElt)
	{
		event.type = IonStartAssertedRange;
	}
	else
	{
		event.type = IonStartImputedRange;
	}

	sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
			&event, rfx_erase_data, NULL);
	event.time = rxref->toTime;
	if (rxref->rangeElt)
	{
		event.type = IonStopAssertedRange;
	}
	else
	{
		event.type = IonStopImputedRange;
	}

	sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
			&event, rfx_erase_data, NULL);

	/*	Apply to current state as necessary.			*/

	if (currentTime >= rxref->fromTime && currentTime <= rxref->toTime)
	{
		if (rxref->fromNode == getOwnNodeNbr())
		{
			neighbor = findNeighbor(vdb, rxref->toNode, &nextElt);
			if (neighbor)
			{
				neighbor->owltOutbound = 0;
			}
		}

		if (rxref->toNode == getOwnNodeNbr())
		{
			neighbor = findNeighbor(vdb, rxref->fromNode, &nextElt);
			if (neighbor)
			{
				neighbor->owltInbound = 0;
			}
		}
	}

	/*	Delete range from index.				*/

	if (rxref->toTime > currentTime)	/*	Affects routes.	*/
	{
		vdb->lastEditTime = currentTime;
	}

	sm_rbt_delete(ionwm, vdb->rangeIndex, rfx_order_ranges, rxref,
			rfx_erase_data, NULL);
}