PhysicalObject.cpp

Go to the documentation of this file.

#include "PhysicalObject.h"
#include "Simulator.h"
#include "Integrator.h"
#include "purgeContainer.h"
#include "ContactInfo.h"
#include "GlobalContactInfo.h"

PhysicalObject::PhysicalObject(std::string iniLabel, int nSensors,
                        real saturationForce,
                        Color outlineColor, Color fillColor) :
      GraphicalObject(outlineColor, fillColor),
    label(iniLabel), nSensors(nSensors), saturationForce(saturationForce)
{
   //vectors are initialized with 0 by their constructors
   m=0.0;
   alpha=0.0;
   omega=0.0;
   I=0.0;
   externalTorque=0;
   parent=NULL;
   relativeAlpha=0.0;
   if(nSensors){
      activations=new real[nSensors];
      for(int i=0; i<nSensors; i++)
         activations[i]=0.0;
   } else {
      activations=NULL;
   }
   boxMin.setXY(-ThyrixParameters::infinity,-ThyrixParameters::infinity);
   boxMax.setXY(+ThyrixParameters::infinity,+ThyrixParameters::infinity);
}

PhysicalObject::~PhysicalObject(){
   deleteContacts();
   if(activations!=NULL)
      delete[] activations;
}

void PhysicalObject::registerPrimitives(Simulator* simulator){
   simulator->registerPrimitive(this);
}

void PhysicalObject::deleteContacts() { 
   purgeContainer(contacts); 
   resetSensors();
}

void PhysicalObject::resetSensors(){
   if(nSensors){
      for(unsigned int i=0; i<nSensors; i++)
         activations[i]=0;
   }
}


void PhysicalObject::integrate(const Integrator &integrator) { 
    rOld = r;
    integrator.integrate(r, v);
    alphaOld = alpha;
    integrator.integrate(alpha, omega);
    normalizeAlpha();
      computeBox();
}

void PhysicalObject::rollback() {
    r=rOld;
    alpha=alphaOld;
      computeBox();
}

void PhysicalObject::computeDerivativesWithoutContacts(ContactSolver* contactSolver) { 
    v=externalForce;
      v/=m;
    omega=externalTorque/I;
      //reset external force and torque
      externalForce.setToZero();
      externalTorque=0;
}
   
void PhysicalObject::computeDerivatives(GlobalContactInfoVector* globalContacts){
   ContactInfo* current;
   Vector2 vt;

   for(ContactInfoPVector::iterator it=contacts.begin(), end=contacts.end();
        it!=end;++it){
      current=*it;
      vt=current->n;
      vt*=current->force*current->sigma/m;
      v+=vt;
      omega+=current->force*current->sigma*current->pxn/I;     
   }
   setSensors();
}

void PhysicalObject::fillContactMatrix(ContactSolver* contactSolver, ContactInfo* contact){
   /*
   contactInfo is the info of contact alpha relative to the current object
   
   Assumes v_0, w_0 (the velocity without the contribution of the contacts) are
   already computed as v, omega. 
   
   This method works for both simple objects and composed objects.

   */
   ContactInfo* current;
   Vector2 vt1, vt2;

   for(ContactInfoPVector::iterator it=contacts.begin(), end=contacts.end();
        it!=end; ++it){
      current=*it;
      vt1=current->n;
      vt1/=m;
      vt2=contact->p;
      vt2.cross(current->pxn);
      vt2/=I;
      vt1+=vt2;
      contactSolver->contactMatrix[contact->alpha][current->alpha]+=
         vt1*contact->n*contact->sigma*current->sigma;      
   }
   vt1=contact->p;
   vt1.cross(omega);
   vt1+=v;
   contactSolver->contactVector[contact->alpha]+=vt1*contact->n*contact->sigma;
}

void PhysicalObject::setSensors(){
   if(nSensors){
      for(ContactInfoPVector::iterator it=contacts.begin(), end=contacts.end();
               it!=end; ++it){
         setSensor(*it);
      }
   }
}

void PhysicalObject::drawContactForces(GUI* gui){
   for(ContactInfoPVector::iterator it=contacts.begin(), end=contacts.end();
        it != end; ++it){
        ContactInfo* current = *it;
      Vector2 rt = r + current->p;
      Vector2 force = current->n * 
            (current->force * current->sigma);
      gui->drawForce(rt, force);
   }
} 

Thyrix homepage • Users' guide

(C) Arxia 2004-2005