Simulator.cpp

Go to the documentation of this file.

#include "Simulator.h"
#include "Border.h"
#include "Circle.h"
#include "SystemSolver.h"
#include "purgeContainer.h"
#include <algorithm>


Simulator::Simulator() :
   integrator(dtDefault),
   draggedObject(NULL) {
      
   realTime=(real)0.0;

   isContact=0;

   contactSolver=new ContactSolver(&contacts);

   isRightMouseButtonPressed=false;

}

Simulator::~Simulator(){
   delete(contactSolver); 
   deleteObjects();
}



void Simulator::registerObject(PhysicalObject* object){
   objects.push_back(object);
   object->registerPrimitives(this); //register the object's primitives
   sortedObjects[0].push_back(object);
   sortedObjects[1].push_back(object);
}

void Simulator::registerPrimitive(PhysicalObject* object){
   primitives.push_back(object);
}

void Simulator::deleteObjects(){
   purgeContainer(objects);
}

void Simulator::controll(){
   PhysicalObjectPVector::iterator objectsI=objects.begin(), objectsEnd=objects.end();
   for(; objectsI!=objectsEnd; ++objectsI)
      (*objectsI)->controll();
}

void Simulator::advanceTime(){
   realTime += integrator.getDt();
   ++timeStep;


   PhysicalObjectPVector::iterator objectsI, objectsEnd;

   objectsEnd=objects.end();

   deleteContacts(); 
   //if we move contact deletion at the end of the cycle, there is no contact information to be drawn on the GUI

   detectContacts();
   
   isContact = !contacts.empty();

   if(isContact){ 
      indexContacts();     
   } //if is contact

   if(draggedObject){
      applyMouseForce();
   }
    
   for(objectsI=objects.begin(); objectsI!=objectsEnd; ++objectsI) {
      (*objectsI)->computeDerivativesWithoutContacts(contactSolver);
   }

   if(isContact){
      fillContactMatrix();
      contactSolver->computeContacts();
      contactSolver->uploadForces();
      for(objectsI=objects.begin(); objectsI!=objectsEnd; ++objectsI){
         (*objectsI)->computeDerivatives(&contacts);
      }        
   }

   //controll is called here because it may need information about derivatives, for example
   //proprioception of speed; it is also called before integration because it may need information
   //about the positions of objects
   controll();

   for(objectsI=objects.begin(); objectsI!=objectsEnd; objectsI++)
      (*objectsI)->integrate(integrator); 

   
}

/*
void Simulator::detectContacts(){
   // basic method, inefficient: n^2 complexity
   PhysicalObjectPVector::iterator objectsI1, objectsI2, objectsEnd=objects.end();
   for(objectsI1=objects.begin(); objectsI1!=objectsEnd; ++objectsI1){
      for(objectsI2 = objectsI1, ++objectsI2; 
            objectsI2 != objectsEnd; ++objectsI2) {
         (*objectsI1)->detectContacts(*objectsI2, &contacts);
        }
      (*objectsI1)->detectInternalContacts(&contacts);
   }
}
*/

/*
void Simulator::detectContacts(){
   // maintains a list of x-sorted objects
   sortObjects(0);
   PhysicalObjectPVector::iterator i1, i2, e=sortedObjects[0].end();
   for(i1=sortedObjects[0].begin(); i1!=e; ++i1){
      i2 = i1;
      ++i2;
      //we know that i2minx>=i1minx
      while(i2 != e && (*i2)->boxMin.x<=(*i1)->boxMax.x)  {
         if( ((*i1)->boxMin.y>=(*i2)->boxMin.y && (*i1)->boxMin.y<=(*i2)->boxMax.y) ||
               ((*i2)->boxMin.y>=(*i1)->boxMin.y && (*i2)->boxMin.y<=(*i1)->boxMax.y) ){
            (*i1)->detectContacts(*i2, &contacts);
         }
         ++i2;
      }
      (*i1)->detectInternalContacts(&contacts);
   }
}
*/


void Simulator::detectContacts(){
   // maintains 2 lists of x-sorted and y-sorted objects
   PhysicalObjectPVector::iterator i1, i2, e;
   ObjectPair o;

   sortObjects(0);
   xContacts.clear();
   e=sortedObjects[0].end();
   for(i1=sortedObjects[0].begin(); i1!=e; ++i1){
      i2 = i1;
      ++i2;
      while(i2 != e && (*i2)->boxMin[0]<=(*i1)->boxMax[0])  {
         xContacts.insert(ObjectPair(*i1, *i2));
         ++i2;
      }
      (*i1)->detectInternalContacts(&contacts);
   }

   sortObjects(1);
   xyContacts.clear();
   e=sortedObjects[1].end();
   for(i1=sortedObjects[1].begin(); i1!=e; ++i1){
      i2 = i1;
      ++i2;
      while(i2 != e && (*i2)->boxMin[1]<=(*i1)->boxMax[1])  {
         o=ObjectPair(*i1,*i2);
         if(xContacts.find(o)!=xContacts.end()) xyContacts.insert(o);
         ++i2;
      } 
   }
   
   std::set<ObjectPair>::iterator si=xyContacts.begin(),se=xyContacts.end();
   for(;si!=se;++si){
      (*si).o1->detectContacts((*si).o2,&contacts);
   }
}


void Simulator::sortObjects(int n){
   //insertion sort
   assert(n==0 || n==1);
   if(sortedObjects[n].size()>1){
      PhysicalObject* o;
      PhysicalObjectPVector::iterator ib=sortedObjects[n].begin(), ip=ib, i=ip, it, e=sortedObjects[n].end();
      PhysicalObjectPVector::reverse_iterator ir;
      ++i;
      while(i!=e){
         if((*i)->boxMin[n]<(*ip)->boxMin[n]){
            it=i;
            o=*i;
            do {
               *it=*(it-1);
               --it;
            }  while(it!=ib && o->boxMin[n]<(*(it-1))->boxMin[n]);
            *it=o;
         }
         ip=i;
         ++i;
      }
   }
}


void Simulator::indexContacts(){
   /* Set the indexes of contacts. */
   unsigned int k=0;
   for(GlobalContactInfoVector::iterator it = contacts.begin(), end = contacts.end(); it != end; ++it){
      it->contact1->alpha=it->alpha=k;
      if(it->contact2!=NULL)
         it->contact2->alpha=k;
      k++;
   }

   contactSolver->init();

}
 
void Simulator::fillContactMatrix(){
   for(GlobalContactInfoVector::iterator it = contacts.begin(), end = contacts.end(); 
        it != end; ++it){
      it->contact1->parentObject->fillContactMatrix(contactSolver, it->contact1);
      //contact2 is NULL for torque contacts
      if(it->contact2 != NULL) {
         it->contact2->parentObject->fillContactMatrix(contactSolver, it->contact2);
      }
   }
}



void Simulator::computeContacts(){
    contactSolver->computeContacts();
}

void Simulator::deleteContacts(){
   PhysicalObjectPVector::iterator objectsI, objectsEnd;
   for(objectsI=objects.begin(), objectsEnd=objects.end(); objectsI!=objectsEnd; ++objectsI)
      (*objectsI)->deleteContacts();
   contacts.clear();
}

void Simulator::setMouseForce(float x, float y){
   PhysicalObjectPVector::iterator objectsI, objectsEnd;
   bool objectFound=false;
   Vector2 r(x, y);
   Vector2 p;
   PhysicalObject* o;
   for(objectsI=objects.begin(), objectsEnd=objects.end(); objectsI!=objectsEnd && !objectFound; 
                        ++objectsI){
      objectFound=(*objectsI)->detectMouseContact(r, p, o);
      if(objectFound) {
         draggedObject=o;
         draggingPoint=p;
      }
   }
}

void Simulator::unsetMouseForce(){
   /*
   if(draggedObject){
      draggedObject->externalTorque=0;
      draggedObject->externalForce.setToZero();
   }
   */
   draggedObject=NULL;
}

void Simulator::applyMouseForce(){
   Vector2 p=draggingPoint;
   p.rotate(draggedObject->alpha);
   //p is the position of the dragging point relative to the object, 
   //expressed in the laboratory reference system

   Vector2 f=p;
   f+=draggedObject->r; //f is now the absolute position of the dragging point
   f.x=mouseX-f.x;
   f.y=mouseY-f.y; //f is now the position of the mouse relative to the dragging point
   f*=ThyrixParameters::kMouseForce; //f is now the force

   draggedObject->externalTorque+=p^f;
   draggedObject->externalForce+=f;
}


void Simulator::draw(GUI *gui){
   World::draw(gui);

   gui->setBrushColor(GUI::colorTransparent);    
   
   PhysicalObjectPVector::iterator it,end;
   for(it = objects.begin(), end = objects.end(); it != end; ++it) {
      (*it)->draw(gui);

      /*
      //draw bounding boxes
      gui->setBrushColor(GUI::colorTransparent);  
      gui->drawRectangle((*it)->boxMin.x,(*it)->boxMin.y,(*it)->boxMax.x-(*it)->boxMin.x,(*it)->boxMax.y-(*it)->boxMin.y);
      */
   }

   /*
   //draw primitives bounding boxes
   for(it = primitives.begin(), end = primitives.end(); it != end; ++it) {
      gui->drawRectangle((*it)->boxMin.x,(*it)->boxMin.y,(*it)->boxMax.x-(*it)->boxMin.x,(*it)->boxMax.y-(*it)->boxMin.y);
   }
   */
   
   

   /*
   //illustrate dragging action
   if(draggedObject){
      Vector2 r(draggingPoint);
      r.rotate(draggedObject->alpha);
      gui->drawLine(draggedObject->r+r,mouseX,mouseY);
   }
   */
}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005