Circle.cpp

Go to the documentation of this file.

#include "Circle.h"
#include "Simulator.h"
#include "ContactInfo.h"
#include "GlobalContactInfo.h"


Circle::Circle(real R, real x, real y, std::string label, int nSensors,
            real sensorsStartAngle, real saturationForce,
             Color outlineColor, Color fillColor) : 
    PhysicalObject(label, nSensors, saturationForce, outlineColor, fillColor), 
      sensorsStartAngle(sensorsStartAngle)
{
   r.x=x;
   r.y=y;
   this->R=R;
   computeMass();
   computeInertia();
   computeBox();
}


Circle::~Circle(){
}



void Circle::setRadius (real R){
   this->R=R;
   computeMass();
   computeInertia();
   computeBox();
}

void Circle::computeMass(real density){
   m=density*(real)(M_PI)*R*R;
}

void Circle::computeInertia(){
   I=m*R*R/2.0;
}


bool Circle::detectContacts(Border* border, GlobalContactInfoVector* contacts){
   real d, delta, ut;
   Vector2 p;
   ContactInfo* contact1;
   ContactInfo* contact2;
   real penetration=(real)0.0;
   bool isContact=false;
   
   /*compute the distance between the circle and the border, as 
   (r_border-r_circle)*normal-R  */
   d= (r.x-border->r.x)*border->normal.x+
      (r.y-border->r.y)*border->normal.y;
   delta=d-R;

   if (delta<=ThyrixParameters::epsilonContact) {
      isContact=true;
      //contact1: of the circle
      p=border->normal;
      p*=-R;
      contact1=new ContactInfo(this, 1, p, border->normal, getISensor(p));

      //contact2: of the border
      p.setToZero();
      contact2=new ContactInfo(border, -1, p, border->normal);

      if(delta<0){
         ut=R*R-d*d;
         if(ut>0)
            penetration=-delta*sqrt(ut);
         else
            penetration=0.0;
      }
      if(contacts)
         contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));      
   }  //if contact
   return isContact;
}

bool Circle::detectContacts(Circle* circle, GlobalContactInfoVector* contacts){
   real penetration=0.0;
  real dx = circle->r.x - r.x;
   real dy = circle->r.y - r.y;
  real d2 = sqr(dx) + sqr(dy);
   real d = sqrt(d2);
   bool isContact=false;

   real delta = d - R - circle->R;
   if (delta <= ThyrixParameters::epsilonContact) {
      isContact=true;
      Vector2 n = r - circle->r;
      n.normalize();

      //contact1: of this circle
      Vector2 p = n * (-R);
      ContactInfo* contact1 = new ContactInfo(this, 1, p, n, getISensor(p));

      //contact2: of the other circle
      p = n * circle->R;
      ContactInfo* contact2 = new ContactInfo(circle, -1, p, n, circle->getISensor(p));


      if(delta<0){
         if(d>1e-6){
            real ut=(-d+circle->R-R)*(-d-circle->R+R)*(-delta)*(-d+circle->R+R);
            if(ut>0)
               penetration=-delta*sqrt(ut)/(2.0*d);
            else
               penetration=0.0;
         } else {
            penetration=M_PI*circle->R*R;
         }
      }
      if(contacts)
         contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));      
   }  //if contact
   return isContact;
}

bool Circle::detectMouseContact(const Vector2& rMouse, Vector2& p, PhysicalObject*& object){
   p=rMouse;
   p-=r;
   if(p.getSquaredModule()<=R*R){
      p.rotate(-alpha);
      /* The rotation of the vector representing the relative position of the dragging point is needed
      because the dragged object will rotate and the dragging point, which is fixed on the object, will
      rotate along with the object. Thus, p should be necessarily expressed in the object reference system.
      */
      object=this;
      return true;
   } else {
      return false;
   }
}

int Circle::getISensor(Vector2& p){
   if(nSensors){
      real u;
      u = atan2(p.y, p.x); //-Pi..Pi
      u-=alpha+sensorsStartAngle; //-5Pi..Pi
      u/=2*M_PI; //-5/2..1/2
      while (u<0) u+=1; 
      while (u>=1) u-=1;   // [0..1)
      return (int)(floor(u*nSensors));
   } else {
      return -1;
   }
}

void Circle::setSensor(ContactInfo* contact){
   if(contact->iSensor>=0){
      real f=contact->force/saturationForce+activations[contact->iSensor];
      if(f>1) f=1.0;
      activations[contact->iSensor]=f;
   }
}


void Circle::draw(GUI *gui) {
   gui->setPenColor(outlineColor);
   gui->setBrushColor(fillColor);
   gui->drawCircle(r.x, r.y, R);
   //gui->setPenColor(GUI::GREEN);
   //drawContactForces(gui);
   //drawSensors(gui);
   //gui->setPenColor(GUI::BLACK);
}

void Circle::drawSensors(GUI *gui){
   if(nSensors){
      //gui->setPenColor(GUI::RED);
      real u, dx, dy;
      int i;
      for(i=0; i<nSensors; i++){
         u=alpha+sensorsStartAngle+(i+0.5)*2*M_PI/nSensors;
         dx=R*cos(u);
         dy=R*sin(u);
         gui->drawLine(r.x+dx, r.y+dy, r.x+dx*(1-activations[i]), 
            r.y+dy*(1-activations[i]));
      }
   }
}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005