VisualSensor.cpp

Go to the documentation of this file.

#include "VisualSensor.h"
#include "Circle.h"
#include "CappedRectangle.h"
#include "MathTools.h"


VisualSensor::VisualSensor(PhysicalObject* parent, 
            real relativeX, real relativeY, real relativeAlpha, 
            real R, real viewAngle, unsigned int nSensors,
            std::string label):
   PhysicalObject(label, nSensors),
   viewAngle(viewAngle)
{
   this->parent=parent;
   this->relativeR.setXY(relativeX, relativeY);
   this->relativeAlpha=relativeAlpha;
   assert(viewAngle>0 && viewAngle<=2*M_PI);
   delta=viewAngle/2;
   epsilon=viewAngle/nSensors;
   boxMin.setXY(-ThyrixParameters::infinity,-ThyrixParameters::infinity);
   boxMax.setXY(ThyrixParameters::infinity,ThyrixParameters::infinity);
}

VisualSensor::~VisualSensor(){
}

bool VisualSensor::detectContacts(Circle* circle, GlobalContactInfoVector* contacts){
   if(circle==parent) return false;
   
   real dx = circle->r.x - (parent->r.x+relativeR.x);
   real dy = circle->r.y - (parent->r.y+relativeR.y);
   real d = MathTools::modulus(dx,dy);

   //compute visual activation
   real gamma=computeGamma(dx,dy);

   assert(d > circle->R);
   real phi = asin(circle->R / d); //0..Pi/2
   
   real gammaM = gamma - phi; //-3Pi/2..Pi
   real gammaP = gamma + phi; //-Pi..3Pi/2

   activate(gammaM, gammaP);
   
   return false;
}

bool VisualSensor::detectContacts(CappedRectangle* capsule, GlobalContactInfoVector* contacts){
   if(capsule==parent) return false;

   real lx=capsule->cosAlpha*capsule->l;
   real ly=capsule->sinAlpha*capsule->l;
   
   real dx1 = capsule->r.x+lx - (parent->r.x+relativeR.x);
   real dy1 = capsule->r.y+ly - (parent->r.y+relativeR.y);
   real d1 = MathTools::modulus(dx1,dy1);

   real dx2 = capsule->r.x-lx - (parent->r.x+relativeR.x);
   real dy2 = capsule->r.y-ly - (parent->r.y+relativeR.y);
   real d2 = MathTools::modulus(dx2,dy2);

   //compute visual activation
   real gamma1=computeGamma(dx1,dy1);
   real gamma2=computeGamma(dx2,dy2);

   bool change=false;
   if(gamma1<=gamma2){
      if(gamma2-gamma1>=M_PI) {
         change=true;
         gamma2-=2*M_PI;
      }
   } else {
      change=true;
      if(gamma1-gamma2>=M_PI) {
         change=false;
         gamma1-=2*M_PI;
      }
   }

   if(change){
      real temp=gamma1;
      gamma1=gamma2;
      gamma2=temp;
      temp=d1;
      d1=d2;
      d2=temp;
   }


   //gammaP in (-Pi,Pi); gammaM in (-2Pi,Pi); but gammaP-gammaM in (0,Pi)
   //assert((d1 > capsule->R) && (d2 > capsule->R));
   assert((d1 > 1e-6) && (d2 > 1e-6));
   // asin returns a value 0..Pi/2
   real phi1=asin(capsule->R / d1);
   real phi2=asin(capsule->R / d2);
   real gammaM=gamma1-phi1;
   if(gamma2-phi2<gammaM) gammaM=gamma2-phi2;
   real gammaP=gamma2+phi2;
   if(gamma1+phi1>gammaP) gammaP=gamma1+phi1;

   activate(gammaM, gammaP);
   
   return false;
}


void VisualSensor::activate(real gammaM, real gammaP){
   //g1=gammaM; g2=gammaP; //for debugging

   int where;
   
   //check where the beginning of the first sensor is relative to the angle covered by the object
   if(gammaM < -delta){
      if(gammaP >= -delta){
         where=0; //inside
      } else {
         where=1; //after
      }
   } else {
      where=-1; //before
   }
   
   
   real u=-delta;
   for(int i=0; i<nSensors; i++, u+=epsilon){
      
      if(where==1){
         //we are after the covered angle
         //prepare for the case that -2Pi<gammaM<-Pi
         //since gammaP-gammaM<Pi (we assume that the viewed object does not cover the sensor),
         //in this case -Pi<gammaP<0
         gammaM+=2*M_PI;
         gammaP+=2*M_PI;
         where=-1;
         //we are now before the covered angle
         //gammaM in (0,Pi); gammaP in (Pi,2Pi)
      }
      
      if(where==-1){
         //we were before the beginning of the covered angle
         if(gammaM<u+epsilon){
            //the covered angle starts within the current pixel
            if(gammaP<=u+epsilon){
               //the covered angle also ends within the current pixel
               //set pixel to a fractional activation
               activations[i]+=(gammaP-gammaM)/epsilon;
               if(activations[i]>1) activations[i]=1;
               //the beginning of the next pixel will be after the covered angle
               where=1;
            } else {
               //the covered angle ends after the current pixel
               //set pixel to a fractional activation
               activations[i]+=(u+epsilon-gammaM)/epsilon;
               if(activations[i]>1) activations[i]=1;
               //the beginning of the next pixel will be within the covered angle
               where=0;
            }
         }
      } else{
         if(where==0){
            //we are inside the covered angle
            if(gammaP<=u+epsilon){
               //the covered angle also ends within the current pixel
               //set pixel to a fractional activation
               activations[i]+=(gammaP-u)/epsilon;
               if(activations[i]>1) activations[i]=1;
               where=1;
            } else {
               //current pixel is fully covered
               activations[i]=1;
            }
         }
      }
   }
}


void VisualSensor::draw(GUI *gui){
   real l=2, u;
   int i;
   //activations
   gui->setPenColor(Color("orange"));
   real x=parent->r.x+relativeR.x;
   real y=parent->r.y+relativeR.y;
   for(i=0; i<nSensors; i++){
      u=parent->alpha+relativeAlpha-delta+(i+0.5)*epsilon;
      gui->drawLine(x, y, x+l*cos(u)*activations[i], y+l*sin(u)*activations[i]);

   }
   gui->setPenColor(Color("papayaWhip"));
   for(i=0; i<=nSensors; i++){
      u=parent->alpha+relativeAlpha-delta+i*epsilon;
      gui->drawLine(x, y, x+l*cos(u), y+l*sin(u));
   }
   /*
   //for debugging: draw gammas
   gui->setPenColor(Color("red"));
   u=parent->alpha+relativeAlpha+g1;
   gui->drawLine(x, y, x+l*cos(u), y+l*sin(u));
   gui->setPenColor(Color("green"));
   u=parent->alpha+relativeAlpha+g2;
   gui->drawLine(x, y, x+l*cos(u), y+l*sin(u));
   */
}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005