Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
rigidLoop.cpp
- Committer:
- mbedalvaro
- Date:
- 2012-04-07
- Revision:
- 7:0df17f3078bc
- Parent:
- 5:73cd58b58f95
- Child:
- 12:0de9cd2bced5
File content as of revision 7:0df17f3078bc:
#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), 14); //(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(1); angleCorrectionForceLoop=0;//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): saccadeRadius=35; bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 18); //(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=1.3*saccadeRadius; justSearched=false; // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection" // (if things were well adjusted - in particular mirror waiting times, then this could be 0. // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software). // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop. // BUT because we may want to see the blue laser where there is dark zone, then we would try to adjust mirror delay as close as possible to the // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob). displaySensingBuffer.setDelayMirrors(3); // this corresponds to an angular correction of -delayMirrors * 360/numPoints angleCorrectionForceLoop=-5;//-65;//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=85; // default (initial) shape (the scafold belongs to the base class): bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); // Numeric parameters for the simulated mechanical system: massCenter=0.0012; dampMotionCenterMass=0.0005;//0.00015;//00003; factorBouncingForce=0.0018; // this is because we will use a force on the central mass // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): createLoopFromScafold(); // 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(3); angleCorrectionForceLoop=-5;// in degrees break; } saccadeRadius_initial=saccadeRadius; // this is for search mode for instance. } 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); int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!! for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller) momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0 counterDarkZone++; } } momentVector=momentVector*(2*PI/numPoints); float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone vector2D unitTowardsLight; // this is the normed vector, pointing towards the light zone if (momentNorm==0) { unitTowardsLight.set(0,0); recenteringVectorLoop.set(0,0); normRecenteringVector=0; angleRecenteringVector=0; } else { unitTowardsLight=momentVector/(-1.0*momentNorm); // Apply correction angle (delay mirrors): unitTowardsLight.rotateDeg(angleCorrectionForceLoop); // Compute "recenteringVectorLoop" (in fact, the vector making the spot goes completely away form the dark zone): float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1 if (aux>1) aux=1.0; // can happen because of the discrete integration! if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux)); } else { recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux)); } // Compute redundant quantities (if necessary, for sending through OSC, etc): normRecenteringVector=recenteringVectorLoop.length(); angleRecenteringVector=recenteringVectorLoop.angleDegHoriz(); } // Now, depending on the mode of operation, we have 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=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing; // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing) // This is used to update the position of the central mass - WITHOUT INTEGRATION (or with it, but for the time being, we don't do that): centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6; // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the // re-entry and avoid oscillations). // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps? centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?) centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") if (justSearched) { saccadeRadius=saccadeRadius_initial; bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), numPoints); justSearched=false; } } else { // not on something. SEARCH MODE (or go to spot_bouncing mode?) saccadeRadius+=10; if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial; bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), numPoints); justSearched=true; } 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; MANY POSSIBILITIES: // (1) Constant in norm: //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce); // Exactly what is needed to have an elastic bouncing: // Proportional to the penetration depth in the dark zone (spring): centerMass.addForce(recenteringVectorLoop*factorBouncingForce); // Or proportional to the square (or something else) of the penetration: //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce); } // 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>5) { 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... }