CappedRectangle.cpp

Go to the documentation of this file.

#include "CappedRectangle.h"
#include "Simulator.h"
#include "Circle.h"
#include "Border.h"

CappedRectangle::CappedRectangle(real l, real R,
     real x, real y, real alpha, 
     std::string label,
       int nSensorsLateral, int nSensorsSemicircle,
       real saturationForce,
       Color outlineColor, Color fillColor) : 
   nSensorsLateral(nSensorsLateral),
   nSensorsSemicircle(nSensorsSemicircle),
  PhysicalObject(label, 2*(nSensorsLateral+nSensorsSemicircle), 
                                    saturationForce, outlineColor, fillColor){
   this->l=l;
   this->R=R;
   r.x=x;
   r.y=y;
   this->alpha=alpha;
   computeMass();
   computeInertia();
   computeBox();
}

CappedRectangle::~CappedRectangle(){
}

void CappedRectangle::computeMass(real density){
   //rectangle 2l x 2R and 2 half-circles of radius R
   m = density * R * (4*l + (M_PI)*R);
}

void CappedRectangle::computeInertia(){
   I = m * (8.0*l*l + 5.0*R*R) / 6.0;
}

int CappedRectangle::getISensorSemicircle(Vector2& p, int sign){
   if(nSensorsSemicircle){
      real u, startAngle;
      unsigned iSensor;
      u = atan2(p.y, p.x); //-Pi..Pi
      if(sign>0){
         startAngle=3*M_PI/2;
         iSensor=2*nSensorsLateral+nSensorsSemicircle;
      } else {
         startAngle=M_PI/2;
         iSensor=nSensorsLateral;
      }
      u-=alpha+startAngle; //-5Pi..Pi
      u/=2*M_PI; //-5/2..1/2
      while (u<0) u+=1; 
      while (u>=1) u-=1;   // [0..1)
      return (unsigned)(floor(u*nSensorsSemicircle))+iSensor;
   } else {
      return -1;
   }
}


void CappedRectangle::setISensorLateral(int& i1, int& i2, Vector2& p, real e1, real e2){
   real side=getLateralSide(p);
   i1=getISensorLateral(side, e1);
   i2=getISensorLateral(side, e2);
}

int CappedRectangle::getISensorLateral(Vector2& p, real e){
   real side=getLateralSide(p);
   return getISensorLateral(side, e);
}

int CappedRectangle::getISensorLateral(real side, real e){
   assert(-l<=e && e<=l);
   if(nSensorsLateral){
      int i;
      if(side>0){
         //top side
         i=floor((l-e)/2/l*nSensorsLateral);
         if(i==nSensorsLateral) i--;
      } else {
         //bottom side
         i=floor((l+e)/2/l*nSensorsLateral);
         if(i==nSensorsLateral) i--;
         i+=nSensorsLateral+nSensorsSemicircle;
      }
      return i;
   } else {
      return -1;
   }
}

bool CappedRectangle::detectContacts(Border* border, GlobalContactInfoVector* contacts){
   real d1, d2, delta, delta1, delta2, ut;
   Vector2 p;
   ContactInfo* contact1;
   ContactInfo* contact2;
   real penetration=0.0f;
   bool isContact=false;
   
   //capsule - border contact
   d1=   (r.x+l*cosAlpha-border->r.x)*border->normal.x+
         (r.y+l*sinAlpha-border->r.y)*border->normal.y;
   delta1=d1-R;
   d2=   (r.x-l*cosAlpha-border->r.x)*border->normal.x+
         (r.y-l*sinAlpha-border->r.y)*border->normal.y;
   delta2=d2-R;
   delta1<delta2 ? delta=delta1 : delta=delta2;
   if (delta<ThyrixParameters::epsilonContact){
      isContact=true;
      //contact1: of the capsule
      p=border->normal;
      p*=-R;
      
      
      if(delta1<ThyrixParameters::epsilonContact && delta2<ThyrixParameters::epsilonContact){
         /* Parallel case: the capsule is parallel to the border.
         */
         
         /*
         real qt=(delta2-delta1)/(EPSILONCONTACT*2)*l;
         if(qt<-l) qt=-l; else if(qt>l) qt=l;
         p.x+=qt*cosAlpha;
         p.y+=qt*sinAlpha;
         */

         if (delta < 0) {
            penetration=-delta*l;
         }
         
         int im, ip;
         setISensorLateral(im, ip, p, -l, l);

         if(contacts!=NULL){
            Vector2 pt;
            pt=p;
            pt.x+=l*cosAlpha;
            pt.y+=l*sinAlpha;
            contact1=new ContactInfo(this, 1, pt, border->normal, ip, im);
            pt.setToZero();
            contact2=new ContactInfo(border, -1, pt, border->normal);
            contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));

            pt=p;
            pt.x-=l*cosAlpha;
            pt.y-=l*sinAlpha;
            contact1=new ContactInfo(this, 1, pt, border->normal, im, ip);
            pt.setToZero();
            contact2=new ContactInfo(border, -1, pt, border->normal);
            contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));
         }
         
      } else {
         int sign;
         Vector2 p0(p);
         if(delta1<delta2) {
            p.x+=l*cosAlpha;
            p.y+=l*sinAlpha;
            sign=1;
         } else {
            p.x-=l*cosAlpha;
            p.y-=l*sinAlpha;
            sign=-1;
         }
         if(delta<0){
            ut=R*R-sqr(d1<d2 ? d1 : d2);
            if(ut>0)
               penetration=-delta*sqrt(ut);
            else
               penetration=0.0;
         }

         if(contacts!=NULL){
            contact1=new ContactInfo(this, 1, p, border->normal, getISensorSemicircle(p0, sign));

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

            contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));
         }

      }
      
   }  //if contact
   return isContact;
}


bool CappedRectangle::detectContacts(Circle* circle, GlobalContactInfoVector* contacts){
   real d, delta, ut;
   Vector2 rt,rtt;
   ContactInfo* contact1;
   ContactInfo* contact2;
   real penetration=0.0f;
   bool isContact=false;

   real kContact;
   int iSensor=-1;
   rt=circle->r;
   rt-=r;
   d=fabs(rt.x*sinAlpha-rt.y*cosAlpha);
   delta=d-R-circle->R;
   if (delta<ThyrixParameters::epsilonContact){
      real k=rt.x*cosAlpha+rt.y*sinAlpha;
      //TODO: de optimizat inmultirile
      if(k>l){
         rtt.setXY(rt.x-l*cosAlpha,rt.y-l*sinAlpha);
         d=rtt.getModule();
         delta=d-R-circle->R;
         if(delta<ThyrixParameters::epsilonContact){
            isContact=true;
            kContact=l;
            
            //set iSensor of the capsule
            iSensor=getISensorSemicircle(rtt,1);
            
            //set penetration
            if(delta<0){
               if(d>1e-6){
                  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;
               }
            }

         }
      } else if(k<-l) {
         rtt.setXY(rt.x+l*cosAlpha,rt.y+l*sinAlpha);
         d=rtt.getModule();
         delta=d-R-circle->R;
         if(delta<ThyrixParameters::epsilonContact){
            isContact=true;
            kContact=-l;

            //set iSensor of the capsule
            iSensor=getISensorSemicircle(rtt,-1);
            
            //set penetration
            if(delta<0){
               if(d>1e-6){
                  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;
               }
            }

         }
      } else {
         isContact=true;
         kContact=k;

         //set iSensor of the capsule
         iSensor=getISensorLateral(rt, k);

         //set penetration
         if(delta<0){
            ut=sqr(circle->R)-sqr(d-R);
            if(ut>0)
               penetration=-delta*sqrt(ut);
            else
               penetration=0.0;
         }
            
      }
      if(isContact && contacts!=NULL){
         //simulator->outText("Found capsule-circle contact\n");
         Vector2 n;
         n=r;
         n.x+=kContact*cosAlpha;
         n.y+=kContact*sinAlpha;
         n-=circle->r;
         n.normalize();

         //contact1: of the capsule
         rt=n;
         rt*=-R;
         rt.x+=kContact*cosAlpha;
         rt.y+=kContact*sinAlpha;
         contact1=new ContactInfo(this, 1, rt, n, iSensor);

         //contact2: of the circle
         rt=n;
         rt*=circle->R;
         contact2=new ContactInfo(circle, -1, rt, n, circle->getISensor(n));
         contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));
      }
   }
   return isContact;
}

bool CappedRectangle::detectContacts(CappedRectangle* capsule, GlobalContactInfoVector* contacts){ 
   real d, delta;
   Vector2 rt;
   ContactInfo* contact1;
   ContactInfo* contact2;
   real penetration=0.0f;
   bool isContact=false;
   bool wasContactCreated=false;
   real& R1=R;
   real& R2=capsule->R;
   real& l1=l;
   real& l2=capsule->l;

   rt = r - capsule->r;
   if(rt.getSquaredModule()<sqr(R1+l1+R2+l2+ThyrixParameters::epsilonContact)){
      //contact possibility      
      int iSensor1, iSensor2;
      real& sinAlpha1=sinAlpha;
      real& cosAlpha1=cosAlpha;
      real& sinAlpha2=capsule->sinAlpha;
      real& cosAlpha2=capsule->cosAlpha;
      real p=cosAlpha1*cosAlpha2+sinAlpha1*sinAlpha2;
      int sigma, sigma1, sigma2;
      Vector2 u;
      Vector2 p1, p2, n;
      Vector2 rtt;
      real ut;
      real ru1, ru2, rtu;
      real dperp;
      real contactLength;
      
      if(1-fabs(p)<1e-4){
         //parallel case
         int iSensor1a, iSensor1b, iSensor2a, iSensor2b;
         sigma = (p > 0) ? 1 : -1;
         u.setXY((cosAlpha1+sigma*cosAlpha2)/2,(sinAlpha1+sigma*sinAlpha2)/2);
         dperp=fabs(rt.x*u.y-rt.y*u.x);
         d=dperp;
         delta=d-R1-R2;
         if(delta<ThyrixParameters::epsilonContact){
            real ru1p, ru1m, ru2p, ru2m;
            ru1=r*u;
            ru2=capsule->r*u;
            rtu=ru1-ru2;
            ru1p=ru1+l1;
            ru1m=ru1-l1;
            ru2p=ru2+l2;
            ru2m=ru2-l2;
            // segment 1: () ; segment 2: []
            if(ru1m>ru2p){
               //case a ()[]
               d=sqrt(sqr(ru1m-ru2p)+sqr(dperp));
               delta=d-R1-R2;
               if(delta<ThyrixParameters::epsilonContact){
                  isContact=true;
                  n.setXY(-l1*cosAlpha1-l2*cosAlpha2,-l1*sinAlpha1-l2*sinAlpha2); 
                  n+=rt;
                  n.normalize();
                  p1=n; p1*=-R1; 
                  iSensor1=getISensorSemicircle(p1,-1);
                  rtt.setXY(-l1*cosAlpha1,-l1*sinAlpha1); 
                  p1+=rtt;
                  p2=n; p2*=R2; 
                  iSensor2=capsule->getISensorSemicircle(p2,sigma);
                  rtt.setXY(l2*cosAlpha2,l2*sinAlpha2); 
                  p2+=rtt;
                  
                  
                  //compute penetration surface
                  if(delta<0){
                     if(d>1e-6){
                        ut=(-d+R2-R1)*(-d-R2+R1)*(-delta)*(-d+R2+R1);
                        if(ut>0)
                           penetration=-delta*sqrt(ut)/(2.0*d);
                        else
                           penetration=0.0;
                     } else {
                        penetration=M_PI*R1*R2;
                     }
                  }

               }
            } else if(ru1p>ru2m){
               //case b [ ) && ( ]
               //parallel contact
               Vector2 p1a, p1b, p2a, p2b;
               //simulator->outText("Parallel case b\n");

               //compute the normal n
               n=u; n*=-(rt*u); n+=rt;
               ut=n.getModule();
               if(ut==0.0){
                  n=rt;
                  n.normalize();
               } else {
                  n/=ut;
               }

               real lambda1a, lambda1b, lambda2a, lambda2b;
               
               if(ru1p>ru2p) {
                  // [ ] ) && ( ]
                  if(ru1m>ru2m){
                     // case b1: [ ( ] )
                     contactLength=(ru2+l2)-(ru1-l1);
                     lambda1a=-l1;
                     lambda1b=-l1+contactLength;
                     lambda2a=l2-contactLength;
                     lambda2b=l2;
                  } else {
                     // case b2: ( [ ] )
                     contactLength=l2*2;
                     lambda1a=-rtu-l2;
                     lambda1b=-rtu+l2;
                     lambda2a=-l2;
                     lambda2b=l2;
                  }
               } else {
                  // [ ) ] && ( ]
                  if(ru1m>ru2m){
                     // case b3: [ ( ) ]
                     contactLength=l1*2;
                     lambda1a=-l1;
                     lambda1b=l1;
                     lambda2a=rtu-l;
                     lambda2b=rtu+l;
                  } else {
                     // case b4: ( [ ) ]
                     contactLength=(ru1+l1)-(ru2-l2);
                     lambda1a=l1-contactLength;
                     lambda1b=l1;
                     lambda2a=-l2;
                     lambda2b=-l2+contactLength;
                  }
               }

               p1=n; p1*=-R1; 
               p1a=p1b=p1;
               p2=n; p2*=R2; 
               p2a=p2b=p2;

               rtt=u; rtt*=lambda1a; p1a+=rtt;
               rtt=u; rtt*=lambda1b; p1b+=rtt;
               setISensorLateral(iSensor1a, iSensor1b, p1, lambda1a, lambda1b);
               
               rtt=u; rtt*=lambda2a; p2a+=rtt;
               rtt=u; rtt*=lambda2b; p2b+=rtt;
               capsule->setISensorLateral(iSensor2a, iSensor2b, p2, sigma*lambda2a, 
                                                   sigma*lambda2b);

               if(delta<0)
                  penetration=-delta*contactLength/2;

               isContact=true;
               wasContactCreated=true;

               if(contacts!=NULL){
                  //parallel contact
                  contact1=new ContactInfo(this, 1, p1a, n, iSensor1a, iSensor1b);
                  contact2=new ContactInfo(capsule, -1, p2a, n, iSensor2a, iSensor2b);
                  contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));

                  contact1=new ContactInfo(this, 1, p1b, n, iSensor1b, iSensor1a);
                  contact2=new ContactInfo(capsule, -1, p2b, n, iSensor2b, iSensor2a);
                  contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));
               }

            } else {
               //case c
               d=sqrt(sqr(ru2m-ru1p)+sqr(dperp));
               delta=d-R-R2;
               if(delta<ThyrixParameters::epsilonContact){
                  isContact=1;
                  //simulator->outText("Parallel case c\n");
                  n.setXY(l*cosAlpha1+l2*cosAlpha2,l*sinAlpha1+l2*sinAlpha2); 
                  n+=rt;
                  n.normalize();
                  p1=n; p1*=-R1; 
                  iSensor1=getISensorSemicircle(p1,1);
                  rtt.setXY(l*cosAlpha1,l*sinAlpha1); p1+=rtt;
                  p2=n; p2*=R2; 
                  iSensor2=capsule->getISensorSemicircle(p2,-sigma);
                  rtt.setXY(-l2*cosAlpha2,-l2*sinAlpha2); p2+=rtt;
                  
                  if(delta<0){
                     if(d>1e-6){
                        ut=(-d+R2-R)*(-d-R2+R)*(-delta)*(-d+R2+R);
                        if(ut>0)
                           penetration=-delta*sqrt(ut)/(2.0*d);
                        else
                           penetration=0.0;
                     } else {
                        penetration=M_PI*R2*R;
                     }
                  }
               }
            }
         }
         
      } else {
         //non-parallel case
         ru1=rt.x*cosAlpha1+rt.y*sinAlpha1;
         ru2=rt.x*cosAlpha2+rt.y*sinAlpha2;
         real lambda1, lambda2, lambda1m, lambda2m, lambda1mp, lambda2mp, dp;
         ut=p*p-1.0;
         lambda1=(ru1-p*ru2)/ut;
         lambda2=(p*ru1-ru2)/ut;
         if(lambda1>-l1 && lambda1<l1 && lambda2>-l2 && lambda2<l2){
            //intersection case
            isContact=true;
            //simulator->outText("Intersection case\n");
            lambda1<0 ? sigma1=1 : sigma1=-1;
            lambda2<0 ? sigma2=1 : sigma2=-1;
            n.setXY(sigma1*cosAlpha1-sigma2*cosAlpha2,sigma1*sinAlpha1-sigma2*sinAlpha2);
            n.normalize();
            p1.setXY(lambda1*cosAlpha1,lambda1*sinAlpha1);
            p2.setXY(lambda2*cosAlpha2,lambda2*sinAlpha2);
            rtt.setXY(-sigma1*cosAlpha1,-sigma1*sinAlpha1);
            iSensor1=getISensorSemicircle(rtt,-sigma1);
            rtt.setXY(sigma2*cosAlpha2,sigma2*sinAlpha2);
            iSensor2=capsule->getISensorSemicircle(rtt,-sigma2);
            penetration=4*R*R2/sqrt(-ut);
         } else {
            //non-intersection case
            lambda1>0 ? sigma1=1 : sigma1=-1;
            lambda2>0 ? sigma2=1 : sigma2=-1;

            bool e1, e2, e1p, e2p; //indicate whether the contact is at end (true) or in the middle (false)

            lambda1m=sigma1*l; e1=true;
            lambda2m=-(lambda1-lambda1m)*p+lambda2;
            if(lambda2m<-l2) {
               lambda2m=-l2;
               e2=true;
            } else if(lambda2m>l2) {
               lambda2m=l2;
               e2=true;
            } else {
               e2=false;
            }

            rtt.setXY(lambda1m*cosAlpha1-lambda2m*cosAlpha2,lambda1m*sinAlpha1-lambda2m*sinAlpha2);
            rtt+=rt;
            d=rtt.getModule();
            
            lambda2mp=sigma2*l2; e2p=true;
            lambda1mp=-(lambda2-lambda2mp)*p+lambda1;
            if(lambda1mp<-l) {
               lambda1mp=-l; 
               e1p=true;
            } else if(lambda1mp>l) {
               lambda1mp=l;            
               e1p=true;
            } else {
               e1p=false;
            }
            rtt.setXY(lambda1mp*cosAlpha1-lambda2mp*cosAlpha2,lambda1mp*sinAlpha1-lambda2mp*sinAlpha2);
            rtt+=rt;
            dp=rtt.getModule();
            
            if(dp<d){
               lambda1m=lambda1mp;
               lambda2m=lambda2mp;
               d=dp;
               e1=e1p;
               e2=e2p;
            }
            delta=d-R-R2;
            if(delta<ThyrixParameters::epsilonContact){
               isContact=true;
               //simulator->outText("Non-intersection case\n");
               n.setXY(lambda1m*cosAlpha1-lambda2m*cosAlpha2,lambda1m*sinAlpha1-lambda2m*sinAlpha2);
               n+=rt;
               n.normalize();
               p1.setXY(lambda1m*cosAlpha1,lambda1m*sinAlpha1); 
               rtt=n; rtt*=-R; p1+=rtt;
               if(e1){
                  iSensor1=getISensorSemicircle(rtt, lambda1m>0?1:-1);
               } else {
                  iSensor1=getISensorLateral(p1, lambda1m);
               }
               p2.setXY(lambda2m*cosAlpha2,lambda2m*sinAlpha2); 
               rtt=n; rtt*=R2; p2+=rtt;
               if(e2){
                  iSensor2=capsule->getISensorSemicircle(rtt, lambda2m>0?1:-1);
               } else {
                  iSensor2=capsule->getISensorLateral(p2, lambda2m);
               }

               if(delta<0){
                  if(d>1e-6){
                     ut=(-d+R2-R)*(-d-R2+R)*(-delta)*(-d+R2+R);
                     if(ut>0)
                        penetration=-delta*sqrt(ut)/(2.0*d);
                     else
                        penetration=0.0;
                  } else {
                     penetration=M_PI*R2*R;
                  }
               }
                  //only circle-circle case considered for penetration, for simplicity
                  //TODO: verify in which case we are, add circle - rectangle case
            }
         }
      }
      if (isContact && !wasContactCreated && contacts!=NULL) {
         //simulator->outText("Found capsule-capsule contact\n");
         
         //contact1: of this capsule
         contact1=new ContactInfo(this, 1, p1, n, iSensor1);
   
         //contact2: of the other capsule
         contact2=new ContactInfo(capsule, -1, p2, n, iSensor2);
         contacts->push_back(GlobalContactInfo(contact1, contact2, penetration));
      }
   }
   return isContact;
}

bool CappedRectangle::detectMouseContact(const Vector2& rMouse, Vector2& p, PhysicalObject*& object){
   Vector2 rt=rMouse;
   rt-=r;
   real d=fabs(rt.x*sinAlpha-rt.y*cosAlpha);
   if (d<=R){
      real k=rt.x*cosAlpha+rt.y*sinAlpha;
      Vector2 rtt;
      if(k>l){
         rtt.setXY(rt.x-l*cosAlpha,rt.y-l*sinAlpha);
         d=rtt.getModule();
         if(d>R){
            return false;
         }
      } else if(k<-l){
         rtt.setXY(rt.x+l*cosAlpha,rt.y+l*sinAlpha);
         d=rtt.getModule();
         if(d>R){
            return false;
         }
      }
      rt.rotate(-alpha);
      p=rt;
      object=this;
      return true;
   } else {
      return false;
   }
}

void CappedRectangle::setSensor(ContactInfo* contact){
   if(contact->iSensor>=0){
      real f;
      if(contact->type!=contactTypeForceParallel){
         //single point contact
         f=contact->force/saturationForce+activations[contact->iSensor];
         if(f>1) f=1.0;
         activations[contact->iSensor]=f;
      } else {
         
         //parallel contact
         assert(contact->iSensor>=0 && contact->iSensor2>=0);
         int n=contact->iSensor2-contact->iSensor;
         int sign;
         if(n>0){
            sign=1;
         } else {
            sign=-1;
            n=-n;
         }
         n++;
         int i=0, j;
         real k=contact->force/saturationForce*0.5/n/(n-1);
         for(i=0;i<n;i++){
            j=contact->iSensor+i*sign;
            assert(j>=0);
            assert(j<nSensors);
            f=i*k+activations[j];
            if(f>1) f=1.0;
            activations[j]=f;
         }
         
      }
   }
}

void CappedRectangle::draw(GUI *gui){
   gui->setPenColor(outlineColor);
   gui->setBrushColor(fillColor);
   gui->drawCappedRectangle(r.x, r.y, l, R, alpha);
   //gui->setPenColor(GUI::RED);
   //drawContactForces(gui);
   //drawSensors(gui);
   //gui->setPenColor(GUI::BLACK);
}

void CappedRectangle::drawSensors(GUI *gui){
   if(nSensors){
      //gui->setPenColor(GUI::RED);
      int i, i0;
      real x, y, dx, dy;
      real h=0.5; //==R;


      if(nSensorsSemicircle){
         real u;  
         i0=2*nSensorsLateral+nSensorsSemicircle;
         x=r.x+l*cosAlpha;
         y=r.y+l*sinAlpha;
         for(i=0; i<nSensorsSemicircle; i++){
            u=alpha+3*M_PI/2+(i+0.5)*2*M_PI/nSensorsSemicircle;
            dx=cos(u);
            dy=sin(u);
            gui->drawLine(x+R*dx, y+R*dy, x+(R-h*activations[i+i0])*dx, 
               y+(R-h*activations[i+i0])*dy);
         }
         i0=nSensorsLateral;
         x=r.x-l*cosAlpha;
         y=r.y-l*sinAlpha;
         for(i=0; i<nSensorsSemicircle; i++){
            u=alpha+M_PI/2+(i+0.5)*2*M_PI/nSensorsSemicircle;
            dx=cos(u);
            dy=sin(u);
            gui->drawLine(x+R*dx, y+R*dy, x+(R-h*activations[i+i0])*dx, 
               y+(R-h*activations[i+i0])*dy);
         }
      }
      //gui->setPenColor(GUI::GREEN);
      
      if(nSensorsLateral){
         real e;
         i0=0;
         dx=-sinAlpha; dy=cosAlpha;
         for(i=0;i<nSensorsLateral; i++){
            e=(0.5-(i+0.5)/nSensorsLateral)*2*l;
            x=r.x+e*cosAlpha; y=r.y+e*sinAlpha;
            gui->drawLine(x+R*dx, y+R*dy, x+dx*(R-h*activations[i+i0]), y+dy*(R-h*activations[i+i0]));
         }
         i0=nSensorsLateral+nSensorsSemicircle;
         dx=-dx;dy=-dy;
         for(i=0;i<nSensorsLateral; i++){
            e=((i+0.5)/nSensorsLateral-0.5)*2*l;
            x=r.x+e*cosAlpha; y=r.y+e*sinAlpha;
            gui->drawLine(x+R*dx, y+R*dy, x+dx*(R-h*activations[i+i0]), y+dy*(R-h*activations[i+i0]));
         }
      }
      
   }
}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005