/******************************************************************************
* /// 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;
}
/**
* @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;
}
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;
*/
}
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;
}
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();
}
}
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;
}
}
}
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;
}
}
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;
}
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);
}
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];
}
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);
}
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;
}
}
}
}
