TrackballControl.cpp

#include "TrackballControl.hh"

#include <Base/Math/Vector3.hh>
#include <Gui/biasgl.h>

using namespace BIAS;
using namespace BIAS;
using namespace std;


TrackballControl::
TrackballControl()
{
  PointOfInterest_ = Vector3<double>(0.0,0.0,0.0);
  controlledObject_=NULL;
  Stepsize_ = 0.2;
  UseLeftDouble_ = true;
}

bool TrackballControl::
RightMouseMoved(int , int , int y1, int y2) {

  // Zoom out
  if (y1 - y2 > 0)
  {
    MoveInViewingDirection(-Stepsize_);
  }
  // Zoom in
  else if(y1 - y2 < 0)
  {
    MoveInViewingDirection(Stepsize_);
  }
  else
  {
    return false;
  }
  return true;
}


bool TrackballControl::
MouseWheelUsed(double stepsize)
{
  MoveInViewingDirection(stepsize);
  return true;
}

void TrackballControl::
MoveInViewingDirection(double stepsize)
{
  RMatrix R(MatrixIdentity);
  Vector3<double> C(0,0,0);
  controlledObject_->GetExtrinsics(C,R);
  Vector3<double> direction;
  double distance;

  // Compute new position of camera.
  PointOfInterest_.Sub(C, direction);
  distance = direction.NormL2();
  if (Equal(distance, 0.0)){
    distance = 1.0;
    direction = Vector3<double>(0., 0., 1.0);
  } else {
    direction.Normalize();
  }
  double nearClippingPlane = 0;
  nearClippingPlane = controlledObject_->GetNearClippingPlane();
  if(distance > (nearClippingPlane+stepsize) ) {
    direction.Multiply(stepsize, direction);
  } else {
    direction.Multiply(nearClippingPlane-distance, direction);
  }
  C.Add(direction, C);

  controlledObject_->SetExtrinsics(HomgPoint3D(C),R);
}

void TrackballControl::
RotateAroundPOI(BIAS::Vector3<double> axis, double angleRAD)
{
  Vector3<double> C,up(0,1,0);
  controlledObject_->GetExtrinsics(C, up);
  Quaternion<ROTATION_MATRIX_TYPE> rotation;
  rotation.SetValueAsAxisRad(axis, angleRAD);

  // Derive direction vector and rotate direction and up vector.
  Vector3<double> direction;
  C.Sub(PointOfInterest_, direction);
  rotation.MultVec(direction, direction);
  rotation.MultVec(up, up);
  direction.Add(PointOfInterest_, C);

  controlledObject_->SetExtrinsics(C, PointOfInterest_, up);
  return;
}

bool TrackballControl::
LeftMouseMoved(int x1, int x2, int y1, int y2)
{
  //left hand image coordinate system if extended by viewing direction

  int viewport[4];
  glGetIntegerv(GL_VIEWPORT, viewport);
  x1-=viewport[0];
  x2-=viewport[0];
  y1-=viewport[1];
  y2-=viewport[1];

  double width = viewport[2];
  double height = viewport[3];

  Vector3<double> C, up(0,1,0);
  RMatrix R(MatrixIdentity);
  controlledObject_->GetExtrinsics(C, up);
  R = controlledObject_->GetMyselfAsProjectionParameterBase()->GetR();

  // Calculate first vector
  // The trackball contains the whole scene.
  double x = x1 / width * 2.0;
  double y = y1 / height * 2.0;
  x = x - 1.0;
  y = 1.0 - y;
  x *=-1.0;
  // calculate clicked point on trackball. Trackball has radius 1
  double z = sqrt(2.0) - x * x  - y * y;
  if (z > 0)
  {
    z = sqrt(z);
  }
  else
  {
    return false;
  }
  Vector3<double> v1 = Vector3<double>(x, y, z);
  v1.Normalize();
  v1 = R*v1;

  // calculate second vector.
  x = x2 / width * 2.0;
  y = y2 / height * 2.0;
  x = x - 1.0;
  y = 1.0 - y;
  x *=-1.0;

  z = sqrt(2.0) - x * x - y * y;
  if (z > 0)
  {
    z = sqrt(z);
  }
  else
  {
    return false;
  }

  Vector3<double> v2 = Vector3<double>(x, y, z);
  v2.Normalize();
  v2 = R*v2;

  // Calculate axis and angle for quaternion
  Vector3<double> axis;

  v2.CrossProduct(v1, axis);
  double angleRAD = acos(v1.ScalarProduct(v2));

  RotateAroundPOI(axis, angleRAD);
  return true;
}

bool TrackballControl::
LeftMouseDoubleClicked(int x, int y, int m)
{
  if (! UseLeftDouble_) {
    return false;
  }

  // Get from 2D mouse position 3D point in scene.
  if(!controlledObject_->GetViewportCoordinates(x, y)) {
    BIASERR("outside used viewport!");
    return false;
  }

  BIAS::HomgPoint3D homgPoint=controlledObject_->UnProject(x, y);

  if (homgPoint[3] != 0.0)
  {
    Vector3<double> poi(homgPoint[0],homgPoint[1], homgPoint[2]);
    SetPointOfInterest(poi);

    //cout << "TrackballControl::LeftMouseDoubleClicked("<<x<<","<<y<<") : "
    //     << "Set point of interest to ("
    //     << homgPoint[0] << ", "<< homgPoint[1] << ", " << homgPoint[2]
    //     << ")." << endl;
    //Vector3<double> C;
    // C = controlledObject_->GetMyselfAsProjectionParameterBase()->GetC();
    //  cout<<"And Camera Center:"<<C<<endl;

  }
  else
  {
    ;//BIASERR("Error: Point of interest / 3D point could not be set.");
  }
  return true;
}

bool TrackballControl::
MiddleMouseMoved(int x1, int x2, int y1, int y2)
{
  // get current extrinsics
  Vector3<double> C(0,0,0), up(0,0,1);
  controlledObject_->GetExtrinsics(C, up);

  // compute vector of camera center to poi
  Vector3<double> localPOI(0,0,0);
  PointOfInterest_.Sub(C, localPOI);

  // compute distance of camera center to poi
  double poiDepth = localPOI.Length();

  // project point of interest
  HomgPoint3D  tmp(PointOfInterest_);
  HomgPoint2D projectedPOI = controlledObject_->Project(tmp);

  // move projected point of interest
  projectedPOI[0] += (x1 - x2);
  projectedPOI[1] += (y1 - y2);
  projectedPOI[2] = 1.;

  // set new poi at old depth
  tmp = controlledObject_->UnProject(projectedPOI, poiDepth);
  tmp.Homogenize();
  PointOfInterest_[0] = tmp[0]; PointOfInterest_[1] = tmp[1]; PointOfInterest_[2] = tmp[2];
  // adjust center of camera
  PointOfInterest_.Sub(localPOI, C);

  controlledObject_->SetExtrinsics(C, PointOfInterest_, up);
  return true;

  /*
    Vector3<double> directionRight, directionUp, up(0,1,0), C, view;
    if (controlledObject_->PoseValid()){
    controlledObject_->GetCurrentExtrinsics(C, up);
    } else {
    BIASERR("TrackballControl: PoseInvalid");
    }
    // Derive vector for moving right / left
    PointOfInterest_.Sub(C, view);
    view.CrossProduct(up, directionRight);
    directionRight.Normalize();
    directionUp = up;

    if (x1-x2 != 0)
    {
    if (x1 > x2)
    {
    // Move left
    directionRight.MultIP(Stepsize_);
    C.AddIP(directionRight);
    PointOfInterest_.AddIP(directionRight);
    }
    else
    {
    // Move right
    directionRight.MultIP(-Stepsize_);
    C.AddIP(directionRight);
    PointOfInterest_.AddIP(directionRight);
    }
    }

    if (y1-y2 != 0)
    {
    if (y1 > y2)
    {
    // Move up
    directionUp.MultIP(-Stepsize_);
    C.AddIP(directionUp);
    PointOfInterest_.AddIP(directionUp);
    }
    else
    {
    // Move down
    directionUp.MultIP(Stepsize_);
    C.AddIP(directionUp);
    PointOfInterest_.AddIP(directionUp);
    }
    }

    controlledObject_->SetExtrinsics(C, PointOfInterest_, up);
    return true;
  */
}


void TrackballControl::
RotateAroundViewingDirection(double rotationAngleRAD)
{
  Vector3<double> C, up(0,1,0), rotationAxis;
  controlledObject_->GetExtrinsics(C, up);

  rotationAxis = PointOfInterest_ - C;
  if(rotationAxis.NormL2() == 0.0){
    BIASERR("point of interest equals camera center\n");
    RMatrix R(MatrixIdentity);
    R = controlledObject_->GetMyselfAsProjectionParameterBase()->GetR();
    for(unsigned int i=0; i<3; i++)
      rotationAxis[i] = R[i][2];
  }

  // Create quaternion and derive new up vector
  Quaternion<ROTATION_MATRIX_TYPE> delta;
  delta.SetValueAsAxisRad(rotationAxis, rotationAngleRAD);
  delta.MultVec(up, up);

  controlledObject_->SetExtrinsics(C,
                                   PointOfInterest_,
                                   up);
}

bool TrackballControl::
LeftAndRightMouseMoved(int x1, int x2, int y1, int y2)
{
  //left hand image coordinate system if extended by viewing direction
  //   int temp = y2;
  //   y2 = y1;
  //   y1 = temp;

  int viewport[4];
  glGetIntegerv(GL_VIEWPORT, viewport);
  int width = viewport[2];
  int height = viewport[3];

  Vector3<double> C, up(0,1,0), viewingDirection;
  controlledObject_->GetExtrinsics(C, up);

  viewingDirection = PointOfInterest_ - C;

  Vector3<double> rotationAxis;
  // Compute rotation axis and angle
  Vector3<double> v1 = Vector3<double>((x1 - (width/2.0)),
                                       (y1 - (height/2.0)),
                                       0);

  Vector3<double> v2 = Vector3<double>(x2 - (width/2.0),
                                       y2 - (height/2.0),
                                       0);

  v1.Normalize();
  v2.Normalize();

  v1.CrossProduct(v2, rotationAxis);
  double rotationAngle = acos(v1.ScalarProduct(v2));

  if(viewingDirection.ScalarProduct(rotationAxis) > 0 )
     rotationAngle *= -1.0;

  RotateAroundViewingDirection(rotationAngle);

  return true;
}

void TrackballControl::
SetPointOfInterest(const BIAS::Vector3<double>& poi)
{
  PointOfInterest_ = poi;

  if(controlledObject_==NULL) return;
  // Calculate new camera parameters.
  Vector3<double> C, up(0, 1, 0);
  if (controlledObject_->PoseValid()){
    controlledObject_->GetExtrinsics(C,up);
  } else {
    BIASERR("TrackballControl: PoseInvalid");
  }

  //automaticly set stepsize to a 5 percent of distance to point of interest
  Vector3<double> dist = PointOfInterest_ - C;
  Stepsize_ = 0.05*dist.NormL2();
  //cout<<"[Trackball] stepsize = "<<Stepsize_<<endl;
  controlledObject_->SetExtrinsics(C,poi,up);
}


void TrackballControl::
SetGLProjectionParametersInterface(BIAS::GLProjectionParametersInterface* projInterface)
{
  // extract old distance of
  double dist = 10.0;
  if (controlledObject_){
    RMatrix oldR;
    Vector3<double> oldC;
    controlledObject_->GetExtrinsics(oldC, oldR);
    Vector3<double> oldPOI = GetPoI();
    dist = (oldC-oldPOI).NormL2();
  }
  ControlInterface::SetGLProjectionParametersInterface(projInterface);
  BIAS::RMatrix R(BIAS::MatrixIdentity);
  BIAS::Vector3<double> C;
  if (controlledObject_->PoseValid()){
    controlledObject_->GetExtrinsics(C,R);
  } else {
    BIASERR("TrackballControl: PoseInvalid, using identity!");
    C[0]=0;C[1]=0;C[2]=0;
    R.SetIdentity();
  }
  BIAS::Vector3<double> A;
  A[0] = R[0][2];  A[1] = R[1][2];  A[2] = R[2][2];
  A *= dist;
  SetPointOfInterest(C+A);
}