Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
Diff: rigidLoop.cpp
- Revision:
- 5:73cd58b58f95
- Parent:
- 4:f9d364f10335
- Child:
- 7:0df17f3078bc
--- a/rigidLoop.cpp Wed Apr 04 10:05:25 2012 +0000 +++ b/rigidLoop.cpp Thu Apr 05 12:29:14 2012 +0000 @@ -26,7 +26,7 @@ // (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... + // 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) @@ -40,10 +40,10 @@ //setColor(0x07);//0x04+0x02>>i); setColor(0x04); - saccadeRadius=250; + saccadeRadius=150; // 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); + bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(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; @@ -55,15 +55,16 @@ // 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): + // 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 break; case SPOT_FOLLOWING: - // Name of this kind of spot: sprintf(spotName,"rigid_following"); @@ -83,9 +84,14 @@ // 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); + slidingDirection=true; // For contour following (will change direction when touching wall) + speedContourFollowing=0.03; + + // 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=-29;//360.0/bluePrint.scafold.size()/2; // in DEGREES break; @@ -96,17 +102,14 @@ //setColor(0x07);//0x04+0x02>>i); setColor(0x04); - saccadeRadius=250; + saccadeRadius=65; // 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); + bluePrint.buildCircularScafold(saccadeRadius, vector2D(0,0), 16); //(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; + massCenter=0.012; + dampMotionCenterMass=0.0001;//0.00015;//00003; + factorBouncingForce=0.0025; // 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(); @@ -114,9 +117,11 @@ // 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); + // 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 break; } @@ -165,10 +170,14 @@ 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 + momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0 } } - float momentNorm=momentVector.length(); + +// 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 if (momentNorm==0) { recenteringVectorLoop.set(0,0); normRecenteringVector=0; @@ -199,14 +208,17 @@ 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; + //momentVector/=momentNorm; // We can now compute the sliding vector as: - slidingVector=momentVector.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing*saccadeRadius; + 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) // 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; + + centerMass.pos += slidingVector + recenteringVectorLoop * 0.2; + + // centerMass.pos += recenteringVectorLoop*0.1; // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! // 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") @@ -220,7 +232,12 @@ 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); + + + if (displaySensingBuffer.lightTouched) { + // add force: + centerMass.addForce(recenteringVectorLoop*factorBouncingForce); + } // update dynamics for the central mass:: #ifndef VERLET_METHOD @@ -229,6 +246,12 @@ centerMass.update(); // unconstrained centerMass.bounceOffWalls(); // constrain position (and compute wall "hit") + + if (displaySensingBuffer.lightTouched) { + // do collision damping: + centerMass.setSpeed(centerMass.getSpeed()*0.99); + } + break;