Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

rigidLoop.cpp@31:5f039cbddee8, 2012-09-23 (annotated)

Committer:
mbedalvaro
Date:
Sun Sep 23 10:11:43 2012 +0000
Revision:
31:5f039cbddee8
Parent:
30:d8af03f01cd4
Child:
32:52273c3291fe
this is quite nice, but  I am going to make a deep modification of the bouncing principle: instead of depending on the penetration, it will just be a factor over the speed (perfect elastic bouncing being factorElastic=1, and perfectly absorption=0);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 1:a4050fee11f7 1 #include "rigidLoop.h"
mbedalvaro 1:a4050fee11f7 2
mbedalvaro 1:a4050fee11f7 3 // SHOULD NOT BE HERE: (only because I am using AD_MIRRIOR... max and min in the set region function that should not be here)
mbedalvaro 1:a4050fee11f7 4 #include "hardwareIO.h"
mbedalvaro 1:a4050fee11f7 5
mbedalvaro 31:5f039cbddee8 6 rigidLoop::rigidLoop()
mbedalvaro 31:5f039cbddee8 7 {
mbedalvaro 1:a4050fee11f7 8 }
mbedalvaro 1:a4050fee11f7 9
mbedalvaro 31:5f039cbddee8 10 rigidLoop::~rigidLoop()
mbedalvaro 31:5f039cbddee8 11 {
mbedalvaro 1:a4050fee11f7 12
mbedalvaro 1:a4050fee11f7 13 }
mbedalvaro 1:a4050fee11f7 14
mbedalvaro 1:a4050fee11f7 15
mbedalvaro 1:a4050fee11f7 16 // Note: this method is hidding the abstract method in the base class... and has DIFFERENT parameters than another child would have (enum type).
mbedalvaro 31:5f039cbddee8 17 void rigidLoop::createBlob(int _id, RigidLoopMode _elasticBlobMode, vector2Df _initPos, vector2Df _initSpeed)
mbedalvaro 31:5f039cbddee8 18 {
mbedalvaro 1:a4050fee11f7 19 // (1) set ID:
mbedalvaro 1:a4050fee11f7 20 identifier=_id;
mbedalvaro 1:a4050fee11f7 21
mbedalvaro 1:a4050fee11f7 22 updateMode=_elasticBlobMode;
mbedalvaro 31:5f039cbddee8 23
mbedalvaro 4:f9d364f10335 24 startCenter=_initPos;
mbedalvaro 4:f9d364f10335 25 startSpeed=_initSpeed;
mbedalvaro 31:5f039cbddee8 26
mbedalvaro 1:a4050fee11f7 27 // (2) Initialize common variables of all blobs (base class):
mbedalvaro 1:a4050fee11f7 28 initCommonVariables();
mbedalvaro 1:a4050fee11f7 29
mbedalvaro 1:a4050fee11f7 30 // (3) initialize common variables for the different modes of this rigid loop (even if some are not used, better not to have more subclasses...)
mbedalvaro 18:d72935b13858 31 // Sending data:
mbedalvaro 20:8e82b95180e7 32 periodSendingData=10; // in ms
mbedalvaro 18:d72935b13858 33 sendingRecenteringAngle=true;
mbedalvaro 18:d72935b13858 34 sendingAnchorPosition=true;
mbedalvaro 18:d72935b13858 35 sendingOnlyWhenTouch=true;
mbedalvaro 31:5f039cbddee8 36
mbedalvaro 21:bc9b9383f4b6 37 // Gravity field:
mbedalvaro 31:5f039cbddee8 38 gravity.set(0,0);
mbedalvaro 3:b44ff6de81bd 39
mbedalvaro 1:a4050fee11f7 40 // (3) Initialize secondary variables depending on the behaviour mode (may be changed afterwards in real time)
mbedalvaro 1:a4050fee11f7 41
mbedalvaro 1:a4050fee11f7 42 switch (updateMode) {
mbedalvaro 31:5f039cbddee8 43
mbedalvaro 4:f9d364f10335 44 case SPOT_TEST:
mbedalvaro 4:f9d364f10335 45 // Name of this kind of spot:
mbedalvaro 4:f9d364f10335 46 sprintf(spotName,"spot_test");
mbedalvaro 4:f9d364f10335 47
mbedalvaro 4:f9d364f10335 48 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 4:f9d364f10335 49 setColor(0x04);
mbedalvaro 4:f9d364f10335 50
mbedalvaro 14:0fc33a3a7b4b 51 saccadeRadius=250;
mbedalvaro 4:f9d364f10335 52
mbedalvaro 4:f9d364f10335 53 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 14:0fc33a3a7b4b 54 // NOTE: number of points in the case of need to compute recentering vector needs to be EVEN
mbedalvaro 14:0fc33a3a7b4b 55 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 16); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 4:f9d364f10335 56
mbedalvaro 4:f9d364f10335 57 massCenter=0.01;
mbedalvaro 4:f9d364f10335 58 dampMotionCenterMass=0.001;
mbedalvaro 31:5f039cbddee8 59
mbedalvaro 4:f9d364f10335 60 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 4:f9d364f10335 61 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 62
mbedalvaro 5:73cd58b58f95 63 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 64 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 65 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 66 displaySensingBuffer.setDelayMirrors(1);
mbedalvaro 7:0df17f3078bc 67 angleCorrectionForceLoop=0;//360.0/bluePrint.scafold.size()/2; // in DEGREES
mbedalvaro 4:f9d364f10335 68
mbedalvaro 4:f9d364f10335 69 break;
mbedalvaro 31:5f039cbddee8 70
mbedalvaro 30:d8af03f01cd4 71 case SPOT_TRACK:
mbedalvaro 30:d8af03f01cd4 72 // Name of this kind of spot:
mbedalvaro 30:d8af03f01cd4 73 sprintf(spotName,"spot_track");
mbedalvaro 30:d8af03f01cd4 74
mbedalvaro 30:d8af03f01cd4 75 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 30:d8af03f01cd4 76 setColor(0x04);
mbedalvaro 30:d8af03f01cd4 77
mbedalvaro 31:5f039cbddee8 78 saccadeRadius=50;//+rand()%20;
mbedalvaro 31:5f039cbddee8 79 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 31:5f039cbddee8 80 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 31:5f039cbddee8 81
mbedalvaro 31:5f039cbddee8 82 // Numeric parameters for the simulated mechanical system:
mbedalvaro 31:5f039cbddee8 83 massCenter=0.001;//+0.000005*(rand()%100);
mbedalvaro 31:5f039cbddee8 84 dampMotionCenterMass=0.001;//0.00015;//00003;
mbedalvaro 31:5f039cbddee8 85
mbedalvaro 31:5f039cbddee8 86 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 31:5f039cbddee8 87 createLoopFromScafold();
mbedalvaro 31:5f039cbddee8 88
mbedalvaro 31:5f039cbddee8 89 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 31:5f039cbddee8 90 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 31:5f039cbddee8 91 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 31:5f039cbddee8 92 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 31:5f039cbddee8 93 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 31:5f039cbddee8 94
mbedalvaro 31:5f039cbddee8 95 break;
mbedalvaro 31:5f039cbddee8 96
mbedalvaro 31:5f039cbddee8 97 case SPOT_TRACK_DOT:
mbedalvaro 31:5f039cbddee8 98 // Name of this kind of spot:
mbedalvaro 31:5f039cbddee8 99 sprintf(spotName,"spot_track");
mbedalvaro 31:5f039cbddee8 100
mbedalvaro 31:5f039cbddee8 101 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 31:5f039cbddee8 102 setColor(0x04);
mbedalvaro 30:d8af03f01cd4 103
mbedalvaro 30:d8af03f01cd4 104 saccadeRadius=50;//+rand()%20;
mbedalvaro 30:d8af03f01cd4 105 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 31:5f039cbddee8 106 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 30:d8af03f01cd4 107
mbedalvaro 30:d8af03f01cd4 108 // Numeric parameters for the simulated mechanical system:
mbedalvaro 30:d8af03f01cd4 109 massCenter=0.001;//+0.000005*(rand()%100);
mbedalvaro 30:d8af03f01cd4 110 dampMotionCenterMass=0.001;//0.00015;//00003;
mbedalvaro 30:d8af03f01cd4 111
mbedalvaro 30:d8af03f01cd4 112 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 30:d8af03f01cd4 113 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 114
mbedalvaro 30:d8af03f01cd4 115 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 30:d8af03f01cd4 116 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 30:d8af03f01cd4 117 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 30:d8af03f01cd4 118 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 30:d8af03f01cd4 119 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 30:d8af03f01cd4 120
mbedalvaro 30:d8af03f01cd4 121 break;
mbedalvaro 4:f9d364f10335 122
mbedalvaro 1:a4050fee11f7 123 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 124
mbedalvaro 1:a4050fee11f7 125 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 126 sprintf(spotName,"rigid_following");
mbedalvaro 1:a4050fee11f7 127
mbedalvaro 3:b44ff6de81bd 128 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 3:b44ff6de81bd 129 setColor(0x04);
mbedalvaro 1:a4050fee11f7 130
mbedalvaro 1:a4050fee11f7 131 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 30:d8af03f01cd4 132 saccadeRadius=23;
mbedalvaro 12:0de9cd2bced5 133 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 134
mbedalvaro 1:a4050fee11f7 135 // Note: We may assume NO MASS for the center of the contour following loop. Adding mass may be interesting though (smooth motion).
mbedalvaro 1:a4050fee11f7 136 massCenter=0.01;
mbedalvaro 1:a4050fee11f7 137 dampMotionCenterMass=0.001;
mbedalvaro 1:a4050fee11f7 138
mbedalvaro 4:f9d364f10335 139 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 4:f9d364f10335 140 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 141
mbedalvaro 5:73cd58b58f95 142 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 31:5f039cbddee8 143 speedContourFollowing=startSpeed.length();//1.1*saccadeRadius;
mbedalvaro 7:0df17f3078bc 144 justSearched=false;
mbedalvaro 5:73cd58b58f95 145
mbedalvaro 7:0df17f3078bc 146 // per-blob mirror delay: ONLY USEFUL FOR ELASTIC BLOBS, because otherwise it can be corrected by "angleCorrection"
mbedalvaro 7:0df17f3078bc 147 // (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 148 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 7:0df17f3078bc 149 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop.
mbedalvaro 31:5f039cbddee8 150 // 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
mbedalvaro 7:0df17f3078bc 151 // optimal value, and finish the correction (fine tunned) with the angle correction (only possible in the case of circular rigid blob).
mbedalvaro 7:0df17f3078bc 152 displaySensingBuffer.setDelayMirrors(3); // this corresponds to an angular correction of -delayMirrors * 360/numPoints
mbedalvaro 15:56a0bf424e8d 153 angleCorrectionForceLoop= -5;// good for ONE spot: -5;// in DEGREES
mbedalvaro 4:f9d364f10335 154
mbedalvaro 1:a4050fee11f7 155 break;
mbedalvaro 1:a4050fee11f7 156
mbedalvaro 1:a4050fee11f7 157 case SPOT_BOUNCING:
mbedalvaro 1:a4050fee11f7 158 // Name of this kind of spot:
mbedalvaro 1:a4050fee11f7 159 sprintf(spotName,"rigid_bouncing");
mbedalvaro 1:a4050fee11f7 160
mbedalvaro 31:5f039cbddee8 161 setColor(0x05);
mbedalvaro 1:a4050fee11f7 162
mbedalvaro 31:5f039cbddee8 163 saccadeRadius=30;//+rand()%20;
mbedalvaro 1:a4050fee11f7 164 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 12:0de9cd2bced5 165 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 1:a4050fee11f7 166
mbedalvaro 1:a4050fee11f7 167 // Numeric parameters for the simulated mechanical system:
mbedalvaro 31:5f039cbddee8 168 massCenter=0.0007;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 31:5f039cbddee8 169 dampMotionCenterMass=0.0002;//0.00015;//00003;
mbedalvaro 31:5f039cbddee8 170 factorBouncingForce=0.004; // this is because we will use a force on the central mass
mbedalvaro 28:44b7b6e35548 171
mbedalvaro 28:44b7b6e35548 172 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 28:44b7b6e35548 173 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 174
mbedalvaro 30:d8af03f01cd4 175 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 30:d8af03f01cd4 176 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 30:d8af03f01cd4 177 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 30:d8af03f01cd4 178 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 30:d8af03f01cd4 179 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 30:d8af03f01cd4 180
mbedalvaro 30:d8af03f01cd4 181 break;
mbedalvaro 31:5f039cbddee8 182
mbedalvaro 31:5f039cbddee8 183 case SPOT_PACMAN:
mbedalvaro 31:5f039cbddee8 184 // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
mbedalvaro 31:5f039cbddee8 185 // When it collides, we use the position of the pacman (another spot) and the tangent vector to the blocking line to define the new direction of the
mbedalvaro 31:5f039cbddee8 186 // speed vector.
mbedalvaro 31:5f039cbddee8 187
mbedalvaro 31:5f039cbddee8 188 sprintf(spotName,"pacman");
mbedalvaro 30:d8af03f01cd4 189
mbedalvaro 30:d8af03f01cd4 190 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 30:d8af03f01cd4 191 setColor(0x06); // make it green
mbedalvaro 30:d8af03f01cd4 192
mbedalvaro 30:d8af03f01cd4 193 saccadeRadius=35;//+rand()%20;
mbedalvaro 30:d8af03f01cd4 194 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 30:d8af03f01cd4 195 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 30:d8af03f01cd4 196
mbedalvaro 31:5f039cbddee8 197 massCenter=1.0;
mbedalvaro 31:5f039cbddee8 198 dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
mbedalvaro 31:5f039cbddee8 199 factorBouncingForce=0; //actually not used.
mbedalvaro 31:5f039cbddee8 200 centerMass.dampBorder = 0; // no damping when hitting the mirror limits
mbedalvaro 30:d8af03f01cd4 201
mbedalvaro 30:d8af03f01cd4 202 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 30:d8af03f01cd4 203 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 204
mbedalvaro 30:d8af03f01cd4 205 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 30:d8af03f01cd4 206 speedContourFollowing=1.1*saccadeRadius;
mbedalvaro 30:d8af03f01cd4 207 justSearched=false;
mbedalvaro 30:d8af03f01cd4 208
mbedalvaro 30:d8af03f01cd4 209
mbedalvaro 30:d8af03f01cd4 210 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 30:d8af03f01cd4 211 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 30:d8af03f01cd4 212 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 30:d8af03f01cd4 213 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 30:d8af03f01cd4 214 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 30:d8af03f01cd4 215
mbedalvaro 30:d8af03f01cd4 216 break;
mbedalvaro 31:5f039cbddee8 217
mbedalvaro 31:5f039cbddee8 218 case SPOT_GHOST:
mbedalvaro 31:5f039cbddee8 219 // this will define the behaviour of the "ghosts" in pacman. The spot just moves at a constant speed when there is no object.
mbedalvaro 31:5f039cbddee8 220 // When it collides, we use the position of the pacman (another spot) and the tangent vector to the blocking line to define the new direction of the
mbedalvaro 31:5f039cbddee8 221 // speed vector.
mbedalvaro 31:5f039cbddee8 222
mbedalvaro 31:5f039cbddee8 223 sprintf(spotName,"ghost");
mbedalvaro 30:d8af03f01cd4 224
mbedalvaro 30:d8af03f01cd4 225 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 31:5f039cbddee8 226 setColor(0x05);
mbedalvaro 30:d8af03f01cd4 227
mbedalvaro 31:5f039cbddee8 228 massCenter=1.0;
mbedalvaro 31:5f039cbddee8 229 dampMotionCenterMass=0; // no motion damp (but there will be no forces: only constant uniform motion)
mbedalvaro 31:5f039cbddee8 230 factorBouncingForce=0; //actually not used.
mbedalvaro 31:5f039cbddee8 231 centerMass.dampBorder = 0; // no damping when hitting the mirror limits
mbedalvaro 31:5f039cbddee8 232
mbedalvaro 30:d8af03f01cd4 233
mbedalvaro 30:d8af03f01cd4 234 saccadeRadius=35;//+rand()%20;
mbedalvaro 30:d8af03f01cd4 235 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 30:d8af03f01cd4 236 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 30:d8af03f01cd4 237
mbedalvaro 30:d8af03f01cd4 238
mbedalvaro 30:d8af03f01cd4 239 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 30:d8af03f01cd4 240 createLoopFromScafold();
mbedalvaro 30:d8af03f01cd4 241
mbedalvaro 30:d8af03f01cd4 242 slidingDirection=true; // For contour following (will change direction when touching wall)
mbedalvaro 30:d8af03f01cd4 243 speedContourFollowing=1.1*saccadeRadius;
mbedalvaro 30:d8af03f01cd4 244 justSearched=false;
mbedalvaro 30:d8af03f01cd4 245
mbedalvaro 28:44b7b6e35548 246
mbedalvaro 28:44b7b6e35548 247 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 28:44b7b6e35548 248 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 28:44b7b6e35548 249 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 28:44b7b6e35548 250 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 28:44b7b6e35548 251 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 28:44b7b6e35548 252
mbedalvaro 28:44b7b6e35548 253 break;
mbedalvaro 31:5f039cbddee8 254
mbedalvaro 31:5f039cbddee8 255 case SPOT_AIR_HOCKEY:
mbedalvaro 28:44b7b6e35548 256 // Name of this kind of spot:
mbedalvaro 28:44b7b6e35548 257 sprintf(spotName,"air_hockey");
mbedalvaro 28:44b7b6e35548 258
mbedalvaro 28:44b7b6e35548 259 //startCenter.set(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
mbedalvaro 31:5f039cbddee8 260 //startSpeed.set(0,0);
mbedalvaro 28:44b7b6e35548 261
mbedalvaro 28:44b7b6e35548 262 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 28:44b7b6e35548 263 setColor(0x04);
mbedalvaro 31:5f039cbddee8 264
mbedalvaro 28:44b7b6e35548 265 gravity.set(0,0);
mbedalvaro 28:44b7b6e35548 266
mbedalvaro 28:44b7b6e35548 267 saccadeRadius=50;//+rand()%20;
mbedalvaro 28:44b7b6e35548 268 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 28:44b7b6e35548 269 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos,vector2D _vel, int _numScafoldPoints);
mbedalvaro 28:44b7b6e35548 270
mbedalvaro 28:44b7b6e35548 271 // Numeric parameters for the simulated mechanical system:
mbedalvaro 28:44b7b6e35548 272 massCenter=0.0008;//+0.000005*(rand()%100);
mbedalvaro 28:44b7b6e35548 273 dampMotionCenterMass=0.00065;//0.00015;//00003;
mbedalvaro 7:0df17f3078bc 274 factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 1:a4050fee11f7 275
mbedalvaro 4:f9d364f10335 276 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 4:f9d364f10335 277 createLoopFromScafold();
mbedalvaro 4:f9d364f10335 278
mbedalvaro 5:73cd58b58f95 279 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 5:73cd58b58f95 280 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 5:73cd58b58f95 281 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 7:0df17f3078bc 282 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 7:0df17f3078bc 283 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 1:a4050fee11f7 284
mbedalvaro 1:a4050fee11f7 285 break;
mbedalvaro 31:5f039cbddee8 286
mbedalvaro 31:5f039cbddee8 287 case SPOT_LORENTZ_FORCE:
mbedalvaro 27:1ce994629ffc 288 // Name of this kind of spot:
mbedalvaro 27:1ce994629ffc 289 sprintf(spotName,"rigid_fountain");
mbedalvaro 27:1ce994629ffc 290
mbedalvaro 27:1ce994629ffc 291 //setColor(0x07);//0x04+0x02>>i);
mbedalvaro 27:1ce994629ffc 292 setColor(0x04);
mbedalvaro 27:1ce994629ffc 293
mbedalvaro 29:2fc8c12822eb 294 saccadeRadius=20;//+rand()%20;
mbedalvaro 27:1ce994629ffc 295 // default (initial) shape (the scafold belongs to the base class):
mbedalvaro 28:44b7b6e35548 296 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), 18); //(float _radius, vector2D _pos, int _numScafoldPoints);
mbedalvaro 27:1ce994629ffc 297
mbedalvaro 27:1ce994629ffc 298 // Numeric parameters for the simulated mechanical system:
mbedalvaro 27:1ce994629ffc 299 massCenter=0.0005;//+0.000005*(rand()%100);
mbedalvaro 28:44b7b6e35548 300 dampMotionCenterMass=0.001;//0.00015;//00003;
mbedalvaro 28:44b7b6e35548 301 factorBouncingForce=0.0015; // this is because we will use a force on the central mass
mbedalvaro 27:1ce994629ffc 302
mbedalvaro 27:1ce994629ffc 303 // Finally, we can create the loop (not much to do in this case, only set the central position, and some other things):
mbedalvaro 27:1ce994629ffc 304 createLoopFromScafold();
mbedalvaro 27:1ce994629ffc 305
mbedalvaro 27:1ce994629ffc 306 // per-blob mirror delay (if things were well adjusted - in particular mirror waiting times, then this could be 0.
mbedalvaro 27:1ce994629ffc 307 // But in case of unique blobs, it may be interesting to accelerate display AND correct the delay by software).
mbedalvaro 27:1ce994629ffc 308 // Even more interesting: in case of rigid circular blob, this can be coorected using angleCorrectionForceLoop:
mbedalvaro 27:1ce994629ffc 309 displaySensingBuffer.setDelayMirrors(3);
mbedalvaro 27:1ce994629ffc 310 angleCorrectionForceLoop=-5;// in degrees
mbedalvaro 27:1ce994629ffc 311
mbedalvaro 27:1ce994629ffc 312 break;
mbedalvaro 1:a4050fee11f7 313 }
mbedalvaro 31:5f039cbddee8 314
mbedalvaro 31:5f039cbddee8 315 saccadeRadius_initial=saccadeRadius; // this is for search mode for instance.
mbedalvaro 31:5f039cbddee8 316
mbedalvaro 12:0de9cd2bced5 317 // Excursion limits (for all points). Tthis 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:
mbedalvaro 12:0de9cd2bced5 318 setRegionMotion(MIN_AD_MIRRORS+saccadeRadius, MIN_AD_MIRRORS+saccadeRadius, MAX_AD_MIRRORS-saccadeRadius, MAX_AD_MIRRORS-saccadeRadius);
mbedalvaro 12:0de9cd2bced5 319
mbedalvaro 26:c9329c4fc20a 320 // draw it once on the display buffer for good initialization:
mbedalvaro 26:c9329c4fc20a 321 draw();
mbedalvaro 1:a4050fee11f7 322 }
mbedalvaro 1:a4050fee11f7 323
mbedalvaro 1:a4050fee11f7 324
mbedalvaro 31:5f039cbddee8 325 void rigidLoop::initSizeBlob(int _numPoints)
mbedalvaro 31:5f039cbddee8 326 {
mbedalvaro 1:a4050fee11f7 327 // Iinitialize blob size (number of points for the loop, as well as other structures such as lsdTrajectory)
mbedalvaro 1:a4050fee11f7 328 numPoints=_numPoints;
mbedalvaro 1:a4050fee11f7 329
mbedalvaro 1:a4050fee11f7 330 // Sensing and Display trajectory:
mbedalvaro 1:a4050fee11f7 331 displaySensingBuffer.lsdTrajectory.resize(numPoints); // the lsdTrajectory and the elastic loop will have the same number of points (this could be different - decimation?).
mbedalvaro 1:a4050fee11f7 332 }
mbedalvaro 1:a4050fee11f7 333
mbedalvaro 31:5f039cbddee8 334 void rigidLoop::createLoopFromScafold(void)
mbedalvaro 31:5f039cbddee8 335 {
mbedalvaro 1:a4050fee11f7 336
mbedalvaro 1:a4050fee11f7 337 initSizeBlob(bluePrint.scafold.size()); // very simple here (only need to set the size of the lsd buffer)
mbedalvaro 1:a4050fee11f7 338
mbedalvaro 1:a4050fee11f7 339 centerMass.mass=massCenter;
mbedalvaro 1:a4050fee11f7 340 centerMass.dampMotion = dampMotionCenterMass;
mbedalvaro 1:a4050fee11f7 341
mbedalvaro 1:a4050fee11f7 342 // note: the following may not be required in case of contour following:
mbedalvaro 2:34157ebbf56b 343 centerMass.setIntegrationStep(0.23); // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
mbedalvaro 4:f9d364f10335 344 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 1:a4050fee11f7 345 // centerMass.setInitialCondition(2047.0, 2047.0,0.0,0.0);
mbedalvaro 31:5f039cbddee8 346
mbedalvaro 31:5f039cbddee8 347 }
mbedalvaro 31:5f039cbddee8 348
mbedalvaro 31:5f039cbddee8 349 void rigidLoop::setRegionMotion(float mmix, float mmiy, float mmax, float mmay) // wrapper for setWallLimits, because there is no more things to do than set this for a unique mass
mbedalvaro 31:5f039cbddee8 350 {
mbedalvaro 31:5f039cbddee8 351 // centerMass.setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 31:5f039cbddee8 352 // Use the static method of the pointMass class:
mbedalvaro 31:5f039cbddee8 353 pointMass::setWallLimits(mmix+10, mmiy+10, mmax-10, mmay-10);
mbedalvaro 1:a4050fee11f7 354 }
mbedalvaro 1:a4050fee11f7 355
mbedalvaro 31:5f039cbddee8 356 void rigidLoop::speedFactor(float speedfactor)
mbedalvaro 31:5f039cbddee8 357 {
mbedalvaro 31:5f039cbddee8 358 // in case of spot following:
mbedalvaro 31:5f039cbddee8 359 speedContourFollowing*=speedfactor;
mbedalvaro 31:5f039cbddee8 360
mbedalvaro 31:5f039cbddee8 361 // in case of bouncing, there are many ways to change the speed (play with the mass, damping or the bouncing force).
mbedalvaro 31:5f039cbddee8 362 //centerMass.mass/=speedfactor;//0.0008;//+0.000005*(rand()%100);
mbedalvaro 31:5f039cbddee8 363 centerMass.dampMotion/=speedfactor;//0.00045;//0.00015;//00003;
mbedalvaro 31:5f039cbddee8 364 //factorBouncingForce=0.0018; // this is because we will use a force on the central mass
mbedalvaro 31:5f039cbddee8 365
mbedalvaro 31:5f039cbddee8 366
mbedalvaro 1:a4050fee11f7 367 }
mbedalvaro 1:a4050fee11f7 368
mbedalvaro 24:4e52031a495b 369
mbedalvaro 31:5f039cbddee8 370 void rigidLoop::explosion()
mbedalvaro 31:5f039cbddee8 371 {
mbedalvaro 31:5f039cbddee8 372 transientBlobColor=blobColor|0x02;
mbedalvaro 31:5f039cbddee8 373 for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
mbedalvaro 31:5f039cbddee8 374 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 31:5f039cbddee8 375 draw();
mbedalvaro 31:5f039cbddee8 376 }
mbedalvaro 31:5f039cbddee8 377 saccadeRadius=saccadeRadius_initial;
mbedalvaro 31:5f039cbddee8 378 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 31:5f039cbddee8 379 // reset to central position:
mbedalvaro 31:5f039cbddee8 380 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 31:5f039cbddee8 381 transientBlobColor=blobColor;
mbedalvaro 24:4e52031a495b 382 }
mbedalvaro 1:a4050fee11f7 383
mbedalvaro 31:5f039cbddee8 384 vector2Df rigidLoop::getCenter()
mbedalvaro 31:5f039cbddee8 385 {
mbedalvaro 31:5f039cbddee8 386 return(centerMass.pos);
mbedalvaro 31:5f039cbddee8 387 }
mbedalvaro 31:5f039cbddee8 388
mbedalvaro 31:5f039cbddee8 389 void rigidLoop::update(vector2Df referencePos)
mbedalvaro 31:5f039cbddee8 390 {
mbedalvaro 1:a4050fee11f7 391
mbedalvaro 1:a4050fee11f7 392 // (I) process loop geometry: not needed (rigid)
mbedalvaro 1:a4050fee11f7 393 // Just check if the blob touched the borders (only need to do this with the central mass):
mbedalvaro 1:a4050fee11f7 394 blobWallCollision=centerMass.bWallCollision;
mbedalvaro 1:a4050fee11f7 395
mbedalvaro 1:a4050fee11f7 396 // (II) Process sensing buffer and compute light forces:
mbedalvaro 1:a4050fee11f7 397 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)
mbedalvaro 1:a4050fee11f7 398
mbedalvaro 1:a4050fee11f7 399 // (III) Compute recentering vector (the "penetration vector in fact"), using "first order moment":
mbedalvaro 3:b44ff6de81bd 400 // 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
mbedalvaro 3:b44ff6de81bd 401 // sum in the circle is 0).
mbedalvaro 12:0de9cd2bced5 402 vector2Df momentVector(0,0);
mbedalvaro 7:0df17f3078bc 403 int counterDarkZone=0; // note: for a VERY strange reason, if I put this on the laserSensingtrajectory class, the program does not work anymore!!
mbedalvaro 7:0df17f3078bc 404 for (int i = 0; i < numPoints; i++) { // note: numPoints should be EVEN
mbedalvaro 1:a4050fee11f7 405 if (displaySensingBuffer.lsdTrajectory[i].lightZone>0) { // this is, we are in a dark zone (better to integrate there, because it is normally smaller)
mbedalvaro 31:5f039cbddee8 406
mbedalvaro 31:5f039cbddee8 407 momentVector.x+=(float)bluePrint.scafold[i].x; // note: casting is happening here automatically (unsigned short to float), but I put (float) to remember that types are different
mbedalvaro 31:5f039cbddee8 408 momentVector.y+=(float)bluePrint.scafold[i].y;
mbedalvaro 31:5f039cbddee8 409
mbedalvaro 31:5f039cbddee8 410 // We can also do the following, but ATTENTION: momentVector is of type vector2Df, and scafold[i] of type vector2Dd...
mbedalvaro 12:0de9cd2bced5 411 // momentVector+=bluePrint.scafold[i];// note: no need to do -centerMass.pos, because the scafold is "centered" around 0
mbedalvaro 31:5f039cbddee8 412
mbedalvaro 7:0df17f3078bc 413 counterDarkZone++;
mbedalvaro 1:a4050fee11f7 414 }
mbedalvaro 1:a4050fee11f7 415 }
mbedalvaro 7:0df17f3078bc 416 momentVector=momentVector*(2*PI/numPoints);
mbedalvaro 7:0df17f3078bc 417 float momentNorm=momentVector.length(); // = 2.R.sin(half_angle) in the direction of the dark zone
mbedalvaro 31:5f039cbddee8 418
mbedalvaro 12:0de9cd2bced5 419 vector2Df unitTowardsLight; // this is the normed vector, pointing towards the light zone
mbedalvaro 2:34157ebbf56b 420 if (momentNorm==0) {
mbedalvaro 7:0df17f3078bc 421 unitTowardsLight.set(0,0);
mbedalvaro 3:b44ff6de81bd 422 recenteringVectorLoop.set(0,0);
mbedalvaro 3:b44ff6de81bd 423 normRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 424 angleRecenteringVector=0;
mbedalvaro 3:b44ff6de81bd 425 } else {
mbedalvaro 31:5f039cbddee8 426 unitTowardsLight=momentVector/(-1.0*momentNorm);
mbedalvaro 31:5f039cbddee8 427 // Apply correction angle (delay mirrors):
mbedalvaro 7:0df17f3078bc 428 unitTowardsLight.rotateDeg(angleCorrectionForceLoop);
mbedalvaro 31:5f039cbddee8 429
mbedalvaro 31:5f039cbddee8 430 // Compute "recenteringVectorLoop": the vector making the spot goes completely AWAY form the dark zone
mbedalvaro 31:5f039cbddee8 431 float aux=0.5*momentNorm/saccadeRadius; // note: in principle, we ALWAYS have momentNorm < 2.R, so aux < 1
mbedalvaro 31:5f039cbddee8 432 if (aux>1) aux=1.0; // can happen because of the discrete integration!
mbedalvaro 31:5f039cbddee8 433 if (counterDarkZone<=numPoints/2) { // note: numPoints HAS to be EVEN
mbedalvaro 7:0df17f3078bc 434 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0-sqrt(1.0-aux*aux));
mbedalvaro 31:5f039cbddee8 435 } else {
mbedalvaro 31:5f039cbddee8 436 recenteringVectorLoop=unitTowardsLight*saccadeRadius*(1.0+sqrt(1.0-aux*aux));
mbedalvaro 31:5f039cbddee8 437 }
mbedalvaro 31:5f039cbddee8 438
mbedalvaro 31:5f039cbddee8 439
mbedalvaro 3:b44ff6de81bd 440 // Compute redundant quantities (if necessary, for sending through OSC, etc):
mbedalvaro 3:b44ff6de81bd 441 normRecenteringVector=recenteringVectorLoop.length();
mbedalvaro 3:b44ff6de81bd 442 angleRecenteringVector=recenteringVectorLoop.angleDegHoriz();
mbedalvaro 3:b44ff6de81bd 443 }
mbedalvaro 3:b44ff6de81bd 444
mbedalvaro 30:d8af03f01cd4 445 // ======================== Now, depending on the mode of operation, we have different types of behaviour ========================================
mbedalvaro 31:5f039cbddee8 446
mbedalvaro 12:0de9cd2bced5 447 vector2Df slidingVector; //( need to declare it here because a switch "jump" cannot bypass an initialization)
mbedalvaro 31:5f039cbddee8 448 vector2Df auxVector;
mbedalvaro 1:a4050fee11f7 449 switch (updateMode) {
mbedalvaro 31:5f039cbddee8 450 // ================================================================
mbedalvaro 31:5f039cbddee8 451 case SPOT_TEST: // this is just for adjusting mirror delays, checking recentering vector, etc:
mbedalvaro 31:5f039cbddee8 452 // do nothing for the time being
mbedalvaro 31:5f039cbddee8 453 // 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.
mbedalvaro 31:5f039cbddee8 454 // 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.
mbedalvaro 31:5f039cbddee8 455
mbedalvaro 31:5f039cbddee8 456 // (1) current color: change with touch? NO
mbedalvaro 31:5f039cbddee8 457 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 458
mbedalvaro 31:5f039cbddee8 459 break;
mbedalvaro 31:5f039cbddee8 460 // ================================================================
mbedalvaro 30:d8af03f01cd4 461 case SPOT_TRACK:
mbedalvaro 31:5f039cbddee8 462 centerMass.pos +=recenteringVectorLoop;
mbedalvaro 31:5f039cbddee8 463 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 31:5f039cbddee8 464 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 465
mbedalvaro 30:d8af03f01cd4 466 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 467 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 468 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 469 else
mbedalvaro 31:5f039cbddee8 470 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 471
mbedalvaro 30:d8af03f01cd4 472 break;
mbedalvaro 31:5f039cbddee8 473 // ================================================================
mbedalvaro 31:5f039cbddee8 474 case SPOT_TRACK_DOT: // here, a dot in the center of the saccade should remain in the center:
mbedalvaro 31:5f039cbddee8 475 centerMass.pos -=recenteringVectorLoop;
mbedalvaro 31:5f039cbddee8 476 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 31:5f039cbddee8 477 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 478
mbedalvaro 31:5f039cbddee8 479 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 480 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 481 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 482 else
mbedalvaro 31:5f039cbddee8 483 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 484
mbedalvaro 31:5f039cbddee8 485 break;
mbedalvaro 31:5f039cbddee8 486 // ================================================================
mbedalvaro 1:a4050fee11f7 487 case SPOT_FOLLOWING:
mbedalvaro 1:a4050fee11f7 488 // we need to compute the tangencial "speed":
mbedalvaro 1:a4050fee11f7 489 // vector2D slidingVector;
mbedalvaro 2:34157ebbf56b 490 if (momentNorm>0) {
mbedalvaro 5:73cd58b58f95 491 //momentVector/=momentNorm;
mbedalvaro 3:b44ff6de81bd 492 // We can now compute the sliding vector as:
mbedalvaro 7:0df17f3078bc 493 slidingVector=unitTowardsLight.getRotatedDeg(slidingDirection? 90 : -90) * speedContourFollowing;
mbedalvaro 31:5f039cbddee8 494
mbedalvaro 7:0df17f3078bc 495 // Then the final correcting vector is the sum of sliding plus a recentering vector (with a factor if one want some smothing)
mbedalvaro 3:b44ff6de81bd 496 // 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):
mbedalvaro 7:0df17f3078bc 497 centerMass.pos +=slidingVector+ ( unitTowardsLight*(-1.0*saccadeRadius) + recenteringVectorLoop )* 0.6;
mbedalvaro 31:5f039cbddee8 498 // ATTENTION!!! the REAL radius may be smaller if the mirrors are running fast!!! (hence the last factor, that is not only for "smoothing" the
mbedalvaro 31:5f039cbddee8 499 // re-entry and avoid oscillations).
mbedalvaro 3:b44ff6de81bd 500
mbedalvaro 3:b44ff6de81bd 501 // The following function can help constraining the position "pos", but it also does too much. Do something simpler perhaps?
mbedalvaro 7:0df17f3078bc 502 centerMass.posOld=centerMass.pos; // this is necessary to compute bouceOffWalls using Verlet method... (MAKE A new variable INTEGRATION METHOD?)
mbedalvaro 31:5f039cbddee8 503
mbedalvaro 3:b44ff6de81bd 504 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 505
mbedalvaro 7:0df17f3078bc 506 if (justSearched) {
mbedalvaro 7:0df17f3078bc 507 saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 508 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 509 justSearched=false;
mbedalvaro 31:5f039cbddee8 510 }
mbedalvaro 31:5f039cbddee8 511
mbedalvaro 2:34157ebbf56b 512 } else {
mbedalvaro 7:0df17f3078bc 513 // not on something. SEARCH MODE (or go to spot_bouncing mode?)
mbedalvaro 31:5f039cbddee8 514 saccadeRadius+=30;
mbedalvaro 31:5f039cbddee8 515 if (saccadeRadius>800) saccadeRadius=saccadeRadius_initial;
mbedalvaro 12:0de9cd2bced5 516 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 7:0df17f3078bc 517 justSearched=true;
mbedalvaro 2:34157ebbf56b 518 }
mbedalvaro 1:a4050fee11f7 519
mbedalvaro 31:5f039cbddee8 520 // Change color with touch? NO
mbedalvaro 31:5f039cbddee8 521 // if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 522 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 523 // else
mbedalvaro 27:1ce994629ffc 524 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 525
mbedalvaro 31:5f039cbddee8 526 // change sliding direction (for countour following):
mbedalvaro 31:5f039cbddee8 527 if (blobWallCollision) {
mbedalvaro 31:5f039cbddee8 528 if (wallCounter>5) {
mbedalvaro 31:5f039cbddee8 529 slidingDirection=!slidingDirection;
mbedalvaro 31:5f039cbddee8 530 wallCounter=0;
mbedalvaro 31:5f039cbddee8 531 }
mbedalvaro 28:44b7b6e35548 532 }
mbedalvaro 31:5f039cbddee8 533 wallCounter++;
mbedalvaro 27:1ce994629ffc 534
mbedalvaro 1:a4050fee11f7 535 break;
mbedalvaro 31:5f039cbddee8 536 // ================================================================
mbedalvaro 31:5f039cbddee8 537 case SPOT_GHOST:
mbedalvaro 31:5f039cbddee8 538 // This is not completely sliding nor bouncing, but a combination of both
mbedalvaro 31:5f039cbddee8 539 // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
mbedalvaro 31:5f039cbddee8 540 // - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
mbedalvaro 31:5f039cbddee8 541 // - also, choose the direction so as to APPROACH THE PACMAN (position of the pacman is in referencePos, a parameter to "update" method).
mbedalvaro 31:5f039cbddee8 542
mbedalvaro 31:5f039cbddee8 543 if (momentNorm>0) {
mbedalvaro 31:5f039cbddee8 544
mbedalvaro 31:5f039cbddee8 545 // first, get the current speed:
mbedalvaro 31:5f039cbddee8 546 auxVector=centerMass.getSpeed();
mbedalvaro 31:5f039cbddee8 547
mbedalvaro 31:5f039cbddee8 548 // Before recalculating the speed (that will recompute pos from posOld), set posOld well outside the dark zone (this is
mbedalvaro 31:5f039cbddee8 549 // necessary because if the printed pattern move and the speed is slow, then there is not enough bouncing!):
mbedalvaro 31:5f039cbddee8 550 centerMass.posOld=centerMass.pos+recenteringVectorLoop;
mbedalvaro 31:5f039cbddee8 551
mbedalvaro 31:5f039cbddee8 552 // then compute the new bounce speed vector:
mbedalvaro 31:5f039cbddee8 553 float aux=unitTowardsLight.dot(auxVector);
mbedalvaro 31:5f039cbddee8 554 float anglepac=unitTowardsLight.angleDeg(referencePos-centerMass.pos); // angle from unit vector to (pacman-center)
mbedalvaro 31:5f039cbddee8 555 if (abs(anglepac)<85)
mbedalvaro 31:5f039cbddee8 556 slidingVector= referencePos-centerMass.pos;
mbedalvaro 31:5f039cbddee8 557 //slidingVector= auxVector-unitTowardsLight*aux*2; // this is a normal bounce
mbedalvaro 31:5f039cbddee8 558 else
mbedalvaro 31:5f039cbddee8 559 slidingVector=unitTowardsLight.getRotatedDeg((anglepac>0)? 85 : -85);
mbedalvaro 31:5f039cbddee8 560
mbedalvaro 31:5f039cbddee8 561 slidingVector.scale(auxVector.length()); // do not forget to scale...
mbedalvaro 31:5f039cbddee8 562 // then reset speed:
mbedalvaro 31:5f039cbddee8 563 centerMass.setSpeed(slidingVector);
mbedalvaro 31:5f039cbddee8 564 }
mbedalvaro 5:73cd58b58f95 565
mbedalvaro 31:5f039cbddee8 566 // update dynamics for the central mass:
mbedalvaro 31:5f039cbddee8 567 #ifndef VERLET_METHOD
mbedalvaro 31:5f039cbddee8 568 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 31:5f039cbddee8 569 #endif
mbedalvaro 31:5f039cbddee8 570
mbedalvaro 31:5f039cbddee8 571 centerMass.update(); // unconstrained
mbedalvaro 31:5f039cbddee8 572 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 573
mbedalvaro 31:5f039cbddee8 574 // Change color with touch? NO
mbedalvaro 31:5f039cbddee8 575 // if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 576 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 577 // else
mbedalvaro 31:5f039cbddee8 578 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 579
mbedalvaro 31:5f039cbddee8 580
mbedalvaro 31:5f039cbddee8 581 break;
mbedalvaro 31:5f039cbddee8 582
mbedalvaro 31:5f039cbddee8 583 // ================================================================
mbedalvaro 31:5f039cbddee8 584 case SPOT_PACMAN:
mbedalvaro 31:5f039cbddee8 585 // This is not completely sliding nor bouncing, but a combination of both
mbedalvaro 31:5f039cbddee8 586 // Behaviour: - if the spot is NOT touching anything, just move with uniform speed (always constant speed in norm).
mbedalvaro 31:5f039cbddee8 587 // - if the spot touch something, then modify the speed so that it ALIGNS with the tangential vector, without changing its norm.
mbedalvaro 31:5f039cbddee8 588 // - also, choose the direction so that it minimizes the angle with the previous speed vector (a little like bouncing):
mbedalvaro 31:5f039cbddee8 589
mbedalvaro 31:5f039cbddee8 590 if (momentNorm>0) {
mbedalvaro 31:5f039cbddee8 591
mbedalvaro 31:5f039cbddee8 592 // (a) Compute the new speed (will use slidingVector as auxiliary vector2Df):
mbedalvaro 31:5f039cbddee8 593 auxVector=centerMass.getSpeed();
mbedalvaro 31:5f039cbddee8 594 float aux=unitTowardsLight.dot(auxVector);
mbedalvaro 31:5f039cbddee8 595 if (aux<0) {
mbedalvaro 31:5f039cbddee8 596 slidingVector=auxVector-(unitTowardsLight*aux*2); // get only the component perpendicular to unitTowards light
mbedalvaro 31:5f039cbddee8 597 // slidingVector.scale(auxVector.length()); // rescale to the size of the initial speed.
mbedalvaro 31:5f039cbddee8 598 centerMass.setSpeed(slidingVector);
mbedalvaro 31:5f039cbddee8 599 }
mbedalvaro 31:5f039cbddee8 600
mbedalvaro 5:73cd58b58f95 601 }
mbedalvaro 27:1ce994629ffc 602
mbedalvaro 27:1ce994629ffc 603 // update dynamics for the central mass:
mbedalvaro 27:1ce994629ffc 604 #ifndef VERLET_METHOD
mbedalvaro 27:1ce994629ffc 605 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 27:1ce994629ffc 606 #endif
mbedalvaro 27:1ce994629ffc 607
mbedalvaro 27:1ce994629ffc 608 centerMass.update(); // unconstrained
mbedalvaro 27:1ce994629ffc 609 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 610
mbedalvaro 31:5f039cbddee8 611 // Change color with touch? NO
mbedalvaro 31:5f039cbddee8 612 // if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 613 // transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 614 // else
mbedalvaro 31:5f039cbddee8 615 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 616
mbedalvaro 31:5f039cbddee8 617
mbedalvaro 31:5f039cbddee8 618 break;
mbedalvaro 31:5f039cbddee8 619
mbedalvaro 31:5f039cbddee8 620
mbedalvaro 31:5f039cbddee8 621
mbedalvaro 31:5f039cbddee8 622 // ================================================================
mbedalvaro 31:5f039cbddee8 623 case SPOT_BOUNCING:
mbedalvaro 31:5f039cbddee8 624 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector.
mbedalvaro 31:5f039cbddee8 625 // We can also MODIFY the position so as to avoid having completely or partially the spot inside the dark zone (because of inertia).
mbedalvaro 31:5f039cbddee8 626 centerMass.resetForce();
mbedalvaro 31:5f039cbddee8 627
mbedalvaro 31:5f039cbddee8 628 if (momentNorm>0) { //(displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 629 // add force; MANY POSSIBILITIES:
mbedalvaro 31:5f039cbddee8 630 // (1) Constant in norm:
mbedalvaro 31:5f039cbddee8 631 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 632
mbedalvaro 31:5f039cbddee8 633 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 31:5f039cbddee8 634 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 635 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 31:5f039cbddee8 636 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 637
mbedalvaro 31:5f039cbddee8 638 // Also, translate to avoid penetration (need to do this with pos and oldPos, otherwise speed will change):
mbedalvaro 31:5f039cbddee8 639 centerMass.posOld+=recenteringVectorLoop*0.1;
mbedalvaro 31:5f039cbddee8 640 centerMass.pos+=recenteringVectorLoop*0.1;
mbedalvaro 31:5f039cbddee8 641 //note: we don't change the speed, this would be just like the pacman: not really math modelling:
mbedalvaro 31:5f039cbddee8 642 // centerMass.setSpeed(-centerMass.getSpeed());
mbedalvaro 31:5f039cbddee8 643 }
mbedalvaro 31:5f039cbddee8 644
mbedalvaro 31:5f039cbddee8 645 // Gravity? - side or central attraction?
mbedalvaro 31:5f039cbddee8 646 centerMass.addForce(gravity*centerMass.mass);
mbedalvaro 31:5f039cbddee8 647
mbedalvaro 31:5f039cbddee8 648 // or central spring attraction;
mbedalvaro 31:5f039cbddee8 649 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 650 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 31:5f039cbddee8 651 //centerMass.addForce(-dist*centerMass.mass*0.0007);
mbedalvaro 31:5f039cbddee8 652
mbedalvaro 31:5f039cbddee8 653 // or "radial gravity":
mbedalvaro 31:5f039cbddee8 654 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 655 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 31:5f039cbddee8 656 //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 31:5f039cbddee8 657
mbedalvaro 31:5f039cbddee8 658
mbedalvaro 31:5f039cbddee8 659 // update dynamics for the central mass:
mbedalvaro 31:5f039cbddee8 660 #ifndef VERLET_METHOD
mbedalvaro 31:5f039cbddee8 661 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 31:5f039cbddee8 662 #endif
mbedalvaro 31:5f039cbddee8 663
mbedalvaro 31:5f039cbddee8 664 centerMass.update(); // unconstrained
mbedalvaro 31:5f039cbddee8 665 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 666
mbedalvaro 31:5f039cbddee8 667 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 668 // do collision damping:
mbedalvaro 31:5f039cbddee8 669 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 31:5f039cbddee8 670 }
mbedalvaro 31:5f039cbddee8 671
mbedalvaro 27:1ce994629ffc 672 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 673 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 674 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 675 else
mbedalvaro 31:5f039cbddee8 676 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 27:1ce994629ffc 677 break;
mbedalvaro 31:5f039cbddee8 678
mbedalvaro 30:d8af03f01cd4 679 // ================================================================
mbedalvaro 31:5f039cbddee8 680 case SPOT_AIR_HOCKEY:
mbedalvaro 27:1ce994629ffc 681 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 27:1ce994629ffc 682 centerMass.resetForce();
mbedalvaro 27:1ce994629ffc 683
mbedalvaro 31:5f039cbddee8 684 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 685 // add force; MANY POSSIBILITIES:
mbedalvaro 31:5f039cbddee8 686 // (1) Constant in norm:
mbedalvaro 31:5f039cbddee8 687 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 688 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 31:5f039cbddee8 689
mbedalvaro 31:5f039cbddee8 690 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 31:5f039cbddee8 691 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 692 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 31:5f039cbddee8 693 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 694
mbedalvaro 27:1ce994629ffc 695 }
mbedalvaro 31:5f039cbddee8 696
mbedalvaro 28:44b7b6e35548 697 // Gravity? - side or central attraction?
mbedalvaro 31:5f039cbddee8 698 //centerMass.addForce(gravity*centerMass.mass);
mbedalvaro 31:5f039cbddee8 699
mbedalvaro 28:44b7b6e35548 700 // or central spring attraction;
mbedalvaro 31:5f039cbddee8 701 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 28:44b7b6e35548 702 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 28:44b7b6e35548 703 //centerMass.addForce(-dist*centerMass.mass*0.0007);
mbedalvaro 31:5f039cbddee8 704
mbedalvaro 28:44b7b6e35548 705 // or "radial gravity":
mbedalvaro 31:5f039cbddee8 706 //vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 28:44b7b6e35548 707 //vector2Df dist=centerMass.pos-centerAttraction;
mbedalvaro 28:44b7b6e35548 708 //centerMass.addForce(dist.normalize()*centerMass.mass*0.5);
mbedalvaro 31:5f039cbddee8 709
mbedalvaro 1:a4050fee11f7 710
mbedalvaro 25:74cb85b85fd2 711 // update dynamics for the central mass:
mbedalvaro 1:a4050fee11f7 712 #ifndef VERLET_METHOD
mbedalvaro 1:a4050fee11f7 713 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 1:a4050fee11f7 714 #endif
mbedalvaro 1:a4050fee11f7 715
mbedalvaro 1:a4050fee11f7 716 centerMass.update(); // unconstrained
mbedalvaro 1:a4050fee11f7 717 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 718
mbedalvaro 31:5f039cbddee8 719 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 720 // do collision damping:
mbedalvaro 31:5f039cbddee8 721 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 31:5f039cbddee8 722 }
mbedalvaro 31:5f039cbddee8 723
mbedalvaro 27:1ce994629ffc 724 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 725 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 726 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 727 else
mbedalvaro 31:5f039cbddee8 728 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 729
mbedalvaro 31:5f039cbddee8 730 // In case of "air hockey mode", reset position to initial positions and speeds when the spot touches any two opposites sides:
mbedalvaro 31:5f039cbddee8 731 if ((centerMass.innerCollitionDirection.x==1)||( centerMass.innerCollitionDirection.x==-1)) {
mbedalvaro 31:5f039cbddee8 732 transientBlobColor=blobColor|0x02;
mbedalvaro 30:d8af03f01cd4 733 for (saccadeRadius=30; saccadeRadius<900 ; saccadeRadius+=10) {
mbedalvaro 30:d8af03f01cd4 734 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 31:5f039cbddee8 735 draw();
mbedalvaro 30:d8af03f01cd4 736 }
mbedalvaro 30:d8af03f01cd4 737 saccadeRadius=saccadeRadius_initial;
mbedalvaro 30:d8af03f01cd4 738 bluePrint.buildCircularScafold(saccadeRadius, vector2Dd(0,0), numPoints);
mbedalvaro 30:d8af03f01cd4 739 // reset to central position:
mbedalvaro 31:5f039cbddee8 740 centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 30:d8af03f01cd4 741 transientBlobColor=blobColor;
mbedalvaro 31:5f039cbddee8 742 }
mbedalvaro 28:44b7b6e35548 743 break;
mbedalvaro 31:5f039cbddee8 744
mbedalvaro 31:5f039cbddee8 745
mbedalvaro 31:5f039cbddee8 746 // ================================================================
mbedalvaro 31:5f039cbddee8 747 case SPOT_LORENTZ_FORCE:
mbedalvaro 28:44b7b6e35548 748 // this is very simple: we need to give a force to the centralMass that is OPPOSITE to the recenteringVectorLoop vector
mbedalvaro 28:44b7b6e35548 749 centerMass.resetForce();
mbedalvaro 28:44b7b6e35548 750
mbedalvaro 31:5f039cbddee8 751 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 752 // add force; MANY POSSIBILITIES:
mbedalvaro 31:5f039cbddee8 753 // (1) Constant in norm:
mbedalvaro 31:5f039cbddee8 754 //centerMass.addForce(unitTowardsLight*saccadeRadius*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 755 // Exactly what is needed to have an elastic bouncing:
mbedalvaro 31:5f039cbddee8 756
mbedalvaro 31:5f039cbddee8 757 // Proportional to the penetration depth in the dark zone (spring):
mbedalvaro 31:5f039cbddee8 758 centerMass.addForce(recenteringVectorLoop*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 759 // Or proportional to the square (or something else) of the penetration:
mbedalvaro 31:5f039cbddee8 760 //centerMass.addForce(recenteringVectorLoop*normRecenteringVector*factorBouncingForce);
mbedalvaro 31:5f039cbddee8 761
mbedalvaro 28:44b7b6e35548 762 }
mbedalvaro 31:5f039cbddee8 763
mbedalvaro 28:44b7b6e35548 764 // RADIAL GRAVITY for the "fountain mode":
mbedalvaro 31:5f039cbddee8 765 // vector2Df centerAttraction(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 766 // vector2Df radialVector=centerMass.pos-centerAttraction;
mbedalvaro 31:5f039cbddee8 767 // radialVector.rotateDeg(slidingDirection? 80 : 260);
mbedalvaro 31:5f039cbddee8 768 // centerMass.addForce(radialVector.normalize()*centerMass.mass*0.5);
mbedalvaro 31:5f039cbddee8 769
mbedalvaro 28:44b7b6e35548 770 // bubble chamber? LORENTZ FORCE:
mbedalvaro 28:44b7b6e35548 771 vector2Df speedmass=centerMass.getSpeed();
mbedalvaro 28:44b7b6e35548 772 centerMass.addForce( speedmass.getRotatedDeg(90)*0.000002*speedContourFollowing);
mbedalvaro 31:5f039cbddee8 773
mbedalvaro 28:44b7b6e35548 774 // update dynamics for the central mass:
mbedalvaro 28:44b7b6e35548 775 #ifndef VERLET_METHOD
mbedalvaro 28:44b7b6e35548 776 centerMass.addDampingForce(); // // only in case of EULER method (damping in VERLET mode is done automatically when updating)
mbedalvaro 28:44b7b6e35548 777 #endif
mbedalvaro 28:44b7b6e35548 778
mbedalvaro 28:44b7b6e35548 779 centerMass.update(); // unconstrained
mbedalvaro 28:44b7b6e35548 780 centerMass.bounceOffWalls(); // constrain position (and compute wall "hit")
mbedalvaro 31:5f039cbddee8 781
mbedalvaro 31:5f039cbddee8 782 if (displaySensingBuffer.lightTouched) {
mbedalvaro 31:5f039cbddee8 783 // do collision damping:
mbedalvaro 31:5f039cbddee8 784 centerMass.setSpeed(centerMass.getSpeed()*0.99);
mbedalvaro 31:5f039cbddee8 785 }
mbedalvaro 31:5f039cbddee8 786
mbedalvaro 28:44b7b6e35548 787 // Change color with touch? YES
mbedalvaro 31:5f039cbddee8 788 if (displaySensingBuffer.lightTouched)
mbedalvaro 31:5f039cbddee8 789 transientBlobColor=blobColor|0x02; // set green ON on the trajectory, regardless of the initial color
mbedalvaro 31:5f039cbddee8 790 else
mbedalvaro 31:5f039cbddee8 791 transientBlobColor=blobColor; // just the original blob color
mbedalvaro 31:5f039cbddee8 792
mbedalvaro 28:44b7b6e35548 793 // In case of "fountain mode", reset position to initial positions and speeds, or change gravity sign:
mbedalvaro 31:5f039cbddee8 794 // if (blobWallCollision) centerMass.setInitialCondition(startCenter, startSpeed);
mbedalvaro 31:5f039cbddee8 795 if (blobWallCollision) slidingDirection=!slidingDirection;
mbedalvaro 1:a4050fee11f7 796 break;
mbedalvaro 30:d8af03f01cd4 797 // ================================================================
mbedalvaro 1:a4050fee11f7 798 }
mbedalvaro 31:5f039cbddee8 799
mbedalvaro 1:a4050fee11f7 800 }
mbedalvaro 1:a4050fee11f7 801
mbedalvaro 1:a4050fee11f7 802
mbedalvaro 1:a4050fee11f7 803 // Drawing the graphics - this will in fact use the graphic renderer - if any - and produce the trajectory to be displayed by the laser
mbedalvaro 31:5f039cbddee8 804 void rigidLoop::draw()
mbedalvaro 31:5f039cbddee8 805 {
mbedalvaro 1:a4050fee11f7 806 // for the time being, there is no "opengl" like renderer, so we just copy into the lsdTrajectory:
mbedalvaro 1:a4050fee11f7 807 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 808 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 809 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 810 // The shape is drawn by translating the scafold shape (centered on centerMass):
mbedalvaro 12:0de9cd2bced5 811 displaySensingBuffer.lsdTrajectory[i].x= (unsigned short)(bluePrint.scafold[i].x + cx ); // note: it should be an unsigned short!!
mbedalvaro 12:0de9cd2bced5 812 displaySensingBuffer.lsdTrajectory[i].y= (unsigned short)(bluePrint.scafold[i].y + cy );
mbedalvaro 31:5f039cbddee8 813
mbedalvaro 12:0de9cd2bced5 814 // We can also do this, but ATTENTION: centerMass.pos is a vector2Df, and scafold[i] is a vector2Dd (typecasting?)
mbedalvaro 12:0de9cd2bced5 815 // displaySensingBuffer.lsdTrajectory[i]= bluePrint.scafold[i] + centerMass.pos;
mbedalvaro 31:5f039cbddee8 816
mbedalvaro 25:74cb85b85fd2 817 //displaySensingBuffer.displayColor=blobColor; // perhaps per point color is not a good idea for the time being...
mbedalvaro 1:a4050fee11f7 818 }
mbedalvaro 31:5f039cbddee8 819
mbedalvaro 25:74cb85b85fd2 820 // Global color for the whole loop:
mbedalvaro 27:1ce994629ffc 821 displaySensingBuffer.displayColor=transientBlobColor;
mbedalvaro 31:5f039cbddee8 822
mbedalvaro 1:a4050fee11f7 823 }
mbedalvaro 1:a4050fee11f7 824
mbedalvaro 31:5f039cbddee8 825 void rigidLoop::computeBoundingBox()
mbedalvaro 31:5f039cbddee8 826 {
mbedalvaro 1:a4050fee11f7 827 }
mbedalvaro 1:a4050fee11f7 828
mbedalvaro 1:a4050fee11f7 829
mbedalvaro 1:a4050fee11f7 830
mbedalvaro 31:5f039cbddee8 831 void rigidLoop::sendDataSpecific()
mbedalvaro 31:5f039cbddee8 832 {
mbedalvaro 1:a4050fee11f7 833 char auxstring[10];
mbedalvaro 1:a4050fee11f7 834 myled2=1; // for tests...
mbedalvaro 1:a4050fee11f7 835
mbedalvaro 1:a4050fee11f7 836 // First, set the top address of the message to the ID of the blob (not the name):
mbedalvaro 31:5f039cbddee8 837 // sprintf(auxstring, "%d", identifier);
mbedalvaro 31:5f039cbddee8 838 // sendMes.setTopAddress("0");//auxstring);
mbedalvaro 1:a4050fee11f7 839
mbedalvaro 1:a4050fee11f7 840 // ===================== OSC ======================
mbedalvaro 1:a4050fee11f7 841 if (sendOSC) {
mbedalvaro 1:a4050fee11f7 842
mbedalvaro 1:a4050fee11f7 843 // (a) Anchor mass:
mbedalvaro 1:a4050fee11f7 844 if (sendingAnchorPosition) {
mbedalvaro 31:5f039cbddee8 845 sprintf(auxstring, "/p %d",identifier);
mbedalvaro 18:d72935b13858 846 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 847 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 848 x=(long)(centerMass.pos.x);
mbedalvaro 1:a4050fee11f7 849 y=(long)(centerMass.pos.y);
mbedalvaro 1:a4050fee11f7 850 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 851 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 852 }
mbedalvaro 1:a4050fee11f7 853
mbedalvaro 1:a4050fee11f7 854 // (b) data from blob points (this is ONLY FOR TESTS, because the loop is rigid - sending the center is enough)
mbedalvaro 1:a4050fee11f7 855 if (sendingLoopPositions) {
mbedalvaro 1:a4050fee11f7 856 #ifdef SEND_AS_POINTS
mbedalvaro 1:a4050fee11f7 857 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 858 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 859 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 860 for (int i = 0; i < numPoints; i++) {
mbedalvaro 2:34157ebbf56b 861 sprintf(auxstring, "/p%d",identifier*10+ i);//20+ i+(identifier-1)*10); // auxstring read as "/p1", "/p2", ...
mbedalvaro 1:a4050fee11f7 862 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...)
mbedalvaro 1:a4050fee11f7 863 x=(long)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 864 y=(long)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 865 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 866 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 867 }
mbedalvaro 1:a4050fee11f7 868
mbedalvaro 1:a4050fee11f7 869 #endif
mbedalvaro 1:a4050fee11f7 870 #ifdef SEND_AS_BLOB
mbedalvaro 1:a4050fee11f7 871 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 872 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 873 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 874 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 875 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 876 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 877 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx);
mbedalvaro 1:a4050fee11f7 878 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 879 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 880
mbedalvaro 1:a4050fee11f7 881 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 882 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 883 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 884 }
mbedalvaro 1:a4050fee11f7 885 osc.sendOscBlob(&(blobdata[0]), 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 886 #endif
mbedalvaro 1:a4050fee11f7 887 #ifdef SEND_AS_STRING
mbedalvaro 1:a4050fee11f7 888 sendMes.clearArgs(); // no need, we won't use osc.sendOsc()...
mbedalvaro 1:a4050fee11f7 889 uint8_t blobdata[4*numPoints]; // 2 bytes per coordinate, and 2 coordinates
mbedalvaro 1:a4050fee11f7 890 float cx= centerMass.pos.x;
mbedalvaro 1:a4050fee11f7 891 float cy= centerMass.pos.y;
mbedalvaro 1:a4050fee11f7 892 for (int i = 0; i < numPoints; i++ ) {
mbedalvaro 1:a4050fee11f7 893 // note: massesLoop[i].pos.x is a "float"
mbedalvaro 1:a4050fee11f7 894 uint16_t x=(uint16_t)(bluePrint.scafold[i].x + cx );
mbedalvaro 1:a4050fee11f7 895 blobdata[4*i]=(uint8_t)x>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 896 blobdata[4*i+1]=(uint8_t)x;
mbedalvaro 1:a4050fee11f7 897
mbedalvaro 1:a4050fee11f7 898 uint16_t y=(uint16_t)(bluePrint.scafold[i].y + cy);
mbedalvaro 1:a4050fee11f7 899 blobdata[4*i+2]=(uint8_t)y>>8; // BIG ENDIAN (send FIRST the MOST SIGNIFICANT BYTE)
mbedalvaro 1:a4050fee11f7 900 blobdata[4*i+3]=(uint8_t)y;
mbedalvaro 1:a4050fee11f7 901 }
mbedalvaro 1:a4050fee11f7 902 osc.sendOscString(blobdata, 4*numPoints, &sendMes ); // second parameter is osc blob size in bytes
mbedalvaro 1:a4050fee11f7 903 #endif
mbedalvaro 1:a4050fee11f7 904 }
mbedalvaro 1:a4050fee11f7 905 if (sendingLoopRegions) {
mbedalvaro 1:a4050fee11f7 906 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 907 for (int i = 0; i < numPoints; i++) {
mbedalvaro 1:a4050fee11f7 908 sprintf(auxstring, "/r%d", i); // auxstring read as "/f1", "/f2", ...
mbedalvaro 1:a4050fee11f7 909 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...)
mbedalvaro 1:a4050fee11f7 910 x=(long)(displaySensingBuffer.lsdTrajectory[i].lightZone>0? 1 : 0);
mbedalvaro 1:a4050fee11f7 911 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 912 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 913 }
mbedalvaro 1:a4050fee11f7 914 }
mbedalvaro 1:a4050fee11f7 915 if (sendingLoopTouchWall) { // global touch wall for the loop (not per point)
mbedalvaro 1:a4050fee11f7 916 long wall; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 917 sprintf(auxstring, "/bWall");
mbedalvaro 1:a4050fee11f7 918 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 919 wall=(long)(blobWallCollision? 1 : 0);
mbedalvaro 1:a4050fee11f7 920 sendMes.setArgs( "i", &wall);
mbedalvaro 1:a4050fee11f7 921 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 922 }
mbedalvaro 1:a4050fee11f7 923
mbedalvaro 1:a4050fee11f7 924 // (d) Light sensing statistics:
mbedalvaro 1:a4050fee11f7 925 if (sendingBlobMaxMin) {
mbedalvaro 1:a4050fee11f7 926 sendMes.setSubAddress("/maxmin");
mbedalvaro 1:a4050fee11f7 927 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 928 x=(long)(displaySensingBuffer.maxI);
mbedalvaro 1:a4050fee11f7 929 y=(long)(displaySensingBuffer.minI);
mbedalvaro 1:a4050fee11f7 930 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 931 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 932 }
mbedalvaro 1:a4050fee11f7 933
mbedalvaro 1:a4050fee11f7 934 // (e) Recentering vector: (note: redundant with sendingLightForce, IF the correction angle is known).
mbedalvaro 1:a4050fee11f7 935 if (sendingRecenteringVector) {
mbedalvaro 1:a4050fee11f7 936 sendMes.setSubAddress("/rvector");
mbedalvaro 1:a4050fee11f7 937 long x, y; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 938 x=(long)(recenteringVectorLoop.x);
mbedalvaro 1:a4050fee11f7 939 y=(long)(recenteringVectorLoop.y);
mbedalvaro 1:a4050fee11f7 940 sendMes.setArgs( "ii", &x, &y);
mbedalvaro 1:a4050fee11f7 941 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 942 }
mbedalvaro 1:a4050fee11f7 943 if (sendingRecenteringAngle) {
mbedalvaro 18:d72935b13858 944 sprintf(auxstring, "/v %d",identifier);
mbedalvaro 18:d72935b13858 945 sendMes.setSubAddress(auxstring);
mbedalvaro 1:a4050fee11f7 946 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 947 x=(long)(angleRecenteringVector);
mbedalvaro 1:a4050fee11f7 948 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 949 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 950 }
mbedalvaro 1:a4050fee11f7 951 if (sendingRecenteringNorm) {
mbedalvaro 1:a4050fee11f7 952 sendMes.setSubAddress("/rnorm");
mbedalvaro 1:a4050fee11f7 953 long x; //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
mbedalvaro 1:a4050fee11f7 954 x=(long)(normRecenteringVector);
mbedalvaro 1:a4050fee11f7 955 sendMes.setArgs( "i", &x);
mbedalvaro 1:a4050fee11f7 956 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 957 }
mbedalvaro 1:a4050fee11f7 958
mbedalvaro 1:a4050fee11f7 959 if (sendingTouched) {
mbedalvaro 1:a4050fee11f7 960 if (displaySensingBuffer.lightTouched) {
mbedalvaro 1:a4050fee11f7 961 sendMes.clearArgs(); // there are no arguments to send
mbedalvaro 1:a4050fee11f7 962 sendMes.setSubAddress("/touched");
mbedalvaro 1:a4050fee11f7 963 osc.sendOsc( &sendMes );
mbedalvaro 1:a4050fee11f7 964 }
mbedalvaro 1:a4050fee11f7 965 }
mbedalvaro 1:a4050fee11f7 966
mbedalvaro 1:a4050fee11f7 967 } // end of OSC sending per-spot
mbedalvaro 1:a4050fee11f7 968
mbedalvaro 1:a4050fee11f7 969 // ===================== SERIAL ======================
mbedalvaro 1:a4050fee11f7 970 if (sendSerial) {
mbedalvaro 1:a4050fee11f7 971 //.. to do
mbedalvaro 1:a4050fee11f7 972 }
mbedalvaro 1:a4050fee11f7 973
mbedalvaro 1:a4050fee11f7 974 myled2=0; // for tests...
mbedalvaro 1:a4050fee11f7 975 }