CustomDelegate::CustomDelegate(QTableView* tableView)
{
// create grid pen
int gridHint = tableView->style()->styleHint(QStyle::SH_Table_GridLineColor, new QStyleOptionViewItemV4());
QColor gridColor(gridHint);
_gridPen = QPen(gridColor, 0, tableView->gridStyle());
}
void Grid3DOverlay::render(RenderArgs* args) {
if (!_visible) {
return; // do nothing if we're not visible
}
const int MINOR_GRID_DIVISIONS = 200;
const int MAJOR_GRID_DIVISIONS = 100;
const float MAX_COLOR = 255.0f;
// center the grid around the camera position on the plane
glm::vec3 rotated = glm::inverse(getRotation()) * Application::getInstance()->getCamera()->getPosition();
float spacing = _minorGridWidth;
float alpha = getAlpha();
xColor color = getColor();
glm::vec4 gridColor(color.red / MAX_COLOR, color.green / MAX_COLOR, color.blue / MAX_COLOR, alpha);
auto batch = args->_batch;
if (batch) {
Transform transform;
transform.setRotation(getRotation());
// Minor grid
{
batch->_glLineWidth(1.0f);
auto position = glm::vec3(_minorGridWidth * (floorf(rotated.x / spacing) - MINOR_GRID_DIVISIONS / 2),
spacing * (floorf(rotated.y / spacing) - MINOR_GRID_DIVISIONS / 2),
getPosition().z);
float scale = MINOR_GRID_DIVISIONS * spacing;
transform.setTranslation(position);
transform.setScale(scale);
batch->setModelTransform(transform);
DependencyManager::get<GeometryCache>()->renderGrid(*batch, MINOR_GRID_DIVISIONS, MINOR_GRID_DIVISIONS, gridColor);
}
// Major grid
{
batch->_glLineWidth(4.0f);
spacing *= _majorGridEvery;
auto position = glm::vec3(spacing * (floorf(rotated.x / spacing) - MAJOR_GRID_DIVISIONS / 2),
spacing * (floorf(rotated.y / spacing) - MAJOR_GRID_DIVISIONS / 2),
getPosition().z);
float scale = MAJOR_GRID_DIVISIONS * spacing;
transform.setTranslation(position);
transform.setScale(scale);
batch->setModelTransform(transform);
DependencyManager::get<GeometryCache>()->renderGrid(*batch, MAJOR_GRID_DIVISIONS, MAJOR_GRID_DIVISIONS, gridColor);
}
}
}
void SparseOccupancyGridArrayDisplay::processMessage(
const jsk_recognition_msgs::SparseOccupancyGridArray::ConstPtr& msg)
{
allocateCloudsAndNodes(msg->grids.size()); // not enough
for (size_t i = 0; i < msg->grids.size(); i++) {
Ogre::SceneNode* node = nodes_[i];
rviz::PointCloud* cloud = clouds_[i];
const jsk_recognition_msgs::SparseOccupancyGrid grid = msg->grids[i];
Ogre::Vector3 position;
Ogre::Quaternion quaternion;
if(!context_->getFrameManager()->transform(grid.header, grid.origin_pose,
position,
quaternion)) {
ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
qPrintable( getName() ), grid.header.frame_id.c_str(),
qPrintable( fixed_frame_ ));
return; // return?
}
node->setPosition(position);
node->setOrientation(quaternion);
cloud->setDimensions(grid.resolution, grid.resolution, 0.0);
std::vector<rviz::PointCloud::Point> points;
for (size_t ci = 0; ci < grid.columns.size(); ci++) {
const jsk_recognition_msgs::SparseOccupancyGridColumn column = grid.columns[ci];
const int column_index = column.column_index;
for (size_t ri = 0; ri < column.cells.size(); ri++) {
const jsk_recognition_msgs::SparseOccupancyGridCell cell = column.cells[ri];
const int row_index = cell.row_index;
rviz::PointCloud::Point point;
if (!axis_color_) {
QColor color = gridColor(cell.value);
Ogre::ColourValue ogre_color = rviz::qtToOgre(color);
point.color = ogre_color;
}
else {
QColor color = axisColor(quaternion, Ogre::Vector3(1, 0, 0));
Ogre::ColourValue ogre_color = rviz::qtToOgre(color);
point.color = ogre_color;
}
point.position.x = grid.resolution * (column_index + 0.5);
point.position.y = grid.resolution * (row_index + 0.5);
point.position.z = 0.0;
points.push_back(point);
}
}
cloud->clear();
cloud->setAlpha(alpha_);
if (!points.empty()) {
cloud->addPoints(&points.front(), points.size());
}
}
context_->queueRender();
}
QColor SparseOccupancyGridArrayDisplay::axisColor(const Ogre::Quaternion& q,
const Ogre::Vector3& p)
{
Ogre::Vector3 zaxis = q.zAxis();
Ogre::Vector3 reference = p.normalisedCopy();
double dot = zaxis.dotProduct(reference);
if (dot < -1) {
dot = -1.0;
}
else if (dot > 1) {
dot = 1.0;
}
double scale = (dot + 1) / 2.0;
return gridColor(scale);
}
void Grid3DOverlay::render(RenderArgs* args) {
if (!_visible) {
return; // do nothing if we're not visible
}
const float MAX_COLOR = 255.0f;
float alpha = getAlpha();
xColor color = getColor();
glm::vec4 gridColor(color.red / MAX_COLOR, color.green / MAX_COLOR, color.blue / MAX_COLOR, alpha);
auto batch = args->_batch;
if (batch) {
auto minCorner = glm::vec2(-0.5f, -0.5f);
auto maxCorner = glm::vec2(0.5f, 0.5f);
auto position = getPosition();
if (_followCamera) {
// Get the camera position rounded to the nearest major grid line
// This grid is for UI and should lie on worldlines
auto cameraPosition =
(float)_majorGridEvery * glm::round(args->getViewFrustum().getPosition() / (float)_majorGridEvery);
position += glm::vec3(cameraPosition.x, 0.0f, cameraPosition.z);
}
Transform transform;
transform.setRotation(getRotation());
transform.setScale(glm::vec3(getDimensions(), 1.0f));
transform.setTranslation(position);
batch->setModelTransform(transform);
const float MINOR_GRID_EDGE = 0.0025f;
const float MAJOR_GRID_EDGE = 0.005f;
DependencyManager::get<GeometryCache>()->renderGrid(*batch, minCorner, maxCorner,
_minorGridRowDivisions, _minorGridColDivisions, MINOR_GRID_EDGE,
_majorGridRowDivisions, _majorGridColDivisions, MAJOR_GRID_EDGE,
gridColor, _drawInFront);
}
}
//---------------------------------------------------------------
void LonLatGrid::drawLongitude (QPainter &pnt, Projection *proj)
{
int min = (int) floor (proj->getXmin());
int max = (int) ceil (proj->getXmax());
double pixperdeg = proj->getW()/abs(max-min);
double delta = computeDeltaGrid(&min, pixperdeg);
QColor gridColor(40,40,40);
QPen pen (gridColor);
pen.setWidthF(0.8);
pnt.setPen(pen);
double x;
int i, j0,j1;
for (x=min; x<=max; x += delta)
{
proj->map2screen(x, floor(proj->getYmin()), &i, &j0);
proj->map2screen(x, ceil (proj->getYmax()), &i, &j1);
pnt.drawLine (i, j0, i, j1);
}
}
//---------------------------------------------------------------
void LonLatGrid::drawLatitude (QPainter &pnt, Projection *proj)
{
int min = (int) floor (proj->getYmin());
int max = (int) ceil (proj->getYmax());
double pixperdeg = proj->getW()/abs(max-min);
double delta = computeDeltaGrid(&min, pixperdeg);
QColor gridColor(40,40,40);
QPen pen (gridColor);
pen.setWidthF(0.8);
pnt.setPen(pen);
double y;
int i0, i1, j;
for (y=min; y<=max; y += delta)
{
proj->map2screen( floor(proj->getXmin()), y, &i0, &j);
proj->map2screen( ceil (proj->getXmax()), y, &i1, &j);
pnt.drawLine (i0, j, i1, j);
}
}
void myDisplay(void)
{
if (!paused) {
if (step < time_steps) {
step++;
nextStep();
} else {
exitProgram();
}
}
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
gluLookAt((X-1.0)/2.0, -Y/8.0, (X+Y)/2.6, (X-1.0)/grid_look_x, Y/grid_look_y, 0.0, 0.0, 1.0, 0.0);
bmax = 0;
for (int j = 1; j < Y-1; j++) {
for (int i = 1; i < X-1; i++) {
b[(j*X)+i] = grid_amp*(p[(j*X)+i]-p0);
if (b[(j*X)+i] > bmax)
bmax = b[(j*X)+i]; //set global maximum for grid coloring
}
}
//draw grid
glPushMatrix();
glTranslatef((float)X/2, (float)Y/2,0);
glRotatef((float)grid_rot_x, 1.0f, 0.0f, 0.0f);
glRotatef((float)grid_rot_z, 0.0f, 0.0f, 1.0f);
glTranslatef(-(float)X/2, -(float)Y/2, 0);
for (int j = 0; j < Y-1; j++) {
for (int i = 0; i < X-1; i++) {
glBegin(GL_LINES);
gridColor(i, j);
glVertex3f((float)i, (float)j, (float)b[(j*X)+i]);
gridColor(i+1, j);
glVertex3f((float)i+1, (float)j, (float)b[(j*X)+i+1]);
glEnd();
glBegin(GL_LINES);
gridColor(i, j);
glVertex3f((float)i, (float)j, (float)b[(j*X)+i]);
gridColor(i, j+1);
glVertex3f((float)i, (float)j+1, (float)b[((j+1)*X)+i]);
glEnd();
}
glBegin(GL_LINES);
gridColor(X-1, j);
glVertex3f((float)X-1, (float)j, (float)b[(j*X)+X-1]);
gridColor(X-1, j+1);
glVertex3f((float)X-1, (float)j+1, (float)b[((j+1)*X)+X-1]);
glEnd();
}
for (int i = 0; i < X-1; i++) {
glBegin(GL_LINES);
gridColor(i, Y-1);
glVertex3f((float)i, (float)Y-1, (float)b[((Y-1)*X)+i]);
gridColor(i+1, Y-1);
glVertex3f((float)i+1, (float)Y-1, (float)b[((Y-1)*X)+i+1]);
glEnd();
}
glPopMatrix();
glutSwapBuffers();
}
