File content as of revision 37:fa6b1f15819f:

#include "classLaserSensingTrajectory.h"
using namespace std;

LaserSensingTrajectory::LaserSensingTrajectory():lightTouched(false), 
min_contrast_ratio(MIN_CONTRAST_RATIO), threshold_factor(THRESHOLD_FACTOR), 
min_acceptable_intensity(MIN_ACCEPTABLE_INTENSITY), fixedThreshold(FIXED_THRESHOLD) 
{
    lsdTrajectory.clear(); // no need in principle! the constructor of the vector will give an empty vector!
    // attention: properly set the state of the threshold switch in the IO.init (which by the way, in THIS hardware
    // implementation indicates the state for ALL the objects..)
   modeThreshold=AUTO;
  // IO.setSwitchOneState(true);
}

LaserSensingTrajectory::~LaserSensingTrajectory() {
   // lsdTrajectory.clear(); // there is no need to clear the vector, the destructor of this vector is called by default (and it's NOT a vector of pointers)
}

  
void LaserSensingTrajectory::setDelayMirrors(int delay) {
    delayMirrorSamples=delay;
}

void LaserSensingTrajectory::addDelayMirrors(int add_delay) {
    delayMirrorSamples+=add_delay;
}

bool LaserSensingTrajectory::processSensedData() {
    // Compute max and min intensity on the loop
    maxI=0;
    minI=255; // ratio has been normalized between 0 and 255
    unsigned short auxSize=lsdTrajectory.size(); // could be an unsigned char in principle... no more than 255 points per object, but well, memory in future versions
    // of the microprocessor can be larger.
    
    // Compute minimum and maximum intensities:
    for (unsigned short i = 0; i < auxSize; i++) {
        unsigned char mesI=lsdTrajectory[i].intensity; 
        if (maxI<mesI)  maxI=mesI;
        if (minI>mesI)  minI=mesI;
    }

    // Compute autoThreshold:
    switch(modeThreshold) {
    case AUTO:
        if (minI==0) minI=1;
        if (maxI<min_acceptable_intensity) autoThreshold=255;// (we consider that the saccade is FULL on something black - this is noise)
        else if (1.0*maxI/minI > min_contrast_ratio ) {
            autoThreshold = (unsigned char) (1.0 * (maxI-minI) * threshold_factor + minI);    // threshold_factor = 2/3 or 1/2 is a good value.
        } else {// ... otherwise, we consider that the saccade is FULL on something white
            autoThreshold=0;
        }
    break;
    case FIXED:
        autoThreshold=fixedThreshold;
    break;
    }
  

    // Segment the trajectory (only two levels for the time being, but we can have more - meaning different forces, real or even complex values to have different angle forces...):
    // NOTE: if using 1 and -1 instead of 1 and 0, we can avoid having to add a "blob internal pressure"! -1 means a force towards the interior, and +1 outwards...
    // This means that the loop will naturally become inside-out depending on the color of the main surface! (but will mantain its normal size). Much better and elegant solution than the
    // idea of the blob "constant" internal pressure...
    bool isLightZone=false, isDarkZone=false;
    for (unsigned short i = 0; i <  auxSize; i++) {
        unsigned short delayedpoint=(i+auxSize+delayMirrorSamples)%auxSize; // this way we can have negative delayMirrorSamples if required (would be absurd though)
        if (lsdTrajectory[delayedpoint].intensity>=autoThreshold) { // this means a WHITE zone:
            lsdTrajectory[i].lightZone= -1;
            isLightZone=true;
        } else { // this means DARK ZONE
            lsdTrajectory[i].lightZone= 2;
            isDarkZone=true;
        }
    }
    
    // In the case of AUTO mode, we assume that something is touching the object when the trajectory has at least one light zone and one dark zone;
    // in the case of FIXED mode, we assume something is touching the object when there is at least one light zone (this is a convention, because it depends
    // on the background - but we can argue that "no background" corresponds to no reflection (then dark), and, say, a ping pong ball or finger is front 
    // will be "light":
    if (modeThreshold==FIXED)  
        lightTouched=isLightZone;
    else 
        lightTouched=isLightZone&&isDarkZone; // assuming only two modes for modeThreshold
    
    // Return lightTouched for commodity: 
    return(lightTouched);
}