Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
Diff: rigidLoop.cpp
- Revision:
- 1:a4050fee11f7
- Child:
- 2:34157ebbf56b
diff -r 345b3bc7a0ea -r a4050fee11f7 rigidLoop.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rigidLoop.cpp Sat Mar 31 08:19:31 2012 +0000 @@ -0,0 +1,343 @@ +#include "rigidLoop.h" + +// SHOULD NOT BE HERE: (only because I am using AD_MIRRIOR... max and min in the set region function that should not be here) +#include "hardwareIO.h" + +rigidLoop::rigidLoop() { +} + +rigidLoop::~rigidLoop() { + +} + + +// Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type). +void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2D _initPos) { + // (1) set ID: + identifier=_id; + + updateMode=_elasticBlobMode; + + // (2) Initialize common variables of all blobs (base class): + initCommonVariables(); + + // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...) + slidingDirection=true; // For contour following (will change direction when touching wall) + speedContourFollowing=0.3; + saccadeRadius=50; + +// (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time) + + switch (updateMode) { + case SPOT_FOLLOWING: + + // Name of this kind of spot: + sprintf(spotName,"rigid_following"); + + // Color: (use parameter in the future): + setColor(0x07);//0x04+0x02>>i); + + // default (initial) shape (the scafold belongs to the base class): + bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(1,1), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); + + // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion). + massCenter=0.01; + dampMotionCenterMass=0.001; + + angleCorrectionForceLoop=0; + + break; + + case SPOT_BOUNCING: + // Name of this kind of spot: + sprintf(spotName,"rigid_bouncing"); + + + // default (initial) shape (the scafold belongs to the base class): + bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(50,10), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); + + // Numeric parameters for the simulated mechanical system: + massCenter=0.1; + dampMotionCenterMass=0.00005;//00003; + factorBouncingForce=0.0001; // this is because we will use a force on the central mass + + angleCorrectionForceLoop=0; + + + break; + } + + + // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things): + createLoopFromScafold(); + + // Excursion limits (this will set the limits of motion for the rigid loop, which is given by it's central position, so we have to correct by the radius). + setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius); + + // !!!!!!!!!!!!!!!!!!!!! :: + // The following is not nice here (should be in the classLaserSensingTrajectory, not even on the simpleLaserRenderer), but for the time being let's leave it here + setDelayMirrors(4); // ATTN!!! needs to be called AFTER setting the number of points!!! (note: 5 seemed good) + +} + + +void rigidLoop::initSizeBlob(int _numPoints) { + // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory) + numPoints=_numPoints; + + // Sensing and Display trajectory: + displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?). +} + +void rigidLoop::createLoopFromScafold(void) { + + initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer) + + centerMass.mass=massCenter; + centerMass.dampMotion = dampMotionCenterMass; + + // note: the following may not be required in case of contour following: + centerMass.setIntegrationStep(0.03); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed. + centerMass.setInitialCondition(bluePrint.center.x, bluePrint.center.y, bluePrint.speed.x, bluePrint.speed.y); + // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0); +} + +void rigidLoop::setRegionMotion(int mmix, int mmiy, int mmax, int mmay) { // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass + centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10); +} + + +void rigidLoop::update() { + + // (I) process loop geometry: not needed (rigid) + // Just check if the blob touched the borders (only need to do this with the central mass): + blobWallCollision=centerMass.bWallCollision; + + // (II) Process sensing buffer and compute light forces: + displaySensingBuffer.processSensedData(); // note: region with light is -1, and without is 2 (TO CHANGE!!! then we don't need to do "if" in the moment computation, but just a product) + + // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment": + vector2D momentVector(0,0); + for (int i = 0; i < numPoints; i++) { + if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller) + momentVector+=bluePrint.scafold[i]-centerMass.pos; + } + } + float momentNorm=momentVector.length(); + float aux=saccadeRadius/momentNorm; + if (aux>0.5) recenteringVectorLoop=momentVector*sqrt(aux*aux-0.25); + else recenteringVectorLoop=momentVector*sqrt(0.25-aux*aux); + //Rotate by angleCorrection (to correct delays, etc): + recenteringVectorLoop.rotateDeg(angleCorrectionForceLoop); + + // Compute redundant quantities (if necessary, for sending through OSC, etc): + normRecenteringVector=recenteringVectorLoop.length(); + angleRecenteringVector=recenteringVectorLoop.angleDegHoriz(); + + // Now, depending on the mode of operation, we have two types of behaviour: + vector2D slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization) + switch (updateMode) { + + case SPOT_FOLLOWING: + // we need to compute the tangencial "speed": + // vector2D slidingVector; + momentVector/=momentNorm;// let's normalize the moment vector (this simplifies the calculations for the sliding vector - need some calculations, but this is the result): + // We can now compute the sliding vector as: + slidingVector=momentVector.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing*saccadeRadius; + // Then the final correcting vector is the sum of sliding plus recentering vector (with a factor if one want some smothing) + // This is used to update the position of the central mass - WITHOUT INTEGRATION (or with it, but for the time being, we don't do that): + centerMass.pos += slidingVector + recenteringVectorLoop * 0.9; + + break; + + case SPOT_BOUNCING: + // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector + centerMass.resetForce(); + //centerMass.addForce(recenteringVectorLoop*factorBouncingForce); + + // update dynamics for the central mass:: +#ifndef VERLET_METHOD + centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating) +#endif + + centerMass.update(); // unconstrained + centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") + + break; + + } + + // OTHER PARTICULAR THINGS: + // change sliding direction (for countour following): + if (blobWallCollision) { + if (wallCounter>10) { + slidingDirection=!slidingDirection; + wallCounter=0; + } + } + wallCounter++; +} + + +// Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser +void rigidLoop::draw() { + // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory: + float cx= centerMass.pos.x; + float cy= centerMass.pos.y; + for (int i = 0; i < numPoints; i++) { + // The shape is drawn by translating the scafold shape (centered on centerMass): + displaySensingBuffer.lsdTrajectory[i].x= int(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!! + displaySensingBuffer.lsdTrajectory[i].y= int(bluePrint.scafold[i].y + cy ); + //displaySensingBuffer.lsdTrajectory[i].color=blobColor; // perhaps per point color is not a good idea for the time being... + } + // global color for the whole loop: + displaySensingBuffer.displayColor=blobColor; +} + +void rigidLoop::computeBoundingBox() { +} + + + +void rigidLoop::sendDataSpecific() { + char auxstring[10]; + myled2=1; // for tests... + + // First, set the top address of the message to the ID of the blob (not the name): + sprintf(auxstring, "%d", identifier); + sendMes.setTopAddress("0");//auxstring); + + // ===================== OSC ====================== + if (sendOSC) { + + // (a) Anchor mass: + if (sendingAnchorPosition) { + sendMes.setSubAddress("/apos"); + long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + x=(long)(centerMass.pos.x); + y=(long)(centerMass.pos.y); + sendMes.setArgs( "ii", &x, &y); + osc.sendOsc( &sendMes ); + } + + // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough) + if (sendingLoopPositions) { +#ifdef SEND_AS_POINTS + long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + float cx= centerMass.pos.x; + float cy= centerMass.pos.y; + for (int i = 0; i < numPoints; i++) { + sprintf(auxstring, "/p%d", i+identifier*10); // auxstring read as "/p1", "/p2", ... + sendMes.setSubAddress(auxstring); // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...) + x=(long)(bluePrint.scafold[i].x + cx); + y=(long)(bluePrint.scafold[i].y + cy); + sendMes.setArgs( "ii", &x, &y); + osc.sendOsc( &sendMes ); + } + +#endif +#ifdef SEND_AS_BLOB + sendMes.clearArgs(); // no need, we won't use osc.sendOsc()... + uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates + float cx= centerMass.pos.x; + float cy= centerMass.pos.y; + for (int i = 0; i < numPoints; i++ ) { + // note: massesLoop[i].pos.x is a "float" + uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx); + blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) + blobdata[4*i+1]=(uint8_t)x; + + uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy); + blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) + blobdata[4*i+3]=(uint8_t)y; + } + osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes +#endif +#ifdef SEND_AS_STRING + sendMes.clearArgs(); // no need, we won't use osc.sendOsc()... + uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates + float cx= centerMass.pos.x; + float cy= centerMass.pos.y; + for (int i = 0; i < numPoints; i++ ) { + // note: massesLoop[i].pos.x is a "float" + uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx ); + blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) + blobdata[4*i+1]=(uint8_t)x; + + uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy); + blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE) + blobdata[4*i+3]=(uint8_t)y; + } + osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes +#endif + } + if (sendingLoopRegions) { + long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + for (int i = 0; i < numPoints; i++) { + sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ... + sendMes.setSubAddress(auxstring); // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...) + x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0); + sendMes.setArgs( "i", &x); + osc.sendOsc( &sendMes ); + } + } + if (sendingLoopTouchWall) { // global touch wall for the loop (not per point) + long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + sprintf(auxstring, "/bWall"); + sendMes.setSubAddress(auxstring); + wall=(long)(blobWallCollision? 1 : 0); + sendMes.setArgs( "i", &wall); + osc.sendOsc( &sendMes ); + } + + // (d) Light sensing statistics: + if (sendingBlobMaxMin) { + sendMes.setSubAddress("/maxmin"); + long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + x=(long)(displaySensingBuffer.maxI); + y=(long)(displaySensingBuffer.minI); + sendMes.setArgs( "ii", &x, &y); + osc.sendOsc( &sendMes ); + } + + // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known). + if (sendingRecenteringVector) { + sendMes.setSubAddress("/rvector"); + long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + x=(long)(recenteringVectorLoop.x); + y=(long)(recenteringVectorLoop.y); + sendMes.setArgs( "ii", &x, &y); + osc.sendOsc( &sendMes ); + } + if (sendingRecenteringAngle) { + sendMes.setSubAddress("/rangle"); + long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + x=(long)(angleRecenteringVector); + sendMes.setArgs( "i", &x); + osc.sendOsc( &sendMes ); + } + if (sendingRecenteringNorm) { + sendMes.setSubAddress("/rnorm"); + long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!) + x=(long)(normRecenteringVector); + sendMes.setArgs( "i", &x); + osc.sendOsc( &sendMes ); + } + + if (sendingTouched) { + if (displaySensingBuffer.lightTouched) { + sendMes.clearArgs(); // there are no arguments to send + sendMes.setSubAddress("/touched"); + osc.sendOsc( &sendMes ); + } + } + + } // end of OSC sending per-spot + + // ===================== SERIAL ====================== + if (sendSerial) { + //.. to do + } + + myled2=0; // for tests... +}