File content as of revision 1:3be7b7d050f4:

#include "rigidLoop.h"

// SHOULD NOT BE HERE: (only because I am using AD_MIRRIOR... max and min in the set region function that should not be here)
#include "hardwareIO.h"

rigidLoop::rigidLoop()
{
}

rigidLoop::~rigidLoop()
{

}

void rigidLoop::showChildParameters() {
    //pc.printf("Blob Name: 
    pc.printf("Integration Step :%f\n", integrationStep);
    }

// Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type).
void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2Df _initPos, vector2Df _initSpeed)
{
    // (1) set ID:
    identifier=_id;

    updateMode=_elasticBlobMode;

    startCenter=_initPos;
    startSpeed=_initSpeed;
    
    integrationStep=0.30;//0.23;

    // (2) Initialize common variables of all blobs (base class):
    //initCommonVariables();

    // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...)
    // Sending data:
    periodSendingData=10; // in ms
    sendingRecenteringAngle=true;
    sendingAnchorPosition=true;
    sendingBlobMaxMin=true;
    // sending only on EVENT (which means the spot is touching something) in case of rigid loop:
    sendingOnlyWhenTouch=true;
    

    // Gravity field:
    gravity.set(0,0);

// (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)

    switch (updateMode) {

        case SPOT_TEST:
            // Name of this kind of spot:
            sprintf(spotName,"spot_test");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04); // only red?
             blueTouch=true;
             
            saccadeRadius=250;

            // default (initial) shape (the scafold belongs to the base class):
            // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            massCenter=0.01;
            dampMotionCenterMass=0.001;

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;//360.0/bluePrint.scafold.size()/2; // in DEGREES

            break;

        case SPOT_TRACK:
            // Name of this kind of spot:
            sprintf(spotName,"spot_track");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);
             blueTouch=false;

            saccadeRadius=50;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.001;//+0.000005*(rand()%100);
            dampMotionCenterMass=0.001;//0.00015;//00003;

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(2);
            angleCorrectionForceLoop=-39;// in degrees

            break;

    case SPOT_TRACK_DOT:
            // Name of this kind of spot:
            sprintf(spotName,"spot_track");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);
            blueTouch=false;

            saccadeRadius=45;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.001;//+0.000005*(rand()%100);
            dampMotionCenterMass=0.001;//0.00015;//00003;

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();
            
            justSearched=false;

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(3);
            angleCorrectionForceLoop=-39;// in degrees

            break;

        case SPOT_FOLLOWING:

            // Name of this kind of spot:
            sprintf(spotName,"rigid_following");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04); //only R
            blueTouch=false;
            
            // default (initial) shape (the scafold belongs to the base class):
            saccadeRadius=18;
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
            massCenter=0.01;
            dampMotionCenterMass=0.001;

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            slidingDirection=true; //  For contour following (will change direction when touching wall)
            speedContourFollowing=startSpeed.length();//1.1*saccadeRadius;
            justSearched=false;

            // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
            // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
            // BUT because we may want to see the blue laser where there is dark zone, then we would try to adjust mirror delay as close as possible to the
            // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
            displaySensingBuffer.setDelayMirrors(1); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
            angleCorrectionForceLoop= -39;// good for ONE spot: -5;// in DEGREES

            break;

        case  SPOT_BOUNCING:
            // Name of this kind of spot:
            sprintf(spotName,"rigid_bouncing");

            setColor(0x05);// R+B
            blueTouch=false;
            
            saccadeRadius=30;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
            dampMotionCenterMass=0.0002;//0.00015;//00003;
            factorBouncingForce=0.004; // this is because we will use a force on the central mass

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;// in degrees

            break;

       case  SPOT_BOUNCING_FACTOR:
            // Name of this kind of spot:
            sprintf(spotName,"rigid_bouncing");

            setColor(0x05);
            blueTouch=false;
            
            saccadeRadius=50;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
            dampMotionCenterMass=0.0002;//0.00015;//00003;
            
            factorBouncingForce=0.004; // this is because we will use a force on the central mass (not used in SPOT_BOUNCING_FACTOR)

            factorAbsorptionShock=0.9; // coef elastic bouncing
            
            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;// in degrees
            break;

        case SPOT_PACMAN:
            // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
            // When it collides, we use the position of the pacman (another spot) and the tangent vector to the blocking line to define the new direction of the
            // speed vector.

            sprintf(spotName,"pacman");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x06); // red+green = yellowish
            blueTouch=false;
            
            saccadeRadius=35;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            massCenter=1.0;
            dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
            factorBouncingForce=0; //actually not used.
            centerMass.dampBorder = 0; // no damping when hitting the mirror limits

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            slidingDirection=true; //  For contour following (will change direction when touching wall)
            speedContourFollowing=1.1*saccadeRadius;
            justSearched=false;


            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;// in degrees

            break;

        case SPOT_GHOST:
            // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
            // When it collides, we use the position of the pacman (another spot) and the tangent vector to the blocking line to define the new direction of the
            // speed vector.

            sprintf(spotName,"ghost");

            // Make it blueish by default:
            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x05); // red + blue
            blueTouch=false;
            
            massCenter=1.0;
            dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
            factorBouncingForce=0; //actually not used.
            centerMass.dampBorder = 0; // no damping when hitting the mirror limits


            saccadeRadius=35;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);


            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            slidingDirection=true; //  For contour following (will change direction when touching wall)
            speedContourFollowing=1.1*saccadeRadius;
            justSearched=false;


            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;// in degrees

            break;

        case  SPOT_AIR_HOCKEY:
            // Name of this kind of spot:
            sprintf(spotName,"air_hockey");

            //startCenter.set(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
            //startSpeed.set(0,0);

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);
            blueTouch=false;
            
            gravity.set(0,0);

            saccadeRadius=50;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.0008;//+0.000005*(rand()%100);
            dampMotionCenterMass=0.00065;//0.00015;//00003;
            factorBouncingForce=0.0018; // this is because we will use a force on the central mass

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;// in degrees

            break;

        case  SPOT_LORENTZ_FORCE:
            // Name of this kind of spot:
            sprintf(spotName,"rigid_fountain");

            //setColor(0x07);//0x04+0x02>>i);
            setColor(0x04);
            blueTouch=false;
            
            saccadeRadius=28;//+rand()%20;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.0005;//+0.000005*(rand()%100);
            dampMotionCenterMass=0.001;//0.00015;//00003;
            factorBouncingForce=0.0001;//0.00015; // this is because we will use a force on the central mass

            // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
            createLoopFromScafold();

            // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
            // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
            // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
            displaySensingBuffer.setDelayMirrors(1);
            angleCorrectionForceLoop=-39;// in degrees

            break;
            default:
            break;
    }

    saccadeRadius_initial=saccadeRadius; // this is for search mode for instance.

    // Excursion limits (for all points). Tthis will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius:
    setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);

    // draw it once on the display buffer for good initialization:
    draw();
}

void rigidLoop::createLoopFromScafold(void)
{

    initSizeBlob(bluePrint.scafold.size());  // very simple here (only need to set the size of the lsd buffer)

    centerMass.mass=massCenter;
    centerMass.dampMotion = dampMotionCenterMass;

    // note: the following may not be required in case of contour following:
    centerMass.setIntegrationStep(integrationStep); //0.23// VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
    centerMass.setInitialCondition(startCenter, startSpeed);
    // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);

}

void rigidLoop::initSizeBlob(int _numPoints)
{
    // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
    numPoints=_numPoints;

    // Sensing and Display trajectory:
    displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?).
}

void rigidLoop::setRegionMotion(float mmix, float mmiy, float mmax, float mmay)   // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass
{
    // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
    // Use the static method of the pointMass class:
    pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
}

void rigidLoop::setSize(float _newSize) {
    saccadeRadius=_newSize;
    saccadeRadius_initial=_newSize;
    // rebuild the blueprint (that's all it's needed in the case of a rigid loop, IF we don't change the number of points in the scafold of course):
    bluePrint.resizeDimensionScafold(_newSize);
}
void rigidLoop::sizeFactor(float sizeFactor) {
    saccadeRadius*=sizeFactor;
    saccadeRadius_initial*=sizeFactor;
    // rebuild the blueprint (that's all it's needed in the case of a rigid loop, IF we don't change the number of points in the scafold of course):
    bluePrint.resizeFactorDimensionScafold(sizeFactor);
}

void rigidLoop::setSpeed(float _newspeed) {
// in case of spot following:
    speedContourFollowing=_newspeed;

    // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
    //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
    centerMass.dampMotion/=1.0*_newspeed/7;//0.00045;//0.00015;//00003;
    //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
 }
void rigidLoop::speedFactor(float speedfactor)
{
    // in case of spot following:
    speedContourFollowing*=speedfactor;

    // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
    //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
    centerMass.dampMotion/=speedfactor;//0.00045;//0.00015;//00003;
    //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
}

void rigidLoop::explosion()
{
    transientBlobColor=blobColor|0x02;
    for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
        bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
        draw();
    }
    saccadeRadius=saccadeRadius_initial;
    bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
    // reset to central position:
    centerMass.setInitialCondition(startCenter, startSpeed);
    transientBlobColor=blobColor;
}

vector2Df rigidLoop::getCenter()
{
    return(centerMass.pos);
}

void rigidLoop::update(vector2Df referencePos)
{

    // (I) process loop geometry: not needed (rigid)
    // Just check if the blob touched the borders (only need to do this with the central mass):
    blobWallCollision=centerMass.bWallCollision;

    // (II) Process sensing buffer and compute light forces:
    // displaySensingBuffer.processSensedData(); // note: region with light is -1, and without is 2 (TO CHANGE!!! then we don't need to do "if" in the moment computation, but just a product)

    // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
    // ATTENTION!! for this simple method (of "first order moment") to work, we have either to have numPoints very large, OR an EVEN quantity - so the
    // sum in the circle is 0).
    vector2Df momentVector(0,0);
    int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!!
    for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
        if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)

            momentVector.x+=(float)bluePrint.scafold[i].x; // note: casting is happening here automatically (unsigned short to float), but I put (float) to remember that types are different
            momentVector.y+=(float)bluePrint.scafold[i].y;

            // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd...
            // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0

            counterDarkZone++;
        }
    }
    momentVector=momentVector*(2*PI/numPoints);
    float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone

    vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
    if (momentNorm==0) {
        unitTowardsLight.set(0,0);
        recenteringVectorLoop.set(0,0);
        normRecenteringVector=0;
        angleRecenteringVector=0;
    } else {
        unitTowardsLight=momentVector/(-1.0*momentNorm);
        // Apply correction angle (NOT delay mirrors):
        unitTowardsLight.rotateDeg(angleCorrectionForceLoop);

        // Compute "recenteringVectorLoop": the vector making the spot goes completely AWAY form the dark zone
        float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
        if (aux>1) aux=1.0; // can happen because of the discrete integration!
        if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
            recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
        } else {
            recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
        }


        // Compute redundant quantities (if necessary, for sending through OSC, etc):
        normRecenteringVector=recenteringVectorLoop.length();
        angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
    }

    // ========================  Now, depending on the mode of operation, we have different types of behaviour ========================================

    vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
    vector2Df auxVector;
    switch (updateMode) {
            // ================================================================
        case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc:
            // do nothing for the time being
            // NOTE: it is not so easy to show the recentering vector without affecting the mirror delay BECAUSE I AM USING THE INTERRUPT METHOD for display.
            // A possible solution is to instantiate ANOTHER blob with a shape just equal to a line, and rotate it using data from the this blob. Make a new class? Seems a good idea.

            // (1) current color: change with touch? NO
            transientBlobColor=blobColor; // just the original blob color

            break;
            // ================================================================
        case  SPOT_TRACK:
        if (displaySensingBuffer.lightTouched) {
            centerMass.pos +=recenteringVectorLoop*0.6;
            centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
            
                if (justSearched) {
                    saccadeRadius=saccadeRadius_initial;
                    bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                    justSearched=false;
                }

            } else if (displaySensingBuffer.lightState==ALL_DARK) { // not touched nor on something white: SEARCH MODE
                saccadeRadius+=20;
                if (saccadeRadius>200) saccadeRadius=saccadeRadius_initial;
                bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                justSearched=true;
            }

            // Change color with touch? YES
            if (displaySensingBuffer.lightTouched)
                transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            else
                transientBlobColor=blobColor; // just the original blob color

            break;
                 // ================================================================
        case  SPOT_TRACK_DOT: // here, a dot in the center of the saccade should remain in the center:
            centerMass.pos -=recenteringVectorLoop*2.5;
            centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            // Change color with touch? YES
            if (displaySensingBuffer.lightTouched)
                transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            else
                transientBlobColor=blobColor; // just the original blob color

            break;
            // ================================================================
        case SPOT_FOLLOWING:
            // we need to compute the tangencial "speed":
            // vector2D slidingVector;
            if (momentNorm>0) {
                //momentVector/=momentNorm;
                // We can now compute the sliding vector as:
                slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;

                // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing)
                // This is used to update the position of the central mass - WITHOUT INTEGRATION (or with it, but for the time being, we don't do that):
                centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
                // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the
                // re-entry and avoid oscillations).

                // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
                centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)

                centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

                if (justSearched) {
                    saccadeRadius=saccadeRadius_initial;
                    bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                    justSearched=false;
                }

            } else {
                // not on something. SEARCH MODE (or go to spot_bouncing mode?)
                saccadeRadius+=30;
                if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
                bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                justSearched=true;
            }

            // Change color with touch? NO
            // if (displaySensingBuffer.lightTouched)
            //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            // else
            transientBlobColor=blobColor; // just the original blob color

            // change sliding direction (for countour following):
            if (blobWallCollision) {
                if (wallCounter>5) {
                    slidingDirection=!slidingDirection;
                    wallCounter=0;
                }
            }
            wallCounter++;

            break;
            // ================================================================
        case SPOT_GHOST:
            // This is not completely sliding nor bouncing, but a combination of both
            // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
            //            - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
            //            - also, choose the direction so as to APPROACH THE PACMAN (position of the pacman is in referencePos, a parameter to "update" method).

            if (momentNorm>0) {

                // first, get the current speed:
                auxVector=centerMass.getSpeed();
                
                // Before recalculating the speed (that will recompute pos from posOld), set posOld well outside the dark zone (this is
                // necessary because if the printed pattern move and the speed is slow, then there is not enough bouncing!):
                centerMass.posOld=centerMass.pos+recenteringVectorLoop;

                // then compute the new bounce speed vector:
                float aux=unitTowardsLight.dot(auxVector);
                float anglepac=unitTowardsLight.angleDeg(referencePos-centerMass.pos); // angle from unit vector to (pacman-center)
                    if (abs(anglepac)<85)
                        slidingVector= referencePos-centerMass.pos; 
                        //slidingVector= auxVector-unitTowardsLight*aux*2; // this is a normal bounce
                    else
                        slidingVector=unitTowardsLight.getRotatedDeg((anglepac>0)? 85 : -85);
                       
                   slidingVector.scale(auxVector.length()); // do not forget to scale...
                   // then reset speed:
                   centerMass.setSpeed(slidingVector);
            }

            // update dynamics for the central mass:
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            // Change color with touch? NO
            // if (displaySensingBuffer.lightTouched)
            //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            // else
            transientBlobColor=blobColor; // just the original blob color


            break;

            // ================================================================
        case SPOT_PACMAN:
            // This is not completely sliding nor bouncing, but a combination of both
            // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
            //            - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
            //            - also, choose the direction so that it minimizes the angle with the previous speed vector (a little like bouncing):

            if (momentNorm>0) {

                // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
                auxVector=centerMass.getSpeed();
                float aux=unitTowardsLight.dot(auxVector);
                if (aux<0) {
                    slidingVector=auxVector-(unitTowardsLight*aux*2); 
                   // slidingVector.scale(auxVector.length()); // rescale to the size of the initial speed.
                    centerMass.setSpeed(slidingVector);
                }

            }

            // update dynamics for the central mass:
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            // Change color with touch? NO
            // if (displaySensingBuffer.lightTouched)
            //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            // else
            transientBlobColor=blobColor; // just the original blob color


            break;

        case SPOT_BOUNCING_FACTOR:
          
          centerMass.resetForce();
          
            if (momentNorm>0) {
                // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
                auxVector=centerMass.getSpeed();
                float aux=unitTowardsLight.dot(auxVector);
                if (aux<0) {
                    slidingVector=auxVector-(unitTowardsLight*aux*2); // symmetric speed with respet to unitTowardsLight
                    slidingVector.scale(auxVector.length()*factorAbsorptionShock); // rescale to the size of the initial speed.
                    centerMass.setSpeed(slidingVector);
                }
                
                // This is a hack: let's ADD spring force if the penetration is de facto large:
               if (recenteringVectorLoop.length()>(saccadeRadius/4)) centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
                
                // Also, to avoid "tunneling" through dark zones, let's translate the spot:
                centerMass.posOld+=recenteringVectorLoop*0.3;
                centerMass.pos+=recenteringVectorLoop*0.3; 
            }

            // Gravity? - side or central attraction?
            centerMass.addForce(gravity*centerMass.mass);

            // or central spring attraction;
            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            //vector2Df dist=centerMass.pos-centerAttraction;
            //centerMass.addForce(-dist*centerMass.mass*0.0007);

            // or "radial gravity":
            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            //vector2Df dist=centerMass.pos-centerAttraction;
            //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);


            // update dynamics for the central mass:
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            // Change color with touch? NO
            // if (displaySensingBuffer.lightTouched)
            //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            // else
            transientBlobColor=blobColor; // just the original blob color


            break;


            // ================================================================
        case  SPOT_BOUNCING:
            // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector.
            // We can also MODIFY the position so as to avoid having completely or partially the spot inside the dark zone (because of inertia). 
            centerMass.resetForce();

            if (momentNorm>0) { //(displaySensingBuffer.lightTouched) {
                // add force; MANY POSSIBILITIES:
                // (1) Constant in norm:
                //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);

                // Proportional to the penetration depth in the dark zone (spring):
                centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
                // Or proportional to the square (or something else) of the penetration:
                //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
                
                // Also, translate to avoid penetration (need to do this with pos and oldPos, otherwise speed will change):
                centerMass.posOld+=recenteringVectorLoop*0.1;
                centerMass.pos+=recenteringVectorLoop*0.1; 
                //note: we don't change the speed, this would be just like the pacman: not really math modelling:
                // centerMass.setSpeed(-centerMass.getSpeed());
            }

            // Gravity? - side or central attraction?
            centerMass.addForce(gravity*centerMass.mass);

            // or central spring attraction;
            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            //vector2Df dist=centerMass.pos-centerAttraction;
            //centerMass.addForce(-dist*centerMass.mass*0.0007);

            // or "radial gravity":
            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            //vector2Df dist=centerMass.pos-centerAttraction;
            //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);


            // update dynamics for the central mass:
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            if (displaySensingBuffer.lightTouched) {
                // do collision damping:
                centerMass.setSpeed(centerMass.getSpeed()*0.99);
            }

            // Change color with touch? YES
            if (displaySensingBuffer.lightTouched)
                transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            else
                transientBlobColor=blobColor; // just the original blob color
            break;

            // ================================================================
        case  SPOT_AIR_HOCKEY:
            // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
            centerMass.resetForce();

            if (displaySensingBuffer.lightTouched) {
                // add force; MANY POSSIBILITIES:
                // (1) Constant in norm:
                //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
                // Exactly what is needed to have an elastic bouncing:

                // Proportional to the penetration depth in the dark zone (spring):
                centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
                // Or proportional to the square (or something else) of the penetration:
                //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);

            }

            // Gravity? - side or central attraction?
            //centerMass.addForce(gravity*centerMass.mass);

            // or central spring attraction;
            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            //vector2Df dist=centerMass.pos-centerAttraction;
            //centerMass.addForce(-dist*centerMass.mass*0.0007);

            // or "radial gravity":
            //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            //vector2Df dist=centerMass.pos-centerAttraction;
            //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);


            // update dynamics for the central mass:
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            if (displaySensingBuffer.lightTouched) {
                // do collision damping:
                centerMass.setSpeed(centerMass.getSpeed()*0.99);
            }

            // Change color with touch? YES
            if (displaySensingBuffer.lightTouched)
                transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            else
                transientBlobColor=blobColor; // just the original blob color

            // In case of "air hockey mode", reset position to initial positions and speeds when the spot touches any two opposites sides:
            if ((centerMass.innerCollitionDirection.x==1)||( centerMass.innerCollitionDirection.x==-1)) {
                transientBlobColor=blobColor|0x02;
                for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
                    bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                    draw();
                }
                saccadeRadius=saccadeRadius_initial;
                bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
                // reset to central position:
                centerMass.setInitialCondition(startCenter, startSpeed);
                transientBlobColor=blobColor;
            }
            break;


            // ================================================================
        case  SPOT_LORENTZ_FORCE:
            // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
            centerMass.resetForce();

            if (displaySensingBuffer.lightTouched) {
                // add force; MANY POSSIBILITIES:
                // (1) Constant in norm:
                //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
                // Exactly what is needed to have an elastic bouncing:

                // Proportional to the penetration depth in the dark zone (spring):
                centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
                // Or proportional to the square (or something else) of the penetration:
                //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);

            }

            // RADIAL GRAVITY for the "fountain mode":
            // vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
            // vector2Df radialVector=centerMass.pos-centerAttraction;
            // radialVector.rotateDeg(slidingDirection? 80 : 260);
            // centerMass.addForce(radialVector.normalize()*centerMass.mass*0.5);

            // bubble chamber? LORENTZ FORCE:
            vector2Df speedmass=centerMass.getSpeed();
            centerMass.addForce( speedmass.getRotatedDeg(90)*0.000005*speedContourFollowing);//0.000002*speedContourFollowing);

            // update dynamics for the central mass:
#ifndef VERLET_METHOD
            centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
#endif

            centerMass.update();  // unconstrained
            centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            if (displaySensingBuffer.lightTouched) {
                // do collision damping:
                centerMass.setSpeed(centerMass.getSpeed()*0.99);
            }

            // Change color with touch? YES
            if (displaySensingBuffer.lightTouched)
                transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
            else
                transientBlobColor=blobColor; // just the original blob color

            // In case of "fountain mode", reset position to initial positions and speeds, or change gravity sign:
            //  if (blobWallCollision) centerMass.setInitialCondition(startCenter, startSpeed);
            if (blobWallCollision) slidingDirection=!slidingDirection;
            break;
            // ================================================================
    }

}


// Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser
void rigidLoop::draw()
{
    // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
    float cx= centerMass.pos.x;
    float cy= centerMass.pos.y;
    for (int i = 0; i < numPoints; i++) {
        // The shape is drawn by translating the scafold shape (centered on centerMass):
        displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
        displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );

        // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
        // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;

        //displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
    }

    // Global color for the whole loop:
    displaySensingBuffer.displayColor=transientBlobColor;

}

void rigidLoop::computeBoundingBox()
{
}



void rigidLoop::sendDataSpecific()
{
    char auxstring[10];
    myled2=1; // for tests...

    // First, set the top address of the message to the ID of the blob (not the name):
//   sprintf(auxstring, "%d", identifier);
//   sendMes.setTopAddress("0");//auxstring);

    // =====================   OSC  ======================
    if (sendOSC) {

        // (a) Anchor mass:
        if (sendingAnchorPosition) {
            sprintf(auxstring, "/p%d",identifier);
            sendMes.setSubAddress(auxstring);
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(centerMass.pos.x);
            y=(long)(centerMass.pos.y);
            sendMes.setArgs( "ii", &x, &y);
            osc.sendOsc( &sendMes );
        }

        // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
        if (sendingLoopPositions) {
#ifdef SEND_AS_POINTS
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            float cx= centerMass.pos.x;
            float cy= centerMass.pos.y;
            for (int i = 0; i < numPoints; i++) {
                sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
                sendMes.setSubAddress(auxstring);  // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...)
                x=(long)(bluePrint.scafold[i].x + cx);
                y=(long)(bluePrint.scafold[i].y + cy);
                sendMes.setArgs( "ii", &x, &y);
                osc.sendOsc( &sendMes );
            }

#endif
#ifdef SEND_AS_BLOB
            sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
            uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
            float cx= centerMass.pos.x;
            float cy= centerMass.pos.y;
            for (int i = 0; i < numPoints; i++ ) {
                // note: massesLoop[i].pos.x is a "float"
                uint16_t x=(uint16_t)(bluePrint.scafold[i].x +  cx);
                blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+1]=(uint8_t)x;

                uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
                blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+3]=(uint8_t)y;
            }
            osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
#endif
#ifdef SEND_AS_STRING
            sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
            uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
            float cx= centerMass.pos.x;
            float cy= centerMass.pos.y;
            for (int i = 0; i < numPoints; i++ ) {
                // note: massesLoop[i].pos.x is a "float"
                uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
                blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+1]=(uint8_t)x;

                uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
                blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
                blobdata[4*i+3]=(uint8_t)y;
            }
            osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
#endif
        }
        if (sendingLoopRegions) {
            long    x;   //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            for (int i = 0; i < numPoints; i++) {
                sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
                sendMes.setSubAddress(auxstring);  // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...)
                x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
                sendMes.setArgs( "i", &x);
                osc.sendOsc( &sendMes );
            }
        }
        if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
            long    wall;   //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            sprintf(auxstring, "/bWall");
            sendMes.setSubAddress(auxstring);
            wall=(long)(blobWallCollision? 1 : 0);
            sendMes.setArgs( "i", &wall);
            osc.sendOsc( &sendMes );
        }

        // (d) Light sensing statistics:
        if (sendingBlobMaxMin) {
            sendMes.setSubAddress("/maxmin");
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(displaySensingBuffer.maxI);
            y=(long)(displaySensingBuffer.minI);
            sendMes.setArgs( "ii", &x, &y);
            osc.sendOsc( &sendMes );
        }

        // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
        if (sendingRecenteringVector) {
            sendMes.setSubAddress("/rvector");
            long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(recenteringVectorLoop.x);
            y=(long)(recenteringVectorLoop.y);
            sendMes.setArgs( "ii", &x, &y);
            osc.sendOsc( &sendMes );
        }
        if (sendingRecenteringAngle) {
            sprintf(auxstring, "/v %d",identifier);
            sendMes.setSubAddress(auxstring);
            long    x;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(angleRecenteringVector);
            sendMes.setArgs( "i", &x);
            osc.sendOsc( &sendMes );
        }
        if (sendingRecenteringNorm) {
            sendMes.setSubAddress("/rnorm");
            long    x;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
            x=(long)(normRecenteringVector);
            sendMes.setArgs( "i", &x);
            osc.sendOsc( &sendMes );
        }

        if (sendingTouched) {
            if (displaySensingBuffer.lightTouched) {
                sendMes.clearArgs(); // there are no arguments to send
                sendMes.setSubAddress("/touched");
                osc.sendOsc( &sendMes );
            }
        }

    } // end of OSC sending per-spot

    // =====================   SERIAL  ======================
    if (sendSerial) {
        //...
    }

    myled2=0; // for tests...
}