rigidLoop.cpp

00001 #include "rigidLoop.h"
00002 
00003 // SHOULD NOT BE HERE: (only because I am using AD_MIRRIOR... max and min in the set region function that should not be here)
00004 #include "hardwareIO.h"
00005 
00006 rigidLoop::rigidLoop()
00007 {
00008 }
00009 
00010 rigidLoop::~rigidLoop()
00011 {
00012 
00013 }
00014 
00015 void rigidLoop::showChildParameters() {
00016     //pc.printf("Blob Name: 
00017     pc.printf("Integration Step :%f\n", integrationStep);
00018     }
00019 
00020 // Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type).
00021 void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2Df _initPos, vector2Df _initSpeed)
00022 {
00023     // (1) set ID:
00024     identifier=_id;
00025 
00026     updateMode=_elasticBlobMode;
00027 
00028     startCenter=_initPos;
00029     startSpeed=_initSpeed;
00030     
00031     integrationStep=0.30;//0.23;
00032 
00033     // (2) Initialize common variables of all blobs (base class):
00034     //initCommonVariables();
00035 
00036     // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...)
00037     // Sending data:
00038     periodSendingData=10; // in ms
00039     sendingRecenteringAngle=true;
00040     sendingAnchorPosition=true;
00041     sendingBlobMaxMin=true;
00042     // sending only on EVENT (which means the spot is touching something) in case of rigid loop:
00043     sendingOnlyWhenTouch=true;
00044     
00045 
00046     // Gravity field:
00047     gravity.set(0,0);
00048 
00049 // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
00050 
00051     switch (updateMode) {
00052 
00053         case SPOT_TEST:
00054             // Name of this kind of spot:
00055             sprintf(spotName,"spot_test");
00056 
00057             //setColor(0x07);//0x04+0x02>>i);
00058             setColor(0x04); // only red?
00059              blueTouch=true;
00060              
00061             saccadeRadius=250;
00062 
00063             // default (initial) shape (the scafold belongs to the base class):
00064             // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
00065             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00066 
00067             massCenter=0.01;
00068             dampMotionCenterMass=0.001;
00069 
00070             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00071             createLoopFromScafold();
00072 
00073             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00074             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00075             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00076             displaySensingBuffer.setDelayMirrors(1);
00077             angleCorrectionForceLoop=-39;//360.0/bluePrint.scafold.size()/2; // in DEGREES
00078 
00079             break;
00080 
00081         case SPOT_TRACK:
00082             // Name of this kind of spot:
00083             sprintf(spotName,"spot_track");
00084 
00085             //setColor(0x07);//0x04+0x02>>i);
00086             setColor(0x04);
00087              blueTouch=false;
00088 
00089             saccadeRadius=50;//+rand()%20;
00090             // default (initial) shape (the scafold belongs to the base class):
00091             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00092 
00093             // Numeric parameters for the simulated mechanical system:
00094             massCenter=0.001;//+0.000005*(rand()%100);
00095             dampMotionCenterMass=0.001;//0.00015;//00003;
00096 
00097             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00098             createLoopFromScafold();
00099 
00100             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00101             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00102             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00103             displaySensingBuffer.setDelayMirrors(2);
00104             angleCorrectionForceLoop=-39;// in degrees
00105 
00106             break;
00107 
00108     case SPOT_TRACK_DOT:
00109             // Name of this kind of spot:
00110             sprintf(spotName,"spot_track");
00111 
00112             //setColor(0x07);//0x04+0x02>>i);
00113             setColor(0x04);
00114             blueTouch=false;
00115 
00116             saccadeRadius=45;//+rand()%20;
00117             // default (initial) shape (the scafold belongs to the base class):
00118             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00119 
00120             // Numeric parameters for the simulated mechanical system:
00121             massCenter=0.001;//+0.000005*(rand()%100);
00122             dampMotionCenterMass=0.001;//0.00015;//00003;
00123 
00124             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00125             createLoopFromScafold();
00126             
00127             justSearched=false;
00128 
00129             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00130             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00131             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00132             displaySensingBuffer.setDelayMirrors(3);
00133             angleCorrectionForceLoop=-39;// in degrees
00134 
00135             break;
00136 
00137         case SPOT_FOLLOWING:
00138 
00139             // Name of this kind of spot:
00140             sprintf(spotName,"rigid_following");
00141 
00142             //setColor(0x07);//0x04+0x02>>i);
00143             setColor(0x04); //only R
00144             blueTouch=false;
00145             
00146             // default (initial) shape (the scafold belongs to the base class):
00147             saccadeRadius=18;
00148             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 20); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00149 
00150             // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
00151             massCenter=0.01;
00152             dampMotionCenterMass=0.001;
00153 
00154             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00155             createLoopFromScafold();
00156 
00157             slidingDirection=true; //  For contour following (will change direction when touching wall)
00158             speedContourFollowing=startSpeed.length();//1.1*saccadeRadius;
00159             justSearched=false;
00160 
00161             // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
00162             // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00163             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00164             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
00165             // 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
00166             // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
00167             displaySensingBuffer.setDelayMirrors(1); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
00168             angleCorrectionForceLoop= -39;// good for ONE spot: -5;// in DEGREES
00169 
00170             break;
00171 
00172         case  SPOT_BOUNCING:
00173             // Name of this kind of spot:
00174             sprintf(spotName,"rigid_bouncing");
00175 
00176             setColor(0x05);// R+B
00177             blueTouch=false;
00178             
00179             saccadeRadius=30;//+rand()%20;
00180             // default (initial) shape (the scafold belongs to the base class):
00181             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00182 
00183             // Numeric parameters for the simulated mechanical system:
00184             massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
00185             dampMotionCenterMass=0.0002;//0.00015;//00003;
00186             factorBouncingForce=0.004; // this is because we will use a force on the central mass
00187 
00188             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00189             createLoopFromScafold();
00190 
00191             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00192             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00193             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00194             displaySensingBuffer.setDelayMirrors(1);
00195             angleCorrectionForceLoop=-39;// in degrees
00196 
00197             break;
00198 
00199        case  SPOT_BOUNCING_FACTOR:
00200             // Name of this kind of spot:
00201             sprintf(spotName,"rigid_bouncing");
00202 
00203             setColor(0x05);
00204             blueTouch=false;
00205             
00206             saccadeRadius=50;//+rand()%20;
00207             // default (initial) shape (the scafold belongs to the base class):
00208             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00209 
00210             // Numeric parameters for the simulated mechanical system:
00211             massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
00212             dampMotionCenterMass=0.0002;//0.00015;//00003;
00213             
00214             factorBouncingForce=0.004; // this is because we will use a force on the central mass (not used in SPOT_BOUNCING_FACTOR)
00215 
00216             factorAbsorptionShock=0.9; // coef elastic bouncing
00217             
00218             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00219             createLoopFromScafold();
00220 
00221             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00222             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00223             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00224             displaySensingBuffer.setDelayMirrors(1);
00225             angleCorrectionForceLoop=-39;// in degrees
00226             break;
00227 
00228         case SPOT_PACMAN:
00229             // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
00230             // 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
00231             // speed vector.
00232 
00233             sprintf(spotName,"pacman");
00234 
00235             //setColor(0x07);//0x04+0x02>>i);
00236             setColor(0x06); // red+green = yellowish
00237             blueTouch=false;
00238             
00239             saccadeRadius=35;//+rand()%20;
00240             // default (initial) shape (the scafold belongs to the base class):
00241             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00242 
00243             massCenter=1.0;
00244             dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
00245             factorBouncingForce=0; //actually not used.
00246             centerMass.dampBorder = 0; // no damping when hitting the mirror limits
00247 
00248             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00249             createLoopFromScafold();
00250 
00251             slidingDirection=true; //  For contour following (will change direction when touching wall)
00252             speedContourFollowing=1.1*saccadeRadius;
00253             justSearched=false;
00254 
00255 
00256             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00257             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00258             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00259             displaySensingBuffer.setDelayMirrors(1);
00260             angleCorrectionForceLoop=-39;// in degrees
00261 
00262             break;
00263 
00264         case SPOT_GHOST:
00265             // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
00266             // 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
00267             // speed vector.
00268 
00269             sprintf(spotName,"ghost");
00270 
00271             // Make it blueish by default:
00272             //setColor(0x07);//0x04+0x02>>i);
00273             setColor(0x05); // red + blue
00274             blueTouch=false;
00275             
00276             massCenter=1.0;
00277             dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
00278             factorBouncingForce=0; //actually not used.
00279             centerMass.dampBorder = 0; // no damping when hitting the mirror limits
00280 
00281 
00282             saccadeRadius=35;//+rand()%20;
00283             // default (initial) shape (the scafold belongs to the base class):
00284             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00285 
00286 
00287             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00288             createLoopFromScafold();
00289 
00290             slidingDirection=true; //  For contour following (will change direction when touching wall)
00291             speedContourFollowing=1.1*saccadeRadius;
00292             justSearched=false;
00293 
00294 
00295             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00296             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00297             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00298             displaySensingBuffer.setDelayMirrors(1);
00299             angleCorrectionForceLoop=-39;// in degrees
00300 
00301             break;
00302 
00303         case  SPOT_AIR_HOCKEY:
00304             // Name of this kind of spot:
00305             sprintf(spotName,"air_hockey");
00306 
00307             //startCenter.set(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
00308             //startSpeed.set(0,0);
00309 
00310             //setColor(0x07);//0x04+0x02>>i);
00311             setColor(0x04);
00312             blueTouch=false;
00313             
00314             gravity.set(0,0);
00315 
00316             saccadeRadius=50;//+rand()%20;
00317             // default (initial) shape (the scafold belongs to the base class):
00318             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
00319 
00320             // Numeric parameters for the simulated mechanical system:
00321             massCenter=0.0008;//+0.000005*(rand()%100);
00322             dampMotionCenterMass=0.00065;//0.00015;//00003;
00323             factorBouncingForce=0.0018; // this is because we will use a force on the central mass
00324 
00325             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00326             createLoopFromScafold();
00327 
00328             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00329             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00330             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00331             displaySensingBuffer.setDelayMirrors(1);
00332             angleCorrectionForceLoop=-39;// in degrees
00333 
00334             break;
00335 
00336         case  SPOT_LORENTZ_FORCE:
00337             // Name of this kind of spot:
00338             sprintf(spotName,"rigid_fountain");
00339 
00340             //setColor(0x07);//0x04+0x02>>i);
00341             setColor(0x04);
00342             blueTouch=false;
00343             
00344             saccadeRadius=28;//+rand()%20;
00345             // default (initial) shape (the scafold belongs to the base class):
00346             bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos, int _numScafoldPoints);
00347 
00348             // Numeric parameters for the simulated mechanical system:
00349             massCenter=0.0005;//+0.000005*(rand()%100);
00350             dampMotionCenterMass=0.001;//0.00015;//00003;
00351             factorBouncingForce=0.0001;//0.00015; // this is because we will use a force on the central mass
00352 
00353             // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
00354             createLoopFromScafold();
00355 
00356             // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
00357             // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
00358             // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
00359             displaySensingBuffer.setDelayMirrors(1);
00360             angleCorrectionForceLoop=-39;// in degrees
00361 
00362             break;
00363             default:
00364             break;
00365     }
00366 
00367     saccadeRadius_initial=saccadeRadius; // this is for search mode for instance.
00368 
00369     // 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:
00370     setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
00371 
00372     // draw it once on the display buffer for good initialization:
00373     draw();
00374 }
00375 
00376 void rigidLoop::createLoopFromScafold(void)
00377 {
00378 
00379     initSizeBlob(bluePrint.scafold.size());  // very simple here (only need to set the size of the lsd buffer)
00380 
00381     centerMass.mass=massCenter;
00382     centerMass.dampMotion = dampMotionCenterMass;
00383 
00384     // note: the following may not be required in case of contour following:
00385     centerMass.setIntegrationStep(integrationStep); //0.23// VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
00386     centerMass.setInitialCondition(startCenter, startSpeed);
00387     // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);
00388 
00389 }
00390 
00391 void rigidLoop::initSizeBlob(int _numPoints)
00392 {
00393     // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
00394     numPoints=_numPoints;
00395 
00396     // Sensing and Display trajectory:
00397     displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?).
00398 }
00399 
00400 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
00401 {
00402     // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
00403     // Use the static method of the pointMass class:
00404     pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
00405 }
00406 
00407 void rigidLoop::setSize(float _newSize) {
00408     saccadeRadius=_newSize;
00409     saccadeRadius_initial=_newSize;
00410     // 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):
00411     bluePrint.resizeDimensionScafold(_newSize);
00412 }
00413 void rigidLoop::sizeFactor(float sizeFactor) {
00414     saccadeRadius*=sizeFactor;
00415     saccadeRadius_initial*=sizeFactor;
00416     // 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):
00417     bluePrint.resizeFactorDimensionScafold(sizeFactor);
00418 }
00419 
00420 void rigidLoop::setSpeed(float _newspeed) {
00421 // in case of spot following:
00422     speedContourFollowing=_newspeed;
00423 
00424     // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
00425     //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
00426     centerMass.dampMotion/=1.0*_newspeed/7;//0.00045;//0.00015;//00003;
00427     //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
00428  }
00429 void rigidLoop::speedFactor(float speedfactor)
00430 {
00431     // in case of spot following:
00432     speedContourFollowing*=speedfactor;
00433 
00434     // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
00435     //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
00436     centerMass.dampMotion/=speedfactor;//0.00045;//0.00015;//00003;
00437     //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
00438 }
00439 
00440 void rigidLoop::explosion()
00441 {
00442     transientBlobColor=blobColor|0x02;
00443     for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
00444         bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00445         draw();
00446     }
00447     saccadeRadius=saccadeRadius_initial;
00448     bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00449     // reset to central position:
00450     centerMass.setInitialCondition(startCenter, startSpeed);
00451     transientBlobColor=blobColor;
00452 }
00453 
00454 vector2Df rigidLoop::getCenter()
00455 {
00456     return(centerMass.pos);
00457 }
00458 
00459 void rigidLoop::update(vector2Df referencePos)
00460 {
00461 
00462     // (I) process loop geometry: not needed (rigid)
00463     // Just check if the blob touched the borders (only need to do this with the central mass):
00464     blobWallCollision=centerMass.bWallCollision;
00465 
00466     // (II) Process sensing buffer and compute light forces:
00467     // 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)
00468 
00469     // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
00470     // 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
00471     // sum in the circle is 0).
00472     vector2Df momentVector(0,0);
00473     int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!!
00474     for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
00475         if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
00476 
00477             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
00478             momentVector.y+=(float)bluePrint.scafold[i].y;
00479 
00480             // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd...
00481             // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
00482 
00483             counterDarkZone++;
00484         }
00485     }
00486     momentVector=momentVector*(2*PI/numPoints);
00487     float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
00488 
00489     vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
00490     if (momentNorm==0) {
00491         unitTowardsLight.set(0,0);
00492         recenteringVectorLoop.set(0,0);
00493         normRecenteringVector=0;
00494         angleRecenteringVector=0;
00495     } else {
00496         unitTowardsLight=momentVector/(-1.0*momentNorm);
00497         // Apply correction angle (NOT delay mirrors):
00498         unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
00499 
00500         // Compute "recenteringVectorLoop": the vector making the spot goes completely AWAY form the dark zone
00501         float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
00502         if (aux>1) aux=1.0; // can happen because of the discrete integration!
00503         if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
00504             recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
00505         } else {
00506             recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
00507         }
00508 
00509 
00510         // Compute redundant quantities (if necessary, for sending through OSC, etc):
00511         normRecenteringVector=recenteringVectorLoop.length();
00512         angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
00513     }
00514 
00515     // ========================  Now, depending on the mode of operation, we have different types of behaviour ========================================
00516 
00517     vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
00518     vector2Df auxVector;
00519     switch (updateMode) {
00520             // ================================================================
00521         case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc:
00522             // do nothing for the time being
00523             // 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.
00524             // 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.
00525 
00526             // (1) current color: change with touch? NO
00527             transientBlobColor=blobColor; // just the original blob color
00528 
00529             break;
00530             // ================================================================
00531         case  SPOT_TRACK:
00532         if (displaySensingBuffer.lightTouched) {
00533             centerMass.pos +=recenteringVectorLoop*0.6;
00534             centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
00535             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00536             
00537                 if (justSearched) {
00538                     saccadeRadius=saccadeRadius_initial;
00539                     bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00540                     justSearched=false;
00541                 }
00542 
00543             } else if (displaySensingBuffer.lightState==ALL_DARK) { // not touched nor on something white: SEARCH MODE
00544                 saccadeRadius+=20;
00545                 if (saccadeRadius>200) saccadeRadius=saccadeRadius_initial;
00546                 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00547                 justSearched=true;
00548             }
00549 
00550             // Change color with touch? YES
00551             if (displaySensingBuffer.lightTouched)
00552                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00553             else
00554                 transientBlobColor=blobColor; // just the original blob color
00555 
00556             break;
00557                  // ================================================================
00558         case  SPOT_TRACK_DOT: // here, a dot in the center of the saccade should remain in the center:
00559             centerMass.pos -=recenteringVectorLoop*2.5;
00560             centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
00561             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00562 
00563             // Change color with touch? YES
00564             if (displaySensingBuffer.lightTouched)
00565                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00566             else
00567                 transientBlobColor=blobColor; // just the original blob color
00568 
00569             break;
00570             // ================================================================
00571         case SPOT_FOLLOWING:
00572             // we need to compute the tangencial "speed":
00573             // vector2D slidingVector;
00574             if (momentNorm>0) {
00575                 //momentVector/=momentNorm;
00576                 // We can now compute the sliding vector as:
00577                 slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
00578 
00579                 // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing)
00580                 // 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):
00581                 centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
00582                 // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the
00583                 // re-entry and avoid oscillations).
00584 
00585                 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
00586                 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
00587 
00588                 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00589 
00590                 if (justSearched) {
00591                     saccadeRadius=saccadeRadius_initial;
00592                     bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00593                     justSearched=false;
00594                 }
00595 
00596             } else {
00597                 // not on something. SEARCH MODE (or go to spot_bouncing mode?)
00598                 saccadeRadius+=30;
00599                 if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
00600                 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00601                 justSearched=true;
00602             }
00603 
00604             // Change color with touch? NO
00605             // if (displaySensingBuffer.lightTouched)
00606             //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00607             // else
00608             transientBlobColor=blobColor; // just the original blob color
00609 
00610             // change sliding direction (for countour following):
00611             if (blobWallCollision) {
00612                 if (wallCounter>5) {
00613                     slidingDirection=!slidingDirection;
00614                     wallCounter=0;
00615                 }
00616             }
00617             wallCounter++;
00618 
00619             break;
00620             // ================================================================
00621         case SPOT_GHOST:
00622             // This is not completely sliding nor bouncing, but a combination of both
00623             // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
00624             //            - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
00625             //            - also, choose the direction so as to APPROACH THE PACMAN (position of the pacman is in referencePos, a parameter to "update" method).
00626 
00627             if (momentNorm>0) {
00628 
00629                 // first, get the current speed:
00630                 auxVector=centerMass.getSpeed();
00631                 
00632                 // Before recalculating the speed (that will recompute pos from posOld), set posOld well outside the dark zone (this is
00633                 // necessary because if the printed pattern move and the speed is slow, then there is not enough bouncing!):
00634                 centerMass.posOld=centerMass.pos+recenteringVectorLoop;
00635 
00636                 // then compute the new bounce speed vector:
00637                 float aux=unitTowardsLight.dot(auxVector);
00638                 float anglepac=unitTowardsLight.angleDeg(referencePos-centerMass.pos); // angle from unit vector to (pacman-center)
00639                     if (abs(anglepac)<85)
00640                         slidingVector= referencePos-centerMass.pos; 
00641                         //slidingVector= auxVector-unitTowardsLight*aux*2; // this is a normal bounce
00642                     else
00643                         slidingVector=unitTowardsLight.getRotatedDeg((anglepac>0)? 85 : -85);
00644                        
00645                    slidingVector.scale(auxVector.length()); // do not forget to scale...
00646                    // then reset speed:
00647                    centerMass.setSpeed(slidingVector);
00648             }
00649 
00650             // update dynamics for the central mass:
00651 #ifndef VERLET_METHOD
00652             centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
00653 #endif
00654 
00655             centerMass.update();  // unconstrained
00656             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00657 
00658             // Change color with touch? NO
00659             // if (displaySensingBuffer.lightTouched)
00660             //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00661             // else
00662             transientBlobColor=blobColor; // just the original blob color
00663 
00664 
00665             break;
00666 
00667             // ================================================================
00668         case SPOT_PACMAN:
00669             // This is not completely sliding nor bouncing, but a combination of both
00670             // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
00671             //            - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
00672             //            - also, choose the direction so that it minimizes the angle with the previous speed vector (a little like bouncing):
00673 
00674             if (momentNorm>0) {
00675 
00676                 // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
00677                 auxVector=centerMass.getSpeed();
00678                 float aux=unitTowardsLight.dot(auxVector);
00679                 if (aux<0) {
00680                     slidingVector=auxVector-(unitTowardsLight*aux*2); 
00681                    // slidingVector.scale(auxVector.length()); // rescale to the size of the initial speed.
00682                     centerMass.setSpeed(slidingVector);
00683                 }
00684 
00685             }
00686 
00687             // update dynamics for the central mass:
00688 #ifndef VERLET_METHOD
00689             centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
00690 #endif
00691 
00692             centerMass.update();  // unconstrained
00693             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00694 
00695             // Change color with touch? NO
00696             // if (displaySensingBuffer.lightTouched)
00697             //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00698             // else
00699             transientBlobColor=blobColor; // just the original blob color
00700 
00701 
00702             break;
00703 
00704         case SPOT_BOUNCING_FACTOR:
00705           
00706           centerMass.resetForce();
00707           
00708             if (momentNorm>0) {
00709                 // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
00710                 auxVector=centerMass.getSpeed();
00711                 float aux=unitTowardsLight.dot(auxVector);
00712                 if (aux<0) {
00713                     slidingVector=auxVector-(unitTowardsLight*aux*2); // symmetric speed with respet to unitTowardsLight
00714                     slidingVector.scale(auxVector.length()*factorAbsorptionShock); // rescale to the size of the initial speed.
00715                     centerMass.setSpeed(slidingVector);
00716                 }
00717                 
00718                 // This is a hack: let's ADD spring force if the penetration is de facto large:
00719                if (recenteringVectorLoop.length()>(saccadeRadius/4)) centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
00720                 
00721                 // Also, to avoid "tunneling" through dark zones, let's translate the spot:
00722                 centerMass.posOld+=recenteringVectorLoop*0.3;
00723                 centerMass.pos+=recenteringVectorLoop*0.3; 
00724             }
00725 
00726             // Gravity? - side or central attraction?
00727             centerMass.addForce(gravity*centerMass.mass);
00728 
00729             // or central spring attraction;
00730             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00731             //vector2Df dist=centerMass.pos-centerAttraction;
00732             //centerMass.addForce(-dist*centerMass.mass*0.0007);
00733 
00734             // or "radial gravity":
00735             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00736             //vector2Df dist=centerMass.pos-centerAttraction;
00737             //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
00738 
00739 
00740             // update dynamics for the central mass:
00741 #ifndef VERLET_METHOD
00742             centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
00743 #endif
00744 
00745             centerMass.update();  // unconstrained
00746             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00747 
00748             // Change color with touch? NO
00749             // if (displaySensingBuffer.lightTouched)
00750             //      transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00751             // else
00752             transientBlobColor=blobColor; // just the original blob color
00753 
00754 
00755             break;
00756 
00757 
00758             // ================================================================
00759         case  SPOT_BOUNCING:
00760             // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector.
00761             // We can also MODIFY the position so as to avoid having completely or partially the spot inside the dark zone (because of inertia). 
00762             centerMass.resetForce();
00763 
00764             if (momentNorm>0) { //(displaySensingBuffer.lightTouched) {
00765                 // add force; MANY POSSIBILITIES:
00766                 // (1) Constant in norm:
00767                 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
00768 
00769                 // Proportional to the penetration depth in the dark zone (spring):
00770                 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
00771                 // Or proportional to the square (or something else) of the penetration:
00772                 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
00773                 
00774                 // Also, translate to avoid penetration (need to do this with pos and oldPos, otherwise speed will change):
00775                 centerMass.posOld+=recenteringVectorLoop*0.1;
00776                 centerMass.pos+=recenteringVectorLoop*0.1; 
00777                 //note: we don't change the speed, this would be just like the pacman: not really math modelling:
00778                 // centerMass.setSpeed(-centerMass.getSpeed());
00779             }
00780 
00781             // Gravity? - side or central attraction?
00782             centerMass.addForce(gravity*centerMass.mass);
00783 
00784             // or central spring attraction;
00785             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00786             //vector2Df dist=centerMass.pos-centerAttraction;
00787             //centerMass.addForce(-dist*centerMass.mass*0.0007);
00788 
00789             // or "radial gravity":
00790             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00791             //vector2Df dist=centerMass.pos-centerAttraction;
00792             //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
00793 
00794 
00795             // update dynamics for the central mass:
00796 #ifndef VERLET_METHOD
00797             centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
00798 #endif
00799 
00800             centerMass.update();  // unconstrained
00801             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00802 
00803             if (displaySensingBuffer.lightTouched) {
00804                 // do collision damping:
00805                 centerMass.setSpeed(centerMass.getSpeed()*0.99);
00806             }
00807 
00808             // Change color with touch? YES
00809             if (displaySensingBuffer.lightTouched)
00810                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00811             else
00812                 transientBlobColor=blobColor; // just the original blob color
00813             break;
00814 
00815             // ================================================================
00816         case  SPOT_AIR_HOCKEY:
00817             // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
00818             centerMass.resetForce();
00819 
00820             if (displaySensingBuffer.lightTouched) {
00821                 // add force; MANY POSSIBILITIES:
00822                 // (1) Constant in norm:
00823                 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
00824                 // Exactly what is needed to have an elastic bouncing:
00825 
00826                 // Proportional to the penetration depth in the dark zone (spring):
00827                 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
00828                 // Or proportional to the square (or something else) of the penetration:
00829                 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
00830 
00831             }
00832 
00833             // Gravity? - side or central attraction?
00834             //centerMass.addForce(gravity*centerMass.mass);
00835 
00836             // or central spring attraction;
00837             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00838             //vector2Df dist=centerMass.pos-centerAttraction;
00839             //centerMass.addForce(-dist*centerMass.mass*0.0007);
00840 
00841             // or "radial gravity":
00842             //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00843             //vector2Df dist=centerMass.pos-centerAttraction;
00844             //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
00845 
00846 
00847             // update dynamics for the central mass:
00848 #ifndef VERLET_METHOD
00849             centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
00850 #endif
00851 
00852             centerMass.update();  // unconstrained
00853             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00854 
00855             if (displaySensingBuffer.lightTouched) {
00856                 // do collision damping:
00857                 centerMass.setSpeed(centerMass.getSpeed()*0.99);
00858             }
00859 
00860             // Change color with touch? YES
00861             if (displaySensingBuffer.lightTouched)
00862                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00863             else
00864                 transientBlobColor=blobColor; // just the original blob color
00865 
00866             // In case of "air hockey mode", reset position to initial positions and speeds when the spot touches any two opposites sides:
00867             if ((centerMass.innerCollitionDirection.x==1)||( centerMass.innerCollitionDirection.x==-1)) {
00868                 transientBlobColor=blobColor|0x02;
00869                 for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
00870                     bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00871                     draw();
00872                 }
00873                 saccadeRadius=saccadeRadius_initial;
00874                 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
00875                 // reset to central position:
00876                 centerMass.setInitialCondition(startCenter, startSpeed);
00877                 transientBlobColor=blobColor;
00878             }
00879             break;
00880 
00881 
00882             // ================================================================
00883         case  SPOT_LORENTZ_FORCE:
00884             // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
00885             centerMass.resetForce();
00886 
00887             if (displaySensingBuffer.lightTouched) {
00888                 // add force; MANY POSSIBILITIES:
00889                 // (1) Constant in norm:
00890                 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
00891                 // Exactly what is needed to have an elastic bouncing:
00892 
00893                 // Proportional to the penetration depth in the dark zone (spring):
00894                 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
00895                 // Or proportional to the square (or something else) of the penetration:
00896                 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
00897 
00898             }
00899 
00900             // RADIAL GRAVITY for the "fountain mode":
00901             // vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
00902             // vector2Df radialVector=centerMass.pos-centerAttraction;
00903             // radialVector.rotateDeg(slidingDirection? 80 : 260);
00904             // centerMass.addForce(radialVector.normalize()*centerMass.mass*0.5);
00905 
00906             // bubble chamber? LORENTZ FORCE:
00907             vector2Df speedmass=centerMass.getSpeed();
00908             centerMass.addForce( speedmass.getRotatedDeg(90)*0.000005*speedContourFollowing);//0.000002*speedContourFollowing);
00909 
00910             // update dynamics for the central mass:
00911 #ifndef VERLET_METHOD
00912             centerMass.addDampingForce();  // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
00913 #endif
00914 
00915             centerMass.update();  // unconstrained
00916             centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
00917 
00918             if (displaySensingBuffer.lightTouched) {
00919                 // do collision damping:
00920                 centerMass.setSpeed(centerMass.getSpeed()*0.99);
00921             }
00922 
00923             // Change color with touch? YES
00924             if (displaySensingBuffer.lightTouched)
00925                 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
00926             else
00927                 transientBlobColor=blobColor; // just the original blob color
00928 
00929             // In case of "fountain mode", reset position to initial positions and speeds, or change gravity sign:
00930             //  if (blobWallCollision) centerMass.setInitialCondition(startCenter, startSpeed);
00931             if (blobWallCollision) slidingDirection=!slidingDirection;
00932             break;
00933             // ================================================================
00934     }
00935 
00936 }
00937 
00938 
00939 // Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser
00940 void rigidLoop::draw()
00941 {
00942     // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
00943     float cx= centerMass.pos.x;
00944     float cy= centerMass.pos.y;
00945     for (int i = 0; i < numPoints; i++) {
00946         // The shape is drawn by translating the scafold shape (centered on centerMass):
00947         displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
00948         displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );
00949 
00950         // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
00951         // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;
00952 
00953         //displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
00954     }
00955 
00956     // Global color for the whole loop:
00957     displaySensingBuffer.displayColor=transientBlobColor;
00958 
00959 }
00960 
00961 void rigidLoop::computeBoundingBox()
00962 {
00963 }
00964 
00965 
00966 
00967 void rigidLoop::sendDataSpecific()
00968 {
00969     char auxstring[10];
00970     myled2=1; // for tests...
00971 
00972     // First, set the top address of the message to the ID of the blob (not the name):
00973 //   sprintf(auxstring, "%d", identifier);
00974 //   sendMes.setTopAddress("0");//auxstring);
00975 
00976     // =====================   OSC  ======================
00977     if (sendOSC) {
00978 
00979         // (a) Anchor mass:
00980         if (sendingAnchorPosition) {
00981             sprintf(auxstring, "/p %d",identifier);
00982             sendMes.setSubAddress(auxstring);
00983             long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
00984             x=(long)(centerMass.pos.x);
00985             y=(long)(centerMass.pos.y);
00986             sendMes.setArgs( "ii", &x, &y);
00987             osc.sendOsc( &sendMes );
00988         }
00989 
00990         // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
00991         if (sendingLoopPositions) {
00992 #ifdef SEND_AS_POINTS
00993             long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
00994             float cx= centerMass.pos.x;
00995             float cy= centerMass.pos.y;
00996             for (int i = 0; i < numPoints; i++) {
00997                 sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
00998                 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...)
00999                 x=(long)(bluePrint.scafold[i].x + cx);
01000                 y=(long)(bluePrint.scafold[i].y + cy);
01001                 sendMes.setArgs( "ii", &x, &y);
01002                 osc.sendOsc( &sendMes );
01003             }
01004 
01005 #endif
01006 #ifdef SEND_AS_BLOB
01007             sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
01008             uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
01009             float cx= centerMass.pos.x;
01010             float cy= centerMass.pos.y;
01011             for (int i = 0; i < numPoints; i++ ) {
01012                 // note: massesLoop[i].pos.x is a "float"
01013                 uint16_t x=(uint16_t)(bluePrint.scafold[i].x +  cx);
01014                 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
01015                 blobdata[4*i+1]=(uint8_t)x;
01016 
01017                 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
01018                 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
01019                 blobdata[4*i+3]=(uint8_t)y;
01020             }
01021             osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
01022 #endif
01023 #ifdef SEND_AS_STRING
01024             sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
01025             uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
01026             float cx= centerMass.pos.x;
01027             float cy= centerMass.pos.y;
01028             for (int i = 0; i < numPoints; i++ ) {
01029                 // note: massesLoop[i].pos.x is a "float"
01030                 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
01031                 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
01032                 blobdata[4*i+1]=(uint8_t)x;
01033 
01034                 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
01035                 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
01036                 blobdata[4*i+3]=(uint8_t)y;
01037             }
01038             osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
01039 #endif
01040         }
01041         if (sendingLoopRegions) {
01042             long    x;   //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
01043             for (int i = 0; i < numPoints; i++) {
01044                 sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
01045                 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...)
01046                 x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
01047                 sendMes.setArgs( "i", &x);
01048                 osc.sendOsc( &sendMes );
01049             }
01050         }
01051         if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
01052             long    wall;   //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
01053             sprintf(auxstring, "/bWall");
01054             sendMes.setSubAddress(auxstring);
01055             wall=(long)(blobWallCollision? 1 : 0);
01056             sendMes.setArgs( "i", &wall);
01057             osc.sendOsc( &sendMes );
01058         }
01059 
01060         // (d) Light sensing statistics:
01061         if (sendingBlobMaxMin) {
01062             sendMes.setSubAddress("/maxmin");
01063             long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
01064             x=(long)(displaySensingBuffer.maxI);
01065             y=(long)(displaySensingBuffer.minI);
01066             sendMes.setArgs( "ii", &x, &y);
01067             osc.sendOsc( &sendMes );
01068         }
01069 
01070         // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
01071         if (sendingRecenteringVector) {
01072             sendMes.setSubAddress("/rvector");
01073             long    x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
01074             x=(long)(recenteringVectorLoop.x);
01075             y=(long)(recenteringVectorLoop.y);
01076             sendMes.setArgs( "ii", &x, &y);
01077             osc.sendOsc( &sendMes );
01078         }
01079         if (sendingRecenteringAngle) {
01080             sprintf(auxstring, "/v %d",identifier);
01081             sendMes.setSubAddress(auxstring);
01082             long    x;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
01083             x=(long)(angleRecenteringVector);
01084             sendMes.setArgs( "i", &x);
01085             osc.sendOsc( &sendMes );
01086         }
01087         if (sendingRecenteringNorm) {
01088             sendMes.setSubAddress("/rnorm");
01089             long    x;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
01090             x=(long)(normRecenteringVector);
01091             sendMes.setArgs( "i", &x);
01092             osc.sendOsc( &sendMes );
01093         }
01094 
01095         if (sendingTouched) {
01096             if (displaySensingBuffer.lightTouched) {
01097                 sendMes.clearArgs(); // there are no arguments to send
01098                 sendMes.setSubAddress("/touched");
01099                 osc.sendOsc( &sendMes );
01100             }
01101         }
01102 
01103     } // end of OSC sending per-spot
01104 
01105     // =====================   SERIAL  ======================
01106     if (sendSerial) {
01107         //.. to do
01108     }
01109 
01110     myled2=0; // for tests...
01111 }