Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
rigidLoop.cpp
- Committer:
- mbedalvaro
- Date:
- 2012-04-04
- Revision:
- 4:f9d364f10335
- Parent:
- 3:b44ff6de81bd
- Child:
- 5:73cd58b58f95
File content as of revision 4:f9d364f10335:
#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=250; // default (initial) shape (the scafold belongs to the base class): bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion). massCenter=0.01; dampMotionCenterMass=0.001; // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): createLoopFromScafold(); // Excursion limits (this will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius). setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); // 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): displaySensingBuffer.setDelayMirrors(0); break; case SPOT_FOLLOWING: // Name of this kind of spot: sprintf(spotName,"rigid_following"); //setColor(0x07);//0x04+0x02>>i); setColor(0x04); // default (initial) shape (the scafold belongs to the base class): bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion). massCenter=0.01; dampMotionCenterMass=0.001; // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): createLoopFromScafold(); // Excursion limits (this will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius). setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); // 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): displaySensingBuffer.setDelayMirrors(4); break; case SPOT_BOUNCING: // Name of this kind of spot: sprintf(spotName,"rigid_bouncing"); //setColor(0x07);//0x04+0x02>>i); setColor(0x04); saccadeRadius=250; // default (initial) shape (the scafold belongs to the base class): bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 40); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); // Numeric parameters for the simulated mechanical system: massCenter=0.1; dampMotionCenterMass=0.00015;//00003; factorBouncingForce=0.0001; // this is because we will use a force on the central mass slidingDirection=true; // For contour following (will change direction when touching wall) speedContourFollowing=0.0003; // 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): displaySensingBuffer.setDelayMirrors(4); break; } } void rigidLoop::initSizeBlob(int _numPoints) { // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory) numPoints=_numPoints; // Sensing and Display trajectory: displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?). } void rigidLoop::createLoopFromScafold(void) { initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer) centerMass.mass=massCenter; centerMass.dampMotion = dampMotionCenterMass; // note: the following may not be required in case of contour following: centerMass.setIntegrationStep(0.23); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed. centerMass.setInitialCondition(startCenter, startSpeed); // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0); } void rigidLoop::setRegionMotion(int mmix, int mmiy, int mmax, int mmay) { // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10); } void rigidLoop::update() { // (I) process loop geometry: not needed (rigid) // Just check if the blob touched the borders (only need to do this with the central mass): blobWallCollision=centerMass.bWallCollision; // (II) Process sensing buffer and compute light forces: displaySensingBuffer.processSensedData(); // note: region with light is -1, and without is 2 (TO CHANGE!!! then we don't need to do "if" in the moment computation, but just a product) // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment": // ATTENTION!! for this simple method (of "first order moment") to work, we have either to have numPoints very large, OR an EVEN quantity - so the // sum in the circle is 0). vector2D momentVector(0,0); for (int i = 0; i < numPoints; i++) { if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller) // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0 } } float momentNorm=momentVector.length(); if (momentNorm==0) { recenteringVectorLoop.set(0,0); normRecenteringVector=0; angleRecenteringVector=0; } else { float aux=saccadeRadius/momentNorm; if (aux>0.5) recenteringVectorLoop=momentVector*sqrt(aux*aux-0.25); else recenteringVectorLoop=momentVector*sqrt(0.25-aux*aux); //Rotate by angleCorrection (to correct delays, etc): recenteringVectorLoop.rotateDeg(angleCorrectionForceLoop); // Compute redundant quantities (if necessary, for sending through OSC, etc): normRecenteringVector=recenteringVectorLoop.length(); angleRecenteringVector=recenteringVectorLoop.angleDegHoriz(); } // Now, depending on the mode of operation, we have two types of behaviour: vector2D slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization) switch (updateMode) { case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc: // do nothing for the time being // NOTE: it is not so easy to show the recentering vector without affecting the mirror delay BECAUSE I AM USING THE INTERRUPT METHOD for display. // A possible solution is to instantiate ANOTHER blob with a shape just equal to a line, and rotate it using data from the this blob. Make a new class? Seems a good idea. break; case SPOT_FOLLOWING: // we need to compute the tangencial "speed": // vector2D slidingVector; // let's normalize the moment vector (this simplifies the calculations for the sliding vector - need some calculations, but this is the result): if (momentNorm>0) { momentVector/=momentNorm; // 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; // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps? centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") } else { // not on something. Do nothing for the time being } break; case SPOT_BOUNCING: // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector centerMass.resetForce(); 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",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... }