FlyControl.cpp

#include <GLviewer/GLProjectionParametersBase.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include "FlyControl.hh"


#include <Gui/biasgl.h>

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


FlyControl::FlyControl()
{  
  Stepsize_ = 0.2;
}

void FlyControl::SetStepsize(double stepsize)
{
  Stepsize_ = stepsize;
}

void FlyControl::
MoveForward()
{
  MoveDirection(Vector3<double>(0.0,0.0,Stepsize_));
}
    
void FlyControl::
MoveBackward()
{
  MoveDirection(Vector3<double>(0.0,0.0,-Stepsize_));
}
   
void FlyControl::
MoveRight()
{
  MoveDirection(Vector3<double>(Stepsize_,0.0,0.0));
}

void FlyControl::
MoveLeft()
{
  MoveDirection(Vector3<double>(-Stepsize_,0.0,0.0));
}

void FlyControl::
MoveUp()
{
  MoveDirection(Vector3<double>(0.0,Stepsize_,0.0));
}

void FlyControl::
MoveDown()
{
  MoveDirection(Vector3<double>(0.0,-Stepsize_,0.0));
}

void FlyControl::
MoveDirection(Vector3<double> direction)
{
  Vector3<double> C, temp; RMatrix R;
  controlledObject_->GetExtrinsics(C,R);  
  R.Mult(direction, temp);
  C.AddIP(temp);
  
  controlledObject_->SetExtrinsics(HomgPoint3D(C), R);
}

bool FlyControl::
LeftMouseMoved(int x1, int x2, int y1, int y2)
{

  if (x1-x2 == 0 && y1-y2 == 0) {
    return true;
  }

  //left hand image coordinate system if extended by viewing direction

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

  Vector3<double> C, up, A;
  controlledObject_->GetExtrinsics(C, up);
  RMatrix R = controlledObject_->GetMyselfAsProjectionParameterBase()->GetR();
  A = Vector3<double>(R[0][2], R[1][2], R[2][2]);
  
  // 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;
  // old: v1.CrossProduct(v2, axis);    
  v2.CrossProduct(v1, axis);
  double angleRAD = acos(v1.ScalarProduct(v2));
  controlledObject_->GetExtrinsics(C, up);
  
  // prohibit rolling around viewing direction
  Vector3<double> temp;  
  R.Transpose().Mult(axis, temp);
  temp[2] = 0.0;  
  R.Mult(temp, axis);
  axis.Normalize();
  
  Quaternion<ROTATION_MATRIX_TYPE> rotation;
  rotation.SetValueAsAxisRad(axis, angleRAD);
  
  // Derive direction vector and rotate direction and up vector.
  rotation.MultVec(A, A);
  rotation.MultVec(up, up); 
 
  controlledObject_->SetExtrinsics(C, C + A, up);
 
  return true;
}

bool FlyControl::
RightMouseMoved(int /*x1*/, int /*x2*/, int y1, int y2)
{
  if (y1-y2 != 0)                                    
  {
    if (y1 < y2)
    {
      MoveForward();
    }
    else
    {
      MoveBackward();
    }
  }
  return true;
}

bool FlyControl::
MiddleMouseMoved(int x1, int x2, int y1, int y2)
{
  if (x1-x2 != 0) 
  {
    if (x1 > x2)
    {
      MoveRight();
    }
    else
    {
      MoveLeft();
    }
  }
  if (y1-y2 != 0)                                    
  {
    if (y1 > y2)
    {
      MoveUp();
    }
    else
    {
      MoveDown();
    }
  }
  return true;
}

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

  // Get from 2D mouse position 3D point in scene.
  BIAS::HomgPoint3D homgPoint =
    controlledObject_->UnProject(x, y);
  if (homgPoint[3] == 0.0)
  {
    BIASERR("Error: Point of interest / 3D point could not be set.");
  }
  else
  {
    // Set new PoI.
    Vector3<double> C, up, poi;
    poi = Vector3<double>(homgPoint[0], homgPoint[1], homgPoint[2]);
    controlledObject_->GetExtrinsics(C, up);         
    controlledObject_->SetExtrinsics(C, poi, up);

    // Compute stepsize which is 5% from distance to PoI
    Vector3<double> distance = C - poi;
    double stepsize = distance.NormL2()*0.05;
    SetStepsize(stepsize);

    cout<<"New stepsize is: "<<stepsize<<endl;
  }
  return true;
}

bool FlyControl::
LeftAndRightMouseMoved(int x1, int x2, int y1, int y2)
{

  RMatrix R = controlledObject_->GetMyselfAsProjectionParameterBase()->GetR();
  
  int viewport[4];
  glGetIntegerv(GL_VIEWPORT, viewport);
  int width = viewport[2];
  int height = viewport[3];

  Vector3<double> C, up, viewingDirection;
  controlledObject_->GetExtrinsics(C, up);
  viewingDirection = Vector3<double>(R[0][2], R[1][2], R[2][2]);

  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 FlyControl::
RotateAroundViewingDirection(double rotationAngleRAD)
{
  RMatrix R = controlledObject_->GetMyselfAsProjectionParameterBase()->GetR();
  Vector3<double> C, up, rotationAxis, A;
  
  rotationAxis = Vector3<double>(R[0][2], R[1][2], R[2][2]);
  controlledObject_->GetExtrinsics(C, up);

  // Create quaternion and derive new up vector
  Quaternion<ROTATION_MATRIX_TYPE> delta;
  delta.SetValueAsAxisRad(rotationAxis, rotationAngleRAD);
  delta.MultVec(up, up);
  
  controlledObject_->SetExtrinsics(C, C + rotationAxis, up);
}