Diff: rigidLoop.cpp

Revision:

1:a4050fee11f7

Child:

2:34157ebbf56b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rigidLoop.cpp	Sat Mar 31 08:19:31 2012 +0000
@@ -0,0 +1,343 @@
+#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) {
+    // (1) set ID:
+    identifier=_id;
+
+    updateMode=_elasticBlobMode;
+
+    // (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...)
+    slidingDirection=true; //  For contour following (will change direction when touching wall)
+    speedContourFollowing=0.3;
+    saccadeRadius=50;
+
+// (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
+
+    switch (updateMode) {
+        case SPOT_FOLLOWING:
+
+            // Name of this kind of spot:
+            sprintf(spotName,"rigid_following");
+
+            // Color: (use parameter in the future):
+            setColor(0x07);//0x04+0x02>>i);
+
+            // default (initial) shape (the scafold belongs to the base class):
+            bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(1,1), 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;
+
+            angleCorrectionForceLoop=0;
+
+            break;
+
+        case  SPOT_BOUNCING:
+            // Name of this kind of spot:
+            sprintf(spotName,"rigid_bouncing");
+
+
+            // default (initial) shape (the scafold belongs to the base class):
+            bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(50,10), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+
+            // Numeric parameters for the simulated mechanical system:
+            massCenter=0.1;
+            dampMotionCenterMass=0.00005;//00003;
+            factorBouncingForce=0.0001; // this is because we will use a force on the central mass
+
+            angleCorrectionForceLoop=0;
+
+
+            break;
+    }
+            
+
+    // 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);
+
+    // !!!!!!!!!!!!!!!!!!!!! ::
+    // The following is not nice here (should be in the classLaserSensingTrajectory, not even on the simpleLaserRenderer), but for the time being let's leave it here
+    setDelayMirrors(4); // ATTN!!! needs to be called AFTER setting the number of points!!! (note: 5 seemed good)
+
+}
+
+
+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.03); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
+    centerMass.setInitialCondition(bluePrint.center.x, bluePrint.center.y, bluePrint.speed.x, bluePrint.speed.y);
+    // 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":
+    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]-centerMass.pos;
+        }
+    }
+    float momentNorm=momentVector.length();
+    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_FOLLOWING:
+            // we need to compute the tangencial "speed":
+            // vector2D slidingVector;
+            momentVector/=momentNorm;// let's normalize the moment vector (this simplifies the calculations for the sliding vector - need some calculations, but this is the result):
+            // We can now compute the sliding vector as:
+            slidingVector=momentVector.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing*saccadeRadius;
+            // 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.9;
+
+            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();
+            //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")
+
+            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", i+identifier*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...
+}