SceneCoordinateFrame.cpp

#include <GLviewer/Scenes/SceneCoordinateFrame.hh>
#include <GLviewer/GLProjectionParametersInterface.hh>

using namespace BIAS;

SceneCoordinateFrame::
SceneCoordinateFrame(const BIAS::Vector3<double> &origin,
                     const BIAS::RMatrix &orientation, const double scale)
{
  SetCoordinateFrame(origin, orientation, scale);
}

SceneCoordinateFrame::~SceneCoordinateFrame()
{
}

void SceneCoordinateFrame::
SetCoordinateFrame(const BIAS::Vector3<double> &origin,
                   const BIAS::RMatrix &orientation, const double scale)
{
  // Set origin and calculate axes
  origin_.Set(origin[0], origin[1], origin[2]);
  for (unsigned int i = 0; i < 3; i++) {
    axisX_[i] = orientation[i][0] * scale;
    axisY_[i] = orientation[i][1] * scale;
    axisZ_[i] = orientation[i][2] * scale;
  }

  // Make a bounding box to fit the coordinate frame for any rotation
  boundingBoxMin_ = origin;
  boundingBoxMax_ = origin;
  const double extent = fabs(scale) * sqrt(3.0);
  for (unsigned int i = 0; i < 3; i++) {
    boundingBoxMin_[i] -= extent;
    boundingBoxMax_[i] += extent;
  }
}

void SceneCoordinateFrame::
GetBoundingBox(BIAS::Vector3<double> &minVal, BIAS::Vector3<double> &maxVal)
{
  minVal = boundingBoxMin_;
  maxVal = boundingBoxMax_;
}

void SceneCoordinateFrame::Draw()
{
  // Disable lighting
  glPushAttrib(GL_LIGHTING);
  glDisable(GL_LIGHTING);

  // Draw x, y and z axes at origin
  glBegin(GL_LINES);
  glColor3f(1.0f, 0.0f, 0.0f);
  glVertex3f(origin_[0], origin_[1], origin_[2]);
  glVertex3f(origin_[0] + axisX_[0], origin_[1] + axisX_[1], origin_[2] + axisX_[2]);
  glColor3f(0.0f, 1.0f, 0.0f);
  glVertex3f(origin_[0], origin_[1], origin_[2]);
  glVertex3f(origin_[0] + axisY_[0], origin_[1] + axisY_[1], origin_[2] + axisY_[2]);
  glColor3f(0.0f, 0.0f, 1.0f);
  glVertex3f(origin_[0], origin_[1], origin_[2]);
  glVertex3f(origin_[0] + axisZ_[0], origin_[1] + axisZ_[1], origin_[2] + axisZ_[2]);
  glEnd();

  // Draw points at origin and axis ends
  GLfloat pointSize;
  glGetFloatv(GL_POINT_SIZE, &pointSize);
  glPointSize(8.0f);
  glBegin(GL_POINTS);
  glColor3f(0.0f, 0.0f, 0.0f);
  glVertex3f(origin_[0], origin_[1], origin_[2]);
  glColor3f(1.0f, 0.0f, 0.0f);
  glVertex3f(origin_[0] + axisX_[0], origin_[1] + axisX_[1], origin_[2] + axisX_[2]);
  glColor3f(0.0f, 1.0f, 0.0f);
  glVertex3f(origin_[0] + axisY_[0], origin_[1] + axisY_[1], origin_[2] + axisY_[2]);
  glColor3f(0.0f, 0.0f, 1.0f);
  glVertex3f(origin_[0] + axisZ_[0], origin_[1] + axisZ_[1], origin_[2] + axisZ_[2]);
  glEnd();
  glPointSize(pointSize);

  glPopAttrib(); // GL_LIGHTING
}