Diff: rigidLoop.cpp

Revision:

3:b44ff6de81bd

Parent:

2:34157ebbf56b

Child:

4:f9d364f10335

--- a/rigidLoop.cpp	Sat Mar 31 12:50:32 2012 +0000
+++ b/rigidLoop.cpp	Mon Apr 02 05:33:44 2012 +0000
@@ -22,10 +22,10 @@
     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...)
-    saccadeRadius=50;
+    saccadeRadius=250;
     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) {
@@ -34,8 +34,8 @@
             // Name of this kind of spot:
             sprintf(spotName,"rigid_following");
 
-            // Color: (use parameter in the future):
-            setColor(0x07);//0x04+0x02>>i);
+            //setColor(0x07);//0x04+0x02>>i);
+            setColor(0x04);
 
             // default (initial) shape (the scafold belongs to the base class):
             bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(-2,5), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
@@ -50,9 +50,11 @@
             // Name of this kind of spot:
             sprintf(spotName,"rigid_bouncing");
 
+            //setColor(0x07);//0x04+0x02>>i);
+            setColor(0x04);
 
             // default (initial) shape (the scafold belongs to the base class):
-            bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(1,5), 10); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
+            bluePrint.buildCircularScafold(saccadeRadius, _initPos, vector2D(0,0), 40); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
 
             // Numeric parameters for the simulated mechanical system:
             massCenter=0.1;
@@ -65,7 +67,7 @@
 
             break;
     }
-            
+
 
     // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
     createLoopFromScafold();
@@ -75,7 +77,7 @@
 
     // !!!!!!!!!!!!!!!!!!!!! ::
     // 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)
+    setDelayMirrors(5); // ATTN!!! needs to be called AFTER setting the number of points!!! (note: 5 seemed good)
 
 }
 
@@ -116,29 +118,30 @@
     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]-centerMass.pos;
+         //   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();
-}
-  
+        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)
@@ -149,18 +152,18 @@
             // 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")
-           
+                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
+                // not on something. Do nothing for the time being
             }
 
             break;