Ejemplo n.º 1
0
/******************************************************************************
* /// Determines whether the given tetrahedral cell is a ghost cell (or an invalid cell).
******************************************************************************/
bool DelaunayTessellation::isGhostCell(CellHandle cell) const
{
	// Find head vertex with the lowest index.
	const auto& p0 = cell->vertex(0)->point();
	int headVertex = p0.index();
	if(headVertex == -1) {
		OVITO_ASSERT(!isValidCell(cell));
		return true;
	}
	bool isGhost = p0.isGhost();
	for(int v = 1; v < 4; v++) {
		const auto& p = cell->vertex(v)->point();
		if(p.index() == -1) {
			OVITO_ASSERT(!isValidCell(cell));
			return true;
		}
		if(p.index() < headVertex) {
			headVertex = p.index();
			isGhost = p.isGhost();
		}
	}

	OVITO_ASSERT(isValidCell(cell));
	return isGhost;
}
Ejemplo n.º 2
0
Archivo: bfs_3d.cpp Proyecto: ana-GT/NS
/**
 * @function getDist
 */
int BFS3D::getDist(int x, int y, int z)
{
    int idx = xyzToIndex(x,y,z);
    int d = dist_[idx];
    while( d==INFINITE_COST && !q_->empty() ) {
        int x,y,z;
        q_->remove(&x,&y,&z);

        bool onBoundary = x==0 || x==dimX_-1 || y==0 || y==dimY_-1 || z==0 || z==dimZ_-1;

        int parentDist = dist_[xyzToIndex(x,y,z)];
        for(int i=0; i<DIRECTIONS3D; i++) {
            int newX = x+dx[i];
            int newY = y+dy[i];
            int newZ = z+dz[i];
            if((!onBoundary || (newX>=0 && newX<dimX_ && newY>=0 && newY<dimY_ && newZ>=0 && newZ<dimZ_)) && //check if the successor is in work space
                    isValidCell(newX,newY,newZ) && //is not an obstacle
                    (dist_[xyzToIndex(newX,newY,newZ)] == INFINITE_COST)) { //and has not already been put in the queue
                q_->insert(newX,newY,newZ);
                dist_[xyzToIndex(newX,newY,newZ)] = parentDist + cost_1_move_;
            }
        }

        d = dist_[idx];
    }
    return d;
}
 void HeightmapTimeAccumulation::accumulate(
   const sensor_msgs::Image::ConstPtr& msg)
 {
   boost::mutex::scoped_lock lock(mutex_);
   if (!config_) {
     JSK_NODELET_ERROR("no ~input/config is yet available");
     return;
   }
   tf::StampedTransform tf_transform;
   tf_->lookupTransform(fixed_frame_id_, center_frame_id_,
                        msg->header.stamp,
                        tf_transform);
   Eigen::Affine3f from_center_to_fixed;
   tf::transformTFToEigen(tf_transform, from_center_to_fixed);
   cv::Mat new_heightmap = cv_bridge::toCvShare(
     msg, sensor_msgs::image_encodings::TYPE_32FC1)->image;
   // Transform prev_cloud_ to current frame
   Eigen::Affine3f from_prev_to_current
     = prev_from_center_to_fixed_.inverse() * from_center_to_fixed;
   pcl::PointCloud<pcl::PointXYZ> transformed_pointcloud;
   pcl::transformPointCloud(prev_cloud_, transformed_pointcloud, from_prev_to_current.inverse());
   for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
     pcl::PointXYZ p = transformed_pointcloud.points[i];
     if (isValidPoint(p)) {
       cv::Point index = toIndex(p, new_heightmap);
       if (isValidIndex(index, new_heightmap)) {
         if (!isValidCell(index, new_heightmap)) {
           new_heightmap.at<float>(index.y, index.x) = p.z;
         }
       }
     }
   }
   publishHeightmap(new_heightmap, msg->header);
   prev_from_center_to_fixed_ = from_center_to_fixed;
 }
Ejemplo n.º 4
0
bool ClsTopologySparse::isValidIndex(const list<tiPoint >& _lst, int /* _iWidth */, int iIndex) {
#ifdef DEBUG_CLSTOPOLOGYSPARSE
    cout << "ClsTopologySparse::isValidIndex(const list<tiPoint >& _lst, int /* _iWidth */, int iIndex)" << endl;
#endif

    tiPoint p = index2pos(iIndex);
    return isValidCell(_lst, p.first, p.second);

/* old way
   int _iX;
   int _iY;

   _iY = iIndex / _iWidth;
   _iX = iIndex + 1 - _iWidth*_iY + _iWidth;

   list<tiPoint >::iterator it;
   for(it=_lst.begin(); it!=_lst.end();it++){
   int iX = (*it).first;
   int iY = (*it).second;
   if(_iX == iX && _iY == iY){
   return true;
   }
   }
   return false;
*/
}
Ejemplo n.º 5
0
list<tIndexDist> ClsTopologySparse::getCellsForArborization(double fXCenter, double fYCenter, ClsBaseArborization *clsBaseArborization) {
#ifdef DEBUG_CLSTOPOLOGYSPARSE
    cout << "ClsTopologySparse::getCellsForArborization(double fXCenter, double fYCenter, ClsBaseArborization *clsBaseArborization)" << endl;
#endif
    list<tIndexDist> lst;
    list<pair<int, int> > lstPointsLocal;

    lstPointsLocal = ClsBaseTopology::getCellsForArborizationRect(fXCenter, fYCenter, clsBaseArborization);

    /* calculate distance */
    int iGroupHeight = 0;
    list<tiPoint >::iterator it;
    for(it=lstPoints.begin(); it!=lstPoints.end();it++){
	int iY = (*it).second;
	iGroupHeight = (iY > iGroupHeight ? iY : iGroupHeight);
    }

    for(it=lstPointsLocal.begin();it!=lstPointsLocal.end();it++){
	int iX = (*it).first;
	int iY = (*it).second;
	/* only use points within the group boundaries */
	if(isValidCell(iX, iY) && iX>0 && iX<=iGroupWidth && iY>0 && iY<=iGroupHeight){
	    double fDist = calculateDistance(fXCenter, fYCenter, iX, iY);
	    int iIndex = pos2index(iX, iY);
	    tIndexDist tid(iIndex, fDist);
	    lst.push_back(tid);
	}
    }
    return lst;
}
Ejemplo n.º 6
0
void ClsQStateArrayViewSparse::addValue( double fValue, int i, int j ) {
    if(isValidCell(i, j)) {
//	cout << "valid cell: " << i << ", " << j << endl;
        vCells[i][j] += fValue;
        QBrush   qb(Qt::yellow);
        QPainter paintPM(&pmGridValues );
        qb.setColor(getColor(fValue));
//	qDrawPlainRect (&paint, index2pos2( i ), index2pos2( j ), iCellSize -1, iCellSize - 1, black, 0, &qb);
        qDrawPlainRect (&paintPM, index2pos2( i )-BORDER, index2pos2( j )-BORDER, iCellSize -1, iCellSize - 1, Qt::black, 0, &qb);
        paintPM.end();
    }
}
Ejemplo n.º 7
0
void day(cell ** board) {

	int offset[8][2] = {
		{ -1, -1 },
		{ -1, 0 },
		{ -1, 1 },
		{ 0, -1 },
		{ 0, 1 },
		{ 1, -1 },
		{ 1, 0 },
		{ 1, 1 }
	};

	for (int i = 0; i < height; i++)
		for (int j = 0; j < width; j++)
		{

			int count = 0;

			for (int k = 0; k < 8; k++)
			{
				int x = i + offset[k][0];
				int y = j + offset[k][1];
				if (isValidCell(x, y, height, width))
				{
					if (board[x][y].alive)
						count ++;
				}
			}

			board[i][j].numLivingNeighbors = count;
		}

	for (int i = 0; i < height; i++)
		for (int j = 0; j < width; j++)
		{
			/* Stays alive */
			if (board[i][j].alive) {
				if (!(board[i][j].numLivingNeighbors == 2 || board[i][j].numLivingNeighbors == 3))
					board[i][j].alive = false;
			}

			/* Born */
			else {
				if (board[i][j].numLivingNeighbors == 3)
					board[i][j].alive = true;
			}
		}
}
Ejemplo n.º 8
0
void MapWidget::mouseReleaseEvent(QMouseEvent *event)
{
    switch(event->button()) {
    case Qt::MidButton:
        mViewportPos += mDragOffset;
        mDragOffset.rx() = 0;
        mDragOffset.ry() = 0;
        break;

    case Qt::RightButton:
    {
        finishSpecialMode(false);
        QPoint tmp = getCellUnderMouse(event->localPos());

        if (mSelectionEnd) {
            delete mSelectionEnd;
            mSelectionEnd = NULL;
        }

        if (mSelectionBegin) {
            if (!isValidCell(tmp) && tmp == *mSelectionBegin) {
                delete mSelectionBegin;
                mSelectionBegin = NULL;
                emit cellDeselected();
            } else {
                mSelectionEnd = new QPoint;
                *mSelectionEnd = tmp;
                clipCellCoord(*mSelectionBegin);
                clipCellCoord(*mSelectionEnd);
                emit cellSelected();
            }
            update();
        }
        break;
    }

    case Qt::LeftButton:
        finishSpecialMode(true);
        break;

    default:
        break;
    }
}
Ejemplo n.º 9
0
list<tiPoint > ClsTopologySparse::checkList(const list<tiPoint >& lstIn) {
#ifdef DEBUG_CLSTOPOLOGYSPARSE
    cout << "ClsTopologySparse::checkList(const list<tiPoint >& lstIn)" << endl;
#endif

    list<tiPoint > lst;
    list<tiPoint >::iterator it;
    for(it=lstPoints.begin(); it!=lstPoints.end();it++){
	int iX = (*it).first;
	int iY = (*it).second;
	if(isValidCell(lstIn, iX, iY)){
	    tiPoint pairTemp(iX, iY);
	    lst.push_back(pairTemp);
	}
    }

    lst.sort(pointCompareSparse(iGroupWidth));
    return lst;
}
Ejemplo n.º 10
0
bool ClsTopologySparse::isValidCell(int _iX, int _iY) {
#ifdef DEBUG_CLSTOPOLOGYSPARSE
    cout << "ClsTopologySparse::isValidCell(int _iX, int _iY)" << endl;
#endif

/* old way
   list<tiPoint >::iterator it;
   for(it=lstPoints.begin(); it!=lstPoints.end();it++){
   int iX = (*it).first;
   int iY = (*it).second;
   if(_iX == iX && _iY == iY){
   return true;
   }
   }
   return false;
*/

    return isValidCell(lstPoints, _iX, _iY);

}
Ejemplo n.º 11
0
int findLongestLengthDP(int heightArr[][MAX_SIZE], int numRow, int numCol, int row, int col, int longestLengthMemo[][MAX_SIZE])
{
	if(longestLengthMemo[row][col] > 0)
		return longestLengthMemo[row][col];

	int nextRow, nextCol, maxLength = 0, length, direction;

	for(direction = 0; direction < NUM_DIRECTION; direction++)
	{
		nextRow = row + rowOffset[direction];
		nextCol = col + colOffset[direction];

		if(isValidCell(nextRow, nextCol, numRow, numCol) &&
		   heightArr[nextRow][nextCol] < heightArr[row][col])
		{
			length = findLongestLengthDP(heightArr, numRow, numCol, nextRow, nextCol, longestLengthMemo);
			if(maxLength < length)
				maxLength = length;
		}
	}

	longestLengthMemo[row][col] = 1 + maxLength;
	return longestLengthMemo[row][col];
}
Ejemplo n.º 12
0
void MapWidget::paintEvent(QPaintEvent *)
{
    QPainter painter(this);
    QRectF rectf(0, 0, width(), height());
    QPointF vpTopLeft = mViewportPos + mDragOffset;

    painter.setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform);

    painter.fillRect(rectf, Qt::black);

    painter.save();
    painter.translate(vpTopLeft);
    painter.scale(mScale, mScale);

    if (mTileSize.isValid()) {
        int i, j, tile_indx;
        QPoint visAreaBeg = getCellUnderMouse(QPoint(0,0)); // TODO: this is just viewport coords / width-height!
        clipCellCoord(visAreaBeg);
        QPoint visAreaEnd = getCellUnderMouse(visAreaBeg + QPoint(this->width(), this->height()));
        clipCellCoord(visAreaEnd);

        for(i = visAreaBeg.x(); i <= visAreaEnd.x(); ++i) {
            for(j = visAreaBeg.y(); j <= visAreaEnd.y(); ++j) {
                tile_indx = mCells.at(i + j * mCols);
                if (tile_indx >= 0)
                    painter.drawImage(i * mTileSize.width(), j * mTileSize.height(), mTiles[tile_indx].im);
            }
        }

        // highlight cursor
        switch (mEditMode) {
        case NORMAL:
            if (mSelectionBegin && !mSelectionEnd) {
                QRect frame = getSelectedArea(*mSelectionBegin, mCellUnderMouse);
                painter.fillRect(
                    frame.left() * mTileSize.width(),
                    frame.top() * mTileSize.height(),
                    frame.width() * mTileSize.width(),
                    frame.height() * mTileSize.height(),
                    QColor(127, 127, 255, 50));
            } else if (isValidCell(mCellUnderMouse)) {
                int x = mCellUnderMouse.x() * mTileSize.width();
                int y = mCellUnderMouse.y() * mTileSize.height();
                painter.fillRect(x, y, mTileSize.width(), mTileSize.height(), QColor(127, 127, 255, 50));
            }
            break;

        case GRAB:
        case DUPLICATE:
            {
                QRect selArea = getSelectedArea(*mSelectionBegin, *mSelectionEnd);
                QRect frame(
                    mCellUnderMouse.x() - mGrabOrigin.x(),
                    mCellUnderMouse.y() - mGrabOrigin.y(),
                    selArea.width(),
                    selArea.height());
                painter.fillRect(
                    frame.left() * mTileSize.width(),
                    frame.top() * mTileSize.height(),
                    frame.width() * mTileSize.width(),
                    frame.height() * mTileSize.height(),
                    QColor(127, 127, 255, 50));
            }
            break;
        }

        // highlight selected
        if (mSelectionBegin && mSelectionEnd) {
            QRect frame = getSelectedArea(*mSelectionBegin, *mSelectionEnd);
            painter.fillRect(
                frame.left() * mTileSize.width(),
                frame.top() * mTileSize.height(),
                frame.width() * mTileSize.width(),
                frame.height() * mTileSize.height(),
                QColor(0, 255, 0, 75));
        }

        painter.setPen(Qt::red);
        painter.drawRect(-1, -1, mCols * mTileSize.width(), mRows * mTileSize.height());
    } else {
        painter.setPen(Qt::red);
        painter.drawLine(-10, 0, 10, 0);
        painter.drawLine(0, -10, 0, 10);
    }
    painter.restore();

    painter.setPen(Qt::green);
    painter.drawRect(rectf);
}
Ejemplo n.º 13
0
Archivo: bfs_3d.cpp Proyecto: ana-GT/NS
void BFS3D::search3DwithQueue(State3D*** statespace)
{
    State3D* ExpState;
    int newx, newy, newz;
    short unsigned int x,y,z;
    unsigned int g_temp;

    //these are added here temporarily. should be in the class
    int dx[DIRECTIONS3D] = { 1,  1,  1,  0,  0,  0, -1, -1, -1,    1,  1,  1,  0,  0, -1, -1, -1,    1,  1,  1,  0,  0,  0, -1, -1, -1};
    int dy[DIRECTIONS3D] = { 1,  0, -1,  1,  0, -1, -1,  0,  1,    1,  0, -1,  1, -1, -1,  0,  1,    1,  0, -1,  1,  0, -1, -1,  0,  1};
    int dz[DIRECTIONS3D] = {-1, -1, -1, -1, -1, -1, -1, -1, -1,    0,  0,  0,  0,  0,  0,  0,  0,    1,  1,  1,  1,  1,  1,  1,  1,  1};

    //create a queue
    queue<State3D*> Queue;

    //initialize to infinity all
    for (x = 0; x < dimX_; x++)
    {
        for (y = 0; y < dimY_; y++)
        {
            for (z = 0; z < dimZ_; z++)
            {
                dist_[xyzToIndex(x,y,z)] = INFINITE_COST;
                reInitializeState3D(&statespace[x][y][z]);
            }
        }
    }

    //initialization - throw starting states on queue with g cost = 0
    for(unsigned int i = 0; i < goal_.size(); ++i)
    {
        statespace[goal_[i][0]][goal_[i][1]][goal_[i][2]].g = 0;
        Queue.push(&statespace[goal_[i][0]][goal_[i][1]][goal_[i][2]]);
    }

    //expand all of the states
    while((int)Queue.size() > 0)
    {
        //get the state to expand
        ExpState = Queue.front();

        Queue.pop();

        //it may be that the state is already closed
        if(ExpState->iterationclosed == 1)
            continue;

        //close it
        ExpState->iterationclosed = 1;

        //set the corresponding distances to the goal
        dist_[xyzToIndex(ExpState->x, ExpState->y, ExpState->z)] = ExpState->g;

        //iterate through neighbors
        for(int d = 0; d < DIRECTIONS3D; d++)
        {
            newx = ExpState->x + dx[d];
            newy = ExpState->y + dy[d];
            newz = ExpState->z + dz[d];

            //make sure it is inside the map and has no obstacle
            if(0 > newx || newx >= dimX_ || 0 > newy || newy >= dimY_ || 0 > newz || newz >= dimZ_)
                continue;

            if(!isValidCell(newx,newy,newz))
                continue;

            if(statespace[newx][newy][newz].iterationclosed == 0)
            {
                //insert into the stack
                Queue.push(&statespace[newx][newy][newz]);

                //set the g-value
                if (ExpState->x != newx && ExpState->y != newy && ExpState->z != newz)
                    g_temp = ExpState->g + cost_sqrt3_move_;
                else if ((ExpState->y != newy && ExpState->z != newz) ||
                         (ExpState->x != newx && ExpState->z != newz) ||
                         (ExpState->x != newx && ExpState->y != newy))
                    g_temp = ExpState->g + cost_sqrt2_move_;
                else
                    g_temp = ExpState->g + cost_1_move_;

                if(statespace[newx][newy][newz].g > g_temp)
                    statespace[newx][newy][newz].g = g_temp;
            }
        }
    }
}