TrackballBase.cpp

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

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


TrackballBase::TrackballBase()
{  
  Stepsize_ = 0.2;
  controlledObject_=NULL;
  UseLeftDouble_ = true;
}

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

bool TrackballBase::
SpecialKeyPressed(int key)
{
  switch(key) {
    case KEY_PGUP : Stepsize_*=1.1;  cout<<"[Trackball] stepsize = "
                    <<Stepsize_<<endl; 
                   break;
    case KEY_PGDOWN : Stepsize_*=0.9; cout<<"[Trackball] stepsize = "
                    <<Stepsize_<<endl;
                   break;
  }
  return(true);
}

void TrackballBase::GetMoveinViewDirection(GLProjectionParametersInterface* cam,
                                           const double stepsize,
                                           const BIAS::Vector3<double>& viewdir,
                                          BIAS::Vector3<double>& res_translation
                                          )
{
  Vector3<double> direction = viewdir;
  double distance;
  distance = direction.NormL2();
  
  if (Equal(distance, 0.0))
  {
    distance = 1.0;
    direction = Vector3<double>(0., 0., 1.0);
  } 
  else 
  {
    direction.Normalize();
  }

  direction.Multiply(stepsize, direction);
  res_translation = direction;
// cout<<res_translation<<endl;
}

bool TrackballBase::GetRotationAxisAndAngle(GLProjectionParametersInterface* cam,
                                            const int x1, const int x2, 
                                            const int y1, const int y2,
                                            BIAS::Vector3<double>& axis,
                                            double& angleRAD
                                            )
{
  //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(0,1,0);
   RMatrix R(MatrixIdentity);
   cam->GetExtrinsics(C, up);
   R = cam->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       
   //v2.CrossProduct(v1, axis);
   v1.CrossProduct(v2, axis);
   axis = R*axis;
   angleRAD = acos(v1.ScalarProduct(v2));
   return(true);
}

void TrackballBase::GetViewDirection(GLProjectionParametersInterface* cam,
                                     BIAS::Vector3<double>& direction)
{
  RMatrix R(MatrixIdentity);  
  Vector3<double> C;
  cam->GetExtrinsics(C,R);
  
  BIAS::Vector3<double> A;
  A[0] = R[0][2];  A[1] = R[1][2];  A[2] = R[2][2];
  //A *= 7.0;
  
  direction = A;
 
  //cout<<"a: "<<A<<" c: "<<C<<" dir: "<<direction<<endl;

}

//used in middle mouse button case
void TrackballBase::GetTranslation(GLProjectionParametersInterface* cam,
                                   const int x1, const int x2, 
                                   const int y1, const int y2,
                                   const BIAS::Vector3<double>& viewdir,
                                   BIAS::Vector3<double>& transl 
                                   )
{
  Vector3<double> directionRight, directionUp, up(0,1,0), C;
  cam->GetExtrinsics(C, up);
  // Derive vector for moving right / left
  //  PointOfInterest_.Sub(C, view);
  viewdir.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);
    }
  }
  else
  {
    directionRight.SetZero();
  }

  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);
    }
  }
  else
  {
    directionUp.SetZero();
  }
  transl = directionRight + directionUp;
}

double TrackballBase::GetRotationAngle(GLProjectionParametersInterface* cam,
                                       const int x1, 
                                       const int x2, 
                                       const int y1, 
                                       const int y2,
                                       const BIAS::Vector3<double> viewdir)
{
  int viewport[4];
  glGetIntegerv(GL_VIEWPORT, viewport);
  int width = viewport[2];
  int height = viewport[3];
  Vector3<double> C, up(0,1,0);
  cam->GetExtrinsics(C, up);  
  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(viewdir.ScalarProduct(rotationAxis) > 0 )
    {
      rotationAngle *= -1.0;
    }
  return(rotationAngle);
}