File content as of revision 5:73cd58b58f95:

#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() {

}


// 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, vector2D _initPos, vector2D _initSpeed) {
    // (1) set ID:
    identifier=_id;

    updateMode=_elasticBlobMode;
    
    startCenter=_initPos;
    startSpeed=_initSpeed;
    
    // (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...)

  //  angleCorrectionForceLoop=0; // this will depend on the size of the blob (in principle), and number of blobs...


// (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);

            saccadeRadius=150;

            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(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();

            // Excursion limits (this 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);

            // 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(0);
            angleCorrectionForceLoop=360.0/bluePrint.scafold.size()/2; // in DEGREES

            break;

        case SPOT_FOLLOWING:

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

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

            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 10); //(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();

            // Excursion limits (this 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);

            slidingDirection=true; //  For contour following (will change direction when touching wall)
            speedContourFollowing=0.03;

            // 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(0);
            angleCorrectionForceLoop=-29;//360.0/bluePrint.scafold.size()/2; // in DEGREES

            break;

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

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

            saccadeRadius=65;
            // default (initial) shape (the scafold belongs to the base class):
            bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);

            // Numeric parameters for the simulated mechanical system:
            massCenter=0.012;
            dampMotionCenterMass=0.0001;//0.00015;//00003;
            factorBouncingForce=0.0025; // 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();

            // Excursion limits (this 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);

            // 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(0);
            angleCorrectionForceLoop=-28.5+180;//-29+180;//360.0/bluePrint.scafold.size()/2; // in DEGREES

            break;
    }

}


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::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(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::setRegionMotion(int mmix, int mmiy, int mmax, int 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);
}


void rigidLoop::update() {

    // (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).
    vector2D momentVector(0,0);
    for (int i = 0; i < numPoints; i++) {
        if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
            momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
        }
    }
    
//    if (displaySensingBuffer.lightRatio>0.5) momentVector*=-1.0;
//    if (displaySensingBuffer.coko<numPoints/2) momentVector*=-1.0;
    
    float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
    if (momentNorm==0) {
        recenteringVectorLoop.set(0,0);
        normRecenteringVector=0;
        angleRecenteringVector=0;
    } else {
        float aux=saccadeRadius/momentNorm;
        if (aux>0.5) recenteringVectorLoop=momentVector*sqrt(aux*aux-0.25);
        else recenteringVectorLoop=momentVector*sqrt(0.25-aux*aux);
        //Rotate by angleCorrection (to correct delays, etc):
        recenteringVectorLoop.rotateDeg(angleCorrectionForceLoop);
        // 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 two types of behaviour:
    vector2D slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
    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. 
        
        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=recenteringVectorLoop.getRotatedDeg(slidingDirection? 90 : -90) / normRecenteringVector * speedContourFollowing*saccadeRadius;
                // if (displaySensingBuffer.coko<numPoints/2) slidingVector*=-1.0;
                // Then the final correcting vector is the sum of sliding plus 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 + recenteringVectorLoop * 0.2;
                
              //  centerMass.pos += recenteringVectorLoop*0.1; // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!!

                // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
                centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")

            } else {
                // not on something. Do nothing for the time being
            }

            break;

        case  SPOT_BOUNCING:
            // 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:
             centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
            }

            // 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);
             }


            break;

    }

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


// 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= int(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
        displaySensingBuffer.lsdTrajectory[i].y= int(bluePrint.scafold[i].y + cy );
        //displaySensingBuffer.lsdTrajectory[i].color=blobColor; // perhaps per point color is not a good idea for the time being...
    }
    // global color for the whole loop:
    displaySensingBuffer.displayColor=blobColor;
}

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) {
            sendMes.setSubAddress("/apos");
            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) {
            sendMes.setSubAddress("/rangle");
            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) {
        //.. to do
    }

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