AutoControl.cpp

#include <Base/Math/Vector3.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/Quaternion.hh>
#include "AutoControl.hh"

#include <Gui/biasgl.h>

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

AutoControl::AutoControl()
{
  NumberOfSteps_ = 10;
  Step_ = 0;
  Sector_ = 0;
  InitializedSector_ = -1;
  Active_ = false;
}

void AutoControl::Init()
{
  if (PoseList_.size() > 1)
  {
    InitializedSector_ = Sector_;
    Vector3<double> fromC   = PoseList_[Sector_].C;
    Vector3<double> fromUp  = PoseList_[Sector_].up;
    Vector3<double> fromPoI = PoseList_[Sector_].PoI;
    
    Vector3<double> toC   = PoseList_[Sector_ + 1].C;
    Vector3<double> toUp  = PoseList_[Sector_ + 1].up;
    Vector3<double> toPoI = PoseList_[Sector_ + 1].PoI;

    // Calculate direction vectors and stepSize
    VecC_ = toC - fromC;
    StepSizeC_ = VecC_.Length() / NumberOfSteps_;
    VecUp_ = toUp - fromUp;
    StepSizeUp_ = VecUp_.Length() / NumberOfSteps_;
    VecPoI_ = toPoI - fromPoI;
    StepSizePoI_ = VecPoI_.Length() / NumberOfSteps_;
  }
}

int AutoControl::NextStep()
{
  if (PoseList_.size() <= 1)  {
    BIASERR("Not enough camera poses defined.")
    return -1;
  }
  
  if (Sector_ + 2 >= PoseList_.size() && Step_ >= NumberOfSteps_){
    //cout<<"Animation finished"<<endl;
    return -1;
  }


  if (Step_ < NumberOfSteps_-1) {  // Is the current sector finished
    Step_++;
  } else {
    // Go to next sector if there is one left.
    if (Sector_ < PoseList_.size() - 2) {
      Step_ = 0;
      Sector_++;
    } else if (Sector_ + 2 == PoseList_.size() ) {
      // goto the last pose by calling
      // Goto(NumberOfSteps, NumPoses-2)
      //cout << "last pose\n";
      Step_++;
    } 
  }
  //  cout << "sector "<<Sector_<<"\tstep "<<Step_<<endl;
  return GoTo(Step_, Sector_);
}


int AutoControl::GoTo(unsigned int step, unsigned int sector)
{
  if (PoseList_.size() ==0) return -1;
  if (step > NumberOfSteps_) {
    BIASERR("Specified parameter out of range: Step " << step);
    return -1;
  }
  if (sector >= PoseList_.size() - 1) {
    BIASERR("Specified parameter out of range: Sector " << sector);
    return -1;
  }
  Step_ = step;
  Sector_ = sector;
  //cout << "step = "<<step<<"\tsector = "<<sector<<endl;
  if ((int)Sector_ != InitializedSector_) { 
    //cout << "calling init\n"; 
    Init(); 
  }
  Vector3<double> fromC   = PoseList_[Sector_].C;
  Vector3<double> fromUp  = PoseList_[Sector_].up;
  Vector3<double> fromPoI = PoseList_[Sector_].PoI;
  Vector3<double> delta;
  //cout << "C: "<<fromC<<"\nUp: "<<fromUp<<"\nPOI: "<<fromPoI<<endl;
  // Compute new camera      Vector3<double> C, up; pose.
  if (!VecC_.IsZero())    {
    delta = VecC_;
    delta.Normalize();
    delta *= (StepSizeC_   * Step_);
    fromC += delta;
  }
  
  if (!VecPoI_.IsZero())  {
    delta = VecPoI_;
    delta.Normalize();
    delta *= (StepSizePoI_   * Step_);
    fromPoI += delta;
  }
    
  if (!VecUp_.IsZero())  {
    delta = VecUp_;
    delta.Normalize();    
    delta *= (StepSizeUp_   * Step_);
    fromUp += delta;
  }
  //  update camera pose and redraw scene.
  //cout << "setting extrinsigs to "<<fromC<<", "<<fromPoI<<", "<<fromUp<<endl;
  int res = controlledObject_->SetExtrinsics(fromC, fromPoI, fromUp);
  Context_->Redisplay();
  return res;
}

int AutoControl::GoToPose(int pose)
{
  if ((int)(PoseList_.size()) < pose+1) {
    BIASERR("Specified parameter is out of range!");
    return -1;
  }

  //  update camera pose and redraw scene.
  //cout << "setting extrinsigs to "<<PoseList_[pose].C<<", "<<PoseList_[pose].PoI<<", "<<PoseList_[pose].up<<endl;
  int res = controlledObject_->SetExtrinsics(PoseList_[pose].C, 
                                             PoseList_[pose].PoI, 
                                             PoseList_[pose].up);
  Context_->Redisplay();
  return res;
}

void AutoControl::
AddPose(Vector3<double> C, Vector3<double> PoI, Vector3<double> up)
{
  CameraPose newPose;
  newPose.C = C;
  newPose.PoI = PoI;  
  newPose.up = up;
  PoseList_.push_back(newPose);

//    cout<<"--------- Added Pose:"<<endl;
//    cout<<"C: "<<C<<endl;
//    cout<<"PoI: "<<PoI<<endl;
//    cout<<"up: "<<up<<endl;      

  if (PoseList_.size() == 2)
    {
      Init();
    }
}

void AutoControl::
AddPose()
{
  Vector3<double> C,A,up;
  controlledObject_->GetExtrinsics(C, up);
  RMatrix R = controlledObject_->GetMyselfAsProjectionParameterBase()->GetR();
  A[0] = R[0][2];
  A[1] = R[1][2];
  A[2] = R[2][2];
  AddPose(C,A+C,up);
}



void AutoControl::Rewind()
{
  if (PoseList_.size() <2) return;
  Step_ = 0;
  Sector_ = 0;
  InitializedSector_ = -1;
  Init();
}

void AutoControl::DeleteLast()
{
  if (PoseList_.size() == 0)
  {
    BIASERR("No poses in list.")
    return;
  }
    
  PoseList_.pop_back();
  Sector_ = 0;
  Step_ = 0;
  Init();
}

void AutoControl::DeletePose(int pose)
{
  if (PoseList_.size() == 0)
  {
    BIASERR("No poses in list.")
    return;
  }

  // get pose and erase from PoseList_
  vector<CameraPose>::iterator iter=PoseList_.begin();
  for(int i=0; i<pose; i++) {
    iter++;
  }
  PoseList_.erase(iter);
  Sector_ = 0;
  Step_ = 0;
  Init();
}

void AutoControl::DeleteAll()
{
  PoseList_.clear();
}

bool AutoControl::
SpecialKeyPressed(int key)
{
  Vector3<double> C, up;
  switch (key)
  {
    case KEY_F1:
      AddPose();                                              
      break;
    case KEY_F2:
      cout<<"NextStep"<<endl;
      NextStep();
      break;
      
    case KEY_F3:
      cout<<"Rewind"<<endl;
      Rewind();
      break;

    case KEY_F4:
      cout<<"Last added pose deleted."<<endl;
      DeleteLast();
      break;      
  }
  return true;
}



void AutoControl::
SetContext(RenderContextBase* context)
{
  Context_ = context;
}

int AutoControl::SaveToFile(const std::string &file)
{
  ofstream ofs(file.c_str());
  if (!ofs) {
    perror(file.c_str());
    return -1;
  }
  Vector3<double> C,A,up;
  for (unsigned int i=0;i<PoseList_.size(); i++) {
    C = PoseList_[i].C;
    A = PoseList_[i].PoI;
    up = PoseList_[i].up;
    ofs << C[0]<<"   "<<C[1]<<"   "<<C[2]<<"   ";
    ofs << A[0]<<"   "<<A[1]<<"   "<<A[2]<<"   ";
    ofs << up[0]<<"   "<<up[1]<<"   "<<up[2]<<endl;

  }

  ofs.close();
  return 0;
}
 

int AutoControl::LoadFromFile(const std::string &file)
{
  PoseList_.clear();
  InitializedSector_ = -1;
  ifstream ifs(file.c_str());
  if (!ifs) {
    perror(file.c_str());
    return -1;
  }
  Vector3<double> C,A,up;

  while (ifs) {
    ifs >> C[0]>>C[1]>>C[2];
    ifs >> A[0]>>A[1]>>A[2];
    ifs >> up[0]>>up[1]>>up[2];
    if (ifs)       AddPose(C,A,up);

  }
  // cout <<"Found "<<PoseList_.size()<<" poses"<<endl;
  Step_ = 0;
  Sector_ = 0;
  InitializedSector_ = -1;
  Active_ = false;
  Init();
  return PoseList_.size();
}

bool AutoControl::TimerExpired()
{
  bool didsomething = false;
  //  cout <<"AutoControl::TimerExpired()"<<endl;
  if (Active_) {
    int res = NextStep();
    didsomething = true;
    if (res<0) Active_ = false;
  }

  return didsomething;
}