SceneBase.cpp

#include <GLviewer/Scenes/SceneBase.hh>
#include <list>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/HomgPlane3D.hh>
#include <Base/Math/Random.hh>

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

double SceneBase::animationSpeed_  = 1.0;

SceneBase::SceneBase() : 
  IsActive_(true), ActiveChannel_(C_BothMono),camera_(NULL), velocity_(0,0,0)  {
  SetColorMask();
  mass_ = -1.0; 
  gravityImmunity_ = false;
  elasticity_ = 0.95; // power ball
  renderBoundingBox_ = false;
  dPointDrawStyle_ = POINT_SIMPLE;
  dTriangleMeshDrawStyle_ = TRIANGLE_SIMPLE;

}


void SceneBase::Render() {
    if(IsActive_) {
        GLboolean maskHist[4];
        glGetBooleanv(GL_COLOR_WRITEMASK, maskHist);
        glColorMask(colorMask_[0], colorMask_[1],
                colorMask_[2], colorMask_[3]);
        if (renderBoundingBox_) RenderBoundingBox();
        Draw();
        glColorMask(maskHist[0], maskHist[1],
                maskHist[2], maskHist[3]);

    }
}


std::list<BIAS::HomgPoint3D > 
SceneBase::CornersFromBoundingBox(const BIAS::Vector3<double>& b1,
                                  const BIAS::Vector3<double>& b2) {
  std::list<BIAS::HomgPoint3D >  cornerlist;
  HomgPoint3D bcur(b1);
  cornerlist.push_back(bcur);
  bcur[0] = b2[0];
  cornerlist.push_back(bcur);
  bcur[1] = b2[1];
  cornerlist.push_back(bcur);
  bcur[0] = b1[0];
  cornerlist.push_back(bcur);
  bcur[2] = b2[2];
  bcur[1] = b1[1];
  cornerlist.push_back(bcur);
  bcur[0] = b2[0];
  cornerlist.push_back(bcur);
  bcur[1] = b2[1];
  cornerlist.push_back(bcur);
  bcur[0] = b1[0];
  cornerlist.push_back(bcur);
  return cornerlist;
}

std::list<BIAS::HomgPlane3D > 
SceneBase::FacesFromBoundingBox(const BIAS::Vector3<double>& b1,
                                const BIAS::Vector3<double>& b2) {
  std::list<HomgPlane3D >  facelist;
  facelist.push_back(HomgPlane3D(0,0,1,-b1[2]));
  facelist.push_back(HomgPlane3D(0,0,-1,b2[2]));
  facelist.push_back(HomgPlane3D(0,1,0,-b1[1]));
  facelist.push_back(HomgPlane3D(0,-1,0,b2[1]));
  facelist.push_back(HomgPlane3D(1,0,0,-b1[0]));
  facelist.push_back(HomgPlane3D(-1,0,0,b2[0]));
  return facelist;
}

bool SceneBase::HasBoundingBoxCollision(SceneBase* otherScene) {
  // sceneoctree has a finer colision algorithm, call that !
  // NOT HERE, this is a base class. Derive from it and overload this method !!!

  BIAS::Vector3<double>  thismin(0,0,0);
  BIAS::Vector3<double>  thismax(0,0,0);
  GetBoundingBox(thismin, thismax);
  BIAS::Vector3<double>  othermin(0,0,0);
  BIAS::Vector3<double>  othermax(0,0,0);
  otherScene->GetBoundingBox(othermin,othermax);

  std::list<HomgPoint3D > thiscorners = 
    CornersFromBoundingBox(thismin,thismax);

  std::list<BIAS::HomgPlane3D> otherfaces = 
    FacesFromBoundingBox(othermin, othermax);

  // simple test:
  // for wcs-aligned bounding boxes NOT to intersect, all corner points of
  // this must be on an outside of one of the six faces of the other
  bool outside = true;
  std::list<BIAS::HomgPlane3D >::iterator itfaces;
  for (itfaces=otherfaces.begin(); itfaces!=otherfaces.end(); itfaces++) {
    outside = true;
    std::list<BIAS::HomgPoint3D >::iterator itcorners;
    for (itcorners=thiscorners.begin(); itcorners!=thiscorners.end();
         itcorners++) {
      if (itfaces->ScalarProduct(*itcorners)>=0.0) {
        outside = false;
        break;
      }
    }
    if (outside) {
      break;
    }
  }
 
  return !outside;
}

double SceneBase::TimeToContact(SceneBase* otherScene, 
                                Vector3<double>& contactnormal) {

  // sceneoctree has a finer colision algorithm, call that !
  // NOT HERE, this is a base class. Derive from it and overload this method !!!

  Vector3<double> relativeSpeed(velocity_ - otherScene->velocity_);
  //  BIASWARN("relative speed is "<<relativeSpeed);
  BIAS::Vector3<double>  thismin(0,0,0);
  BIAS::Vector3<double>  thismax(0,0,0);
  GetBoundingBox(thismin, thismax);
  BIAS::Vector3<double>  othermin(0,0,0);
  BIAS::Vector3<double>  othermax(0,0,0);
  otherScene->GetBoundingBox(othermin,othermax);

  std::list<HomgPoint3D > thiscorners = 
    CornersFromBoundingBox(thismin,thismax);

  std::list<BIAS::HomgPlane3D> otherfaces = 
    FacesFromBoundingBox(othermin, othermax); 

  std::list<HomgPoint3D > othercorners = 
    CornersFromBoundingBox(othermin,othermax);

  std::list<BIAS::HomgPlane3D> thisfaces = 
    FacesFromBoundingBox(thismin, thismax);

  
  // check which face has the earliest collision with one of the bbcorners of the other object
  std::list<BIAS::HomgPlane3D >::iterator itfaces;
  double besttsofar = 1e100;
  for (itfaces=otherfaces.begin(); itfaces!=otherfaces.end(); itfaces++) {
    std::list<BIAS::HomgPoint3D >::iterator itcorners;
    for (itcorners=thiscorners.begin(); itcorners!=thiscorners.end();
         itcorners++) {
      // compute time to contact this plane:
      double t = -1.0;
      Vector3<double> x(itcorners->GetEuclidian());
      Vector3<double> n(itfaces->GetNormalVector());
      double denominator = relativeSpeed.ScalarProduct(n);
      if (fabs(denominator)>1e-9) {
        // movement NOT parallel to plane
        t = (-x.ScalarProduct(n) - (*itfaces)[3]) / denominator;
        if (t<=0.0) {
          //BIASWARN("negative t"<<t<<" "<< *itcorners<<" "<< *itfaces <<" "
          //         <<relativeSpeed);
        }
      }
            
      if (t>0.0 && t<besttsofar) {
        // check whether 
        Vector3<double> cp = x + t * relativeSpeed;

        if (cp[0]>=othermin[0] && cp[1]>=othermin[1] && cp[2]>=othermin[2]
            && cp[0]<=othermax[0] && cp[1]<=othermax[1] && cp[2]<=othermax[2]){
          besttsofar = t;
          contactnormal = itfaces->GetNormalVector();
        }
      } 
    }
  }

  // now change the roles
  // DO NOT DELETE THIS CODE. IT IS REALLY IMPORTANT :-)
  for (itfaces=thisfaces.begin(); itfaces!=thisfaces.end(); itfaces++) {
    std::list<BIAS::HomgPoint3D >::iterator itcorners;
    for (itcorners=othercorners.begin(); itcorners!=othercorners.end();
         itcorners++) {
      // compute time to contact this plane:
      double t = -1.0;
      Vector3<double> x(itcorners->GetEuclidian());
      Vector3<double> n(itfaces->GetNormalVector());
      double denominator = -1.0 * relativeSpeed.ScalarProduct(n);
      if (fabs(denominator)>1e-9) {
        // movement NOT parallel to plane
        t = (-x.ScalarProduct(n) - (*itfaces)[3]) / denominator;
        if (t<=0.0) {
          // BIASWARN("negative t"<<t<<" "<< *itcorners<<" "<< *itfaces <<" "
          //         <<relativeSpeed);
        }
      }

      if (t>0.0 && t<besttsofar) {
        // check whether
        Vector3<double> cp = x + t * relativeSpeed;

        if (cp[0]>=othermin[0] && cp[1]>=othermin[1] && cp[2]>=othermin[2]
            && cp[0]<=othermax[0] && cp[1]<=othermax[1] && cp[2]<=othermax[2]){
          besttsofar = t;
          contactnormal = itfaces->GetNormalVector();
        }
      }

    }
  }

  return besttsofar;
}


void SceneBase::PerformCollision(SceneBase* otherscene, 
                                 const BIAS::Vector3<double>& contact) {
  
  // sceneoctree has a finer colision algorithm, call that !
  // NOT HERE, this is a base class. Derive from it and overload this method !!!

  // dont collide the static surrounding, like the room
  if (otherscene->gravityImmunity_) return;

  // cout<<"performing collision between "<<typeid(*this).name()<<" and "
  //      <<typeid(*otherscene).name()<<endl;

  // check velocities
  if ((velocity_-otherscene->velocity_).NormL2()<1e-10) {
    // so slow contact that they do fly peacefully near each other
#ifdef BIAS_DEBUG
    BIASWARN("slow contact: "<<velocity_<<"<->"<<otherscene->velocity_ 
             <<", no collision");
#endif
    return;
  } else {
    //BIASWARN("collision velocity: "<<velocity_<<"<->"<<otherscene->velocity_);
  }

  BIAS::Vector3<double>  thismin(0,0,0),thismax(0,0,0);
  GetBoundingBox(thismin, thismax);
  BIAS::Vector3<double> thismean(0.5 * thismin + 0.5 * thismax);
  //cout<<"this:  "<< thismean <<" "<< thismin <<" "<<thismax <<endl;
  BIAS::Vector3<double>  othermin(0,0,0),othermax(0,0,0);
  otherscene->GetBoundingBox(othermin,othermax);
  BIAS::Vector3<double> othermean(0.5 * othermin + 0.5 * othermax);
  //cout<<"other: "<< othermean <<" "<< othermin <<" "<< othermax <<endl;


  // there are several ways to determine the direction of collision
  // a) velocities, valid for points
  // b) centers of gravity, maybe nicer for larger scenes

  BIAS::Vector3<double> collisiondirection(thismean-othermean);
  if (contact[0]!=-1e10) {
    // have plane normal for which collision happened
    collisiondirection = contact;
  }

  if (collisiondirection.NormL2()<1e-10) {
#ifdef BIAS_DEBUG
    BIASWARN("collision only using velocities");
#endif
    collisiondirection = velocity_-otherscene->velocity_;
    if (collisiondirection.NormL2()<1e-10) {
      collisiondirection = Vector3<double>(1,0,0);
#ifdef BIAS_DEBUG
      BIASWARN("no collision direction could be detected, using x");
#endif
    }
  } 
  // get a real unit vector
  collisiondirection.Normalize();
  //BIASWARN("collision direction is "<<collisiondirection);
  
  // reduce to a 1D problem: compute velocities in direction of collision
  double v1 = collisiondirection.ScalarProduct(velocity_);
  double m1 = mass_;
 
  double v2 = collisiondirection.ScalarProduct(otherscene->velocity_);
  double m2 = otherscene->mass_;

  // check if both scenes must change their velocity or whether one of them
  // has an infinite weight (like a wall, which does not move)
  // m<=0 means infinite weight
  if (m1<0.0) {
    m1 = 1;
    m2 = 0;
  }
  if (m2<0.0) {
    m1 = 0;
    m2 = 1;
  }

  // compute collision if it was 100% elastic (no energy/momentum loss)
  // and then for 100% unelastic (glue together)

  // elastic collision:
  double v1elas = ((m1-m2)* v1 + 2.0*m2*v2)/(m1+m2);
  double v2elas = ((m2-m1)* v2 + 2.0*m1*v1)/(m1+m2);
  
  // plastic collision
  double v2plas = (m1*v1 + m2*v2) / (m1+m2);
  double v1plas = v2plas;

  // compute elasticity as a mean between the two materials
  double elasticity = 0.5*(elasticity_ + otherscene->elasticity_);

  if (m1>=0.0) {
    // compute new velocity as a linear combination of theoretic elastic model
    // and theoretic unelastic model.
    // the velocity components orthogonal to the collision dir do not change.
    velocity_             = elasticity * (velocity_  - v1 * collisiondirection)
      + collisiondirection * (elasticity * v1elas + (1.0-elasticity)*v1plas);
  }
  if (m2>=0.0) {
    // old 3d velocity - velocity in collision direction + new vel in collision)
    otherscene->velocity_ = 
      elasticity * (otherscene->velocity_ - v2 * collisiondirection)
      + collisiondirection * (elasticity * v2elas + (1.0-elasticity)*v2plas); 
  }
}

void SceneBase::ApplyTimeStep(const double& delta,
                              const BIAS::Vector3<double>& gravity,
                              const double& frictioncoefficient) {
  if (mass_>0.0 && !gravityImmunity_) {
    if (velocity_.NormL2()<1e10 || velocity_.ScalarProduct(gravity)<=0.0)
      velocity_ += delta * gravity;
    if (delta==1.0) {
      velocity_ *= frictioncoefficient;
    } else {
      velocity_ *= pow(frictioncoefficient, delta);
    }
  }
  //  Random R;
  //velocity_[0] += R.GetUniformDistributed(0.1, -0.1);
  //velocity_[1] += R.GetUniformDistributed(0.1, -0.1);
  //velocity_[2] += R.GetUniformDistributed(0.1, -0.1);
  MoveScene(delta);
}

void SceneBase::MoveScene(const double& delta) {
//  BIASWARN("No movement implemented for this scene type: "<<
//           typeid(*this).name());
}

void SceneBase::DumpInfo() const {
  cout<<"Scene type: "<<typeid(*this).name()<<endl;
  cout<<"active "<< IsActive_ <<endl;
  cout<<"active channel "<<ActiveChannel_<<endl;    
  cout<<"GLProjection: "<<camera_<<endl;
  cout<<"color mask:"<<colorMask_[0]<<colorMask_[1]
      <<colorMask_[2]<<colorMask_[3]<<endl;
  cout<<"mass: "<<mass_<<endl;
  cout<<"velocity: "<<velocity_<<endl;
  cout<<"elasticity: "<<elasticity_<<endl;
}

void SceneBase::RenderBoundingBox() {
  Vector3<double> bbmin,bbmax;
  GetBoundingBox(bbmin,bbmax);
  std::list<BIAS::HomgPoint3D > listvertices = 
    CornersFromBoundingBox(bbmin,bbmax);

  vector< Vector3<double>  > vertices;
  vertices.resize(listvertices.size());
  list<HomgPoint3D>::iterator it = listvertices.begin();
  int i = 0;
  for(; it!=listvertices.end(); it++) {
    vertices[i++] = it->GetEuclidian();
  }

  glColor3f(255,0,0);
  glBegin(GL_LINES);
  glVertex3dv(vertices[0].GetData());     
  glVertex3dv(vertices[1].GetData());
  glVertex3dv(vertices[1].GetData());     
  glVertex3dv(vertices[2].GetData());  
  glVertex3dv(vertices[2].GetData());     
  glVertex3dv(vertices[3].GetData());
  glVertex3dv(vertices[3].GetData());     
  glVertex3dv(vertices[0].GetData());
  glVertex3dv(vertices[4].GetData());     
  glVertex3dv(vertices[5].GetData());
  glVertex3dv(vertices[5].GetData());     
  glVertex3dv(vertices[6].GetData());
  glVertex3dv(vertices[6].GetData());     
  glVertex3dv(vertices[7].GetData());
  glVertex3dv(vertices[7].GetData());     
  glVertex3dv(vertices[4].GetData());
  glVertex3dv(vertices[0].GetData());     
  glVertex3dv(vertices[4].GetData());
  glVertex3dv(vertices[5].GetData());     
  glVertex3dv(vertices[1].GetData());
  glVertex3dv(vertices[2].GetData());     
  glVertex3dv(vertices[6].GetData());
  glVertex3dv(vertices[3].GetData());     
  glVertex3dv(vertices[7].GetData());
  glEnd();

}


void SceneBase::AnimationAndCollision(std::vector<SceneBase*> scenes,
                                      const Vector3<double>& gravity,
                                      const double& friction) {

  if (animationSpeed_>0.0) {
    // simple animation:
    for (unsigned int i=0; i<scenes.size(); i++) {
      // 9.81 m/s^2 
      scenes[i]->ApplyTimeStep(animationSpeed_, gravity, friction); // 0 10 0
    }
    // check for collision
    for (unsigned int i=0; i<scenes.size(); i++) {
      for (unsigned int j=i+1; j<scenes.size(); j++) {
        if (scenes[i]->GetGravityImmunity() &&
            scenes[j]->GetGravityImmunity()) {
          //      BIASWARN("no weight, no hit.");
          // dont check static scene
          continue;
        }

        if (scenes[i]->HasBoundingBoxCollision(scenes[j])) {
          double timetocontact = 1.0;
          //BIASWARN("detected collision between scenes "<<i<<" and "<<j);
          // collision !    change momentum !
          //BIASWARN("one step back for colliding objects");
          scenes[i]->MoveScene(-animationSpeed_);
          scenes[j]->MoveScene(-animationSpeed_);

          Vector3<double> contactnormal(0,0,1);
          double ttc = scenes[i]->TimeToContact(scenes[j], contactnormal);
          if (ttc<0.0) ttc = 0.0;
          if (ttc>animationSpeed_) ttc = animationSpeed_;
          
          timetocontact = ttc;
          scenes[i]->MoveScene(timetocontact);
          scenes[j]->MoveScene(timetocontact);
          
          if (isnan(contactnormal[0]) ||isnan(contactnormal[1])
              ||isnan(contactnormal[2]) ) {
            BIASWARN("contact normal is nan !");
            contactnormal=Vector3<double>(-1e10,-1e10,-1e10);
          }

          //BIASWARN("Before collision velocities are "<<scenes[i]->velocity_<<
          //         " and "<<scenes[j]->velocity_<<", now colliding");
          scenes[i]->PerformCollision(scenes[j],contactnormal);
          //BIASWARN("After collision velocities are  "<<scenes[i]->velocity_<<
          //         " and "<<scenes[j]->velocity_);


          // this would require another collision test:
          scenes[i]->MoveScene(animationSpeed_-timetocontact);
          scenes[j]->MoveScene(animationSpeed_-timetocontact);
        
        }
        //cout<<"XXX velocities are "<<scenes_[i]->velocity_<<
        //  " and "<<scenes_[j]->velocity_<<endl;

      }
    }
  }
}