Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
Diff: rigidLoop.cpp
- Revision:
- 7:0df17f3078bc
- Parent:
- 5:73cd58b58f95
- Child:
- 12:0de9cd2bced5
diff -r 444859c27e78 -r 0df17f3078bc rigidLoop.cpp --- a/rigidLoop.cpp Thu Apr 05 13:06:24 2012 +0000 +++ b/rigidLoop.cpp Sat Apr 07 14:46:51 2012 +0000 @@ -43,7 +43,7 @@ saccadeRadius=150; // default (initial) shape (the scafold belongs to the base class): - bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); + 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; @@ -58,8 +58,8 @@ // 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(0); - angleCorrectionForceLoop=360.0/bluePrint.scafold.size()/2; // in DEGREES + displaySensingBuffer.setDelayMirrors(1); + angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES break; @@ -72,7 +72,8 @@ 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); + 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; @@ -85,13 +86,17 @@ 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=0.03; + speedContourFollowing=1.3*saccadeRadius; + justSearched=false; - // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0. + // 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: - displaySensingBuffer.setDelayMirrors(0); - angleCorrectionForceLoop=-29;//360.0/bluePrint.scafold.size()/2; // in DEGREES + // 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; @@ -102,14 +107,14 @@ //setColor(0x07);//0x04+0x02>>i); setColor(0x04); - saccadeRadius=65; + saccadeRadius=85; // default (initial) shape (the scafold belongs to the base class): - bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); + bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints); // Numeric parameters for the simulated mechanical system: - massCenter=0.012; - dampMotionCenterMass=0.0001;//0.00015;//00003; - factorBouncingForce=0.0025; // this is because we will use a force on the central mass + 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(); @@ -120,11 +125,13 @@ // 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(0); - angleCorrectionForceLoop=-28.5+180;//-29+180;//360.0/bluePrint.scafold.size()/2; // in DEGREES + displaySensingBuffer.setDelayMirrors(3); + angleCorrectionForceLoop=-5;// in degrees break; } + + saccadeRadius_initial=saccadeRadius; // this is for search mode for instance. } @@ -168,26 +175,37 @@ // 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++) { + 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 -// if (displaySensingBuffer.lightRatio>0.5) momentVector*=-1.0; -// if (displaySensingBuffer.coko<numPoints/2) momentVector*=-1.0; - - 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 { - 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); + 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(); @@ -211,20 +229,32 @@ if (momentNorm>0) { //momentVector/=momentNorm; // We can now compute the sliding vector as: - slidingVector=recenteringVectorLoop.getRotatedDeg(slidingDirection? 90 : -90) / normRecenteringVector * speedContourFollowing*saccadeRadius; - // if (displaySensingBuffer.coko<numPoints/2) slidingVector*=-1.0; - // Then the final correcting vector is the sum of sliding plus recentering vector (with a factor if one want some smothing) + 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 + recenteringVectorLoop * 0.2; - - // centerMass.pos += recenteringVectorLoop*0.1; // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! + + 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. Do nothing for the time being + // 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; @@ -232,11 +262,18 @@ 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: + // 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:: @@ -249,7 +286,7 @@ if (displaySensingBuffer.lightTouched) { // do collision damping: - centerMass.setSpeed(centerMass.getSpeed()*0.99); + centerMass.setSpeed(centerMass.getSpeed()*0.99); } @@ -260,7 +297,7 @@ // OTHER PARTICULAR THINGS: // change sliding direction (for countour following): if (blobWallCollision) { - if (wallCounter>10) { + if (wallCounter>5) { slidingDirection=!slidingDirection; wallCounter=0; }