Spherus.cpp

Go to the documentation of this file.

#include "Spherus.h"
#include "RandomController.h"
#include "Circle.h"

static const int nVisionSensors=7; //per circle
static const int nTactileSensors=8;//per circle
//1 proprioceptive extension sensor, 2*2 proprioceptive velocity sensors, 
//total sensors: 2*(nVisionSensors+nTactileSensors)+1+4=35
//2*2 rocket effectors, 1 extension effector
//total effectors: 5
static const real maxExtension=0.8;
static const real maxExtensionForce=0.5;
static const real maxRocketForce=0.5;
static const real elasticK=maxExtensionForce/(maxExtension/5);

const unsigned int Spherus::nSensors=2*(nVisionSensors+nTactileSensors)+1+4;
const unsigned int Spherus::nEffectors=2*2+1;

Spherus::Spherus(real x, real y, real alpha, real extension, real R, Controller* controller) : 
    PhysicalObject("Spherus"){
   this->r.setXY(x,y);
   this->alpha=alpha;
   this->computeSinCos();
   this->extension=extension;
   this->R=R;
   circles[0]=new Circle(R, 0,0, "left body", nTactileSensors, M_PI-M_PI/nTactileSensors, 0.1); //Pi-2Pi/8/2
   circles[1]=new Circle(R, 0,0, "right body", nTactileSensors, M_PI/nTactileSensors, 0.1); //2Pi/8/2
   for(int i=0;i<2;i++){
      eyes[i]=new VisualSensor(circles[i], 0,0, M_PI/2, R, 7*M_PI/12, nVisionSensors);
      rocketActivations[i]=0.0;
      rocketForceValues[i]=0.0;
   }
   extensionActivation=1.0;
   targetExtension=extensionActivation*maxExtension;
   computeMemberPositions();  
   if(controller==NULL){
      this->controller=new RandomController(35,5);
   } else {
      this->controller=controller;
   }
   //bounding box is infinite because of the visual sensors
   boxMin.setXY(-ThyrixParameters::infinity,-ThyrixParameters::infinity);
   boxMax.setXY(ThyrixParameters::infinity,ThyrixParameters::infinity);
}

Spherus::~Spherus(){
   for(int i=0;i<2;i++){
      delete(circles[i]);
      delete(eyes[i]);
   }
   delete controller;
}

void Spherus::computeState(){
   real dx=circles[1]->r.x-circles[0]->r.x;
   real dy=circles[1]->r.y-circles[0]->r.y;
   extension=sqrt(sqr(dx)+sqr(dy));
   alpha=atan2(dy,dx);//-Pi..Pi
   if(alpha<0) alpha+=2*M_PI; //0..2Pi
   computeSinCos();
   r=circles[0]->r;
   r+=circles[1]->r;
   r/=2;
}

void Spherus::computeSinCos(){
   sinAlpha=sin(alpha);
   cosAlpha=cos(alpha);
}

void Spherus::computeMemberPositions(){
   circles[0]->r.setXY(r.x-extension/2*cosAlpha,r.y-extension/2*sinAlpha);
   circles[1]->r.setXY(r.x+extension/2*cosAlpha,r.y+extension/2*sinAlpha);
}

void Spherus::controll(){
   int i, i0;
   /*
   Sensors:
   0..13 vision
   14..29 tactile
   30 length proprioception
   31..34 velocity proprioception

   Effectors:
   0..3 rockets
   4 extension
   */

   //set sensor activations
   //visual sensors
   for(i=0;i<nVisionSensors;i++)
      controller->setInput(i, eyes[1]->activations[i]);
   i0=i;
   for(;i<i0+nVisionSensors;i++)
      controller->setInput(i, eyes[0]->activations[i-i0]);
   i0=i;
   //tactile sensors
   for(;i<i0+nTactileSensors;i++)
      controller->setInput(i, circles[1]->activations[i-i0]);
   i0=i;
   for(;i<i0+nTactileSensors;i++)
      controller->setInput(i, circles[0]->activations[i-i0]);
   //extension proprioception
   controller->setInput(i, extension/maxExtension);
   i++;
   float p1,p2;
   //velocity proprioception
   computeCircleProprioception(circles[1],p1,p2);
   controller->setInput(i, p1); i++;
   controller->setInput(i, p2); i++;
   computeCircleProprioception(circles[0],p1,p2);
   controller->setInput(i, p1); i++;
   controller->setInput(i, p2); i++;
   
   controller->advanceTime();


   //srand( (unsigned)time( NULL ) );
   //each rocket is controlled by 2 neurons, one inhibitory and one excitatory
   //total activation: -1..1
   rocketActivations[1]=controller->getOutput(0)-controller->getOutput(1);
   rocketActivations[0]=controller->getOutput(2)-controller->getOutput(3);
   extensionActivation=controller->getOutput(4);
   
   for(i=0;i<2;i++){
      rocketForceValues[i]=rocketActivations[i]*maxRocketForce;
   }

   targetExtension=extensionActivation*maxExtension;
}

void Spherus::computeCircleProprioception(Circle* circle, float& forward, float& backward){
   real u=-(alpha+M_PI/2); //rotation angle
   u=circle->m*(circle->v.x*cos(u)-circle->v.y*sin(u))/maxRocketForce;
   if(u>0){
      if(u>1.0) u=1.0;
      forward=(float)u;
      backward=0.0;
   } else {
      if(u<-1.0) u=-1.0;
      forward=0.0;
      backward=(float)-u;
   }
}

void Spherus::computeDerivativesWithoutContacts(ContactSolver* contactSolver){
   real elasticForceValue;
   Vector2 force;
   Vector2 elasticForce;

   elasticForceValue=elasticK*(extension-targetExtension); 
   //elastic force applied towards body 0, oriented from body 0 to 1
   //elasticForceValue>0 => compression
   elasticForce.setXY(elasticForceValue*cosAlpha, elasticForceValue*sinAlpha);

   

   rocketForces[0].setXY(-rocketForceValues[0]*sinAlpha,rocketForceValues[0]*cosAlpha);
   force=rocketForces[0];
   force+=elasticForce;
   circles[0]->externalForce+=force;
   circles[0]->computeDerivativesWithoutContacts(contactSolver);

   rocketForces[1].setXY(-rocketForceValues[1]*sinAlpha,rocketForceValues[1]*cosAlpha);
   force=rocketForces[1];
   force-=elasticForce;
   circles[1]->externalForce+=force;
   circles[1]->computeDerivativesWithoutContacts(contactSolver);

}

bool Spherus::detectContacts(PhysicalObject* object, GlobalContactInfoVector* contacts){
   bool isContact=false;
   for(int i = 0; i < 2; ++i){
      isContact=circles[i]->detectContacts(object, contacts) || isContact;
      eyes[i]->detectContacts(object, contacts);
   }
   return isContact;
}

void Spherus::detectInternalContacts(GlobalContactInfoVector* contacts){
   circles[0]->detectContacts(circles[1], contacts);
}

void Spherus::deleteContacts(){
   for(int i=0;i<2;i++){
      circles[i]->deleteContacts();
      eyes[i]->deleteContacts();
   }
}

bool Spherus::detectMouseContact(const Vector2& rMouse, Vector2& p, PhysicalObject*& object){
   bool isContact=false;
   for(int i=0;i<2 && !isContact;i++){
      isContact = circles[i]->detectMouseContact(rMouse, p, object);
   }
   return isContact;
}

void Spherus::computeDerivatives(GlobalContactInfoVector* globalContacts){
   for(int i = 0; i < 2; ++i){
      circles[i]->computeDerivatives(globalContacts);
   }

}

void Spherus::integrate(const Integrator &integrator){
   int i;
   for(i=0;i<2;i++){
      circles[i]->integrate(integrator);
   }
   computeState();
   for(i=0;i<2;i++){
      circles[i]->alpha=alpha;
   }
}

void Spherus::draw(GUI *gui){
   gui->setBrushColor(Color("gray"));
   for(int i=0;i<2;i++){
      eyes[i]->draw(gui);
      circles[i]->draw(gui);
      gui->drawForce(circles[i]->r,rocketForces[i]);
      
   }
   gui->setBrushColor(GUI::colorTransparent);
   
   gui->setPenColor(Color("green"));
   gui->drawLine(circles[0]->r.x,circles[0]->r.y,circles[1]->r.x,circles[1]->r.y);
   gui->setPenColor(GUI::colorBlack);
}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005