Alvaro Cassinelli / Mbed 2 deprecated skinGames_forktest

just a test

Dependencies:   mbed

Fork of scoreLight_Advanced by Alvaro Cassinelli

classPointMass.cpp@12:0de9cd2bced5, 2012-04-12 (annotated)

Committer:
mbedalvaro
Date:
Thu Apr 12 08:38:44 2012 +0000
Revision:
12:0de9cd2bced5
Parent:
5:73cd58b58f95
Child:
24:4e52031a495b
1) template class vector works fine. This way, I have more memory (by defining a rigid scafold using unsigned shorts). I can now make an elastic blob of at least 50 points. ; ; To do: double buffering (this will again take memory, but it may be ok becaus...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 0:345b3bc7a0ea 1 /*
mbedalvaro 0:345b3bc7a0ea 2 * pointMass.cpp
mbedalvaro 0:345b3bc7a0ea 3 * laserBlob
mbedalvaro 0:345b3bc7a0ea 4 *
mbedalvaro 0:345b3bc7a0ea 5 * Created by CASSINELLI ALVARO on 5/19/11.
mbedalvaro 0:345b3bc7a0ea 6 * Copyright 2011 TOKYO UNIVERSITY. All rights reserved.
mbedalvaro 0:345b3bc7a0ea 7 *
mbedalvaro 0:345b3bc7a0ea 8 */
mbedalvaro 0:345b3bc7a0ea 9
mbedalvaro 0:345b3bc7a0ea 10 #include "classPointMass.h"
mbedalvaro 0:345b3bc7a0ea 11
mbedalvaro 12:0de9cd2bced5 12 // Initialization of static member variables:
mbedalvaro 12:0de9cd2bced5 13 vector2Df pointMass::maxWall(4095, 4095);
mbedalvaro 12:0de9cd2bced5 14 vector2Df pointMass::minWall(0, 0);
mbedalvaro 12:0de9cd2bced5 15
mbedalvaro 0:345b3bc7a0ea 16 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 17 pointMass::pointMass(){
mbedalvaro 0:345b3bc7a0ea 18 setIntegrationStep(.01); // default in case we don't call integration step setting
mbedalvaro 0:345b3bc7a0ea 19 // NOTE: it is important to set dt before inital conditions in case of VERLET integration, because we need the integration
mbedalvaro 0:345b3bc7a0ea 20 // step for properly setting the initial speed.
mbedalvaro 0:345b3bc7a0ea 21 setInitialCondition(0,0,0,0);// default in case we don't call to initial conditions.
mbedalvaro 0:345b3bc7a0ea 22 setWallLimits(100,100,4000,4000);
mbedalvaro 0:345b3bc7a0ea 23 mass=1.0;
mbedalvaro 0:345b3bc7a0ea 24 dampMotion = 0.0175;//0.025 was good for bigger blobs;//2f;//0.07f;
mbedalvaro 0:345b3bc7a0ea 25 dampBorder = 0.013f; //0.07f
mbedalvaro 0:345b3bc7a0ea 26 bFixed = false;
mbedalvaro 0:345b3bc7a0ea 27 bWallCollision=false;
mbedalvaro 0:345b3bc7a0ea 28 }
mbedalvaro 0:345b3bc7a0ea 29
mbedalvaro 0:345b3bc7a0ea 30 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 31 void pointMass::resetForce(){
mbedalvaro 0:345b3bc7a0ea 32 totalForce.set(0,0);
mbedalvaro 0:345b3bc7a0ea 33 }
mbedalvaro 0:345b3bc7a0ea 34
mbedalvaro 0:345b3bc7a0ea 35 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 36 void pointMass::addForce(float x, float y){
mbedalvaro 0:345b3bc7a0ea 37 totalForce.x = totalForce.x + x;
mbedalvaro 0:345b3bc7a0ea 38 totalForce.y = totalForce.y + y;
mbedalvaro 0:345b3bc7a0ea 39 }
mbedalvaro 0:345b3bc7a0ea 40
mbedalvaro 0:345b3bc7a0ea 41 //------------------------------------------------------------
mbedalvaro 12:0de9cd2bced5 42 void pointMass::addForce(vector2Df forceToAdd){
mbedalvaro 0:345b3bc7a0ea 43 totalForce+=forceToAdd;
mbedalvaro 0:345b3bc7a0ea 44 }
mbedalvaro 0:345b3bc7a0ea 45
mbedalvaro 0:345b3bc7a0ea 46 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 47 void pointMass::addInvSquareForce(float x, float y, float radiusMin, float radiusMax, float scale){
mbedalvaro 0:345b3bc7a0ea 48
mbedalvaro 12:0de9cd2bced5 49 vector2Df posOfForce;
mbedalvaro 0:345b3bc7a0ea 50 posOfForce.set(x,y);
mbedalvaro 0:345b3bc7a0ea 51
mbedalvaro 12:0de9cd2bced5 52 vector2Df diff = pos - posOfForce; // note: we use the position AT TIME T, so this force is at time t
mbedalvaro 0:345b3bc7a0ea 53 float length = diff.length();
mbedalvaro 0:345b3bc7a0ea 54
mbedalvaro 0:345b3bc7a0ea 55 // check close enough and far enough (to avoid singularities for example):
mbedalvaro 0:345b3bc7a0ea 56 if ((length>radiusMin)&&(length<radiusMax)) {
mbedalvaro 0:345b3bc7a0ea 57 diff.normalize();
mbedalvaro 0:345b3bc7a0ea 58 totalForce += diff * scale * 1.0/(length*length+1);
mbedalvaro 0:345b3bc7a0ea 59 }
mbedalvaro 0:345b3bc7a0ea 60 }
mbedalvaro 0:345b3bc7a0ea 61
mbedalvaro 0:345b3bc7a0ea 62 void pointMass::addInterInvSquareForce(pointMass &theOtherParticle, float radiusMin, float radiusMax, float scale){
mbedalvaro 0:345b3bc7a0ea 63
mbedalvaro 12:0de9cd2bced5 64 vector2Df posOfForce;
mbedalvaro 0:345b3bc7a0ea 65 posOfForce.set(theOtherParticle.pos);
mbedalvaro 0:345b3bc7a0ea 66
mbedalvaro 12:0de9cd2bced5 67 vector2Df diff = pos - posOfForce; // note: we use the position AT TIME T, so this force is at time t
mbedalvaro 0:345b3bc7a0ea 68 float length = diff.length();
mbedalvaro 0:345b3bc7a0ea 69
mbedalvaro 0:345b3bc7a0ea 70 // check close enough and far enough (to avoid singularities for example):
mbedalvaro 0:345b3bc7a0ea 71 if ((length>radiusMin)&&(length<radiusMax)) {
mbedalvaro 0:345b3bc7a0ea 72 diff.normalize();
mbedalvaro 0:345b3bc7a0ea 73 totalForce += diff * scale * 1.0/(length*length+1);
mbedalvaro 0:345b3bc7a0ea 74 theOtherParticle.totalForce -= diff * scale * 1.0/(length*length+1);
mbedalvaro 0:345b3bc7a0ea 75 }
mbedalvaro 0:345b3bc7a0ea 76 }
mbedalvaro 0:345b3bc7a0ea 77
mbedalvaro 0:345b3bc7a0ea 78
mbedalvaro 0:345b3bc7a0ea 79 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 80 void pointMass::addSpringForce(float centerx, float centery, float radius, float scale){
mbedalvaro 0:345b3bc7a0ea 81
mbedalvaro 0:345b3bc7a0ea 82 // ----------- (1) make a vector of where this particle p is:
mbedalvaro 12:0de9cd2bced5 83 vector2Df posOfForce;
mbedalvaro 0:345b3bc7a0ea 84 posOfForce.set(centerx, centery);
mbedalvaro 0:345b3bc7a0ea 85
mbedalvaro 0:345b3bc7a0ea 86 // ----------- (2) calculate the difference & length
mbedalvaro 0:345b3bc7a0ea 87
mbedalvaro 12:0de9cd2bced5 88 vector2Df diff = pos - posOfForce;
mbedalvaro 0:345b3bc7a0ea 89 float length = diff.length();
mbedalvaro 0:345b3bc7a0ea 90
mbedalvaro 0:345b3bc7a0ea 91 // ----------- (3) check close enough
mbedalvaro 0:345b3bc7a0ea 92
mbedalvaro 0:345b3bc7a0ea 93 bool bAmCloseEnough = true;
mbedalvaro 0:345b3bc7a0ea 94 if (radius > 0){
mbedalvaro 0:345b3bc7a0ea 95 if (length > radius){
mbedalvaro 0:345b3bc7a0ea 96 bAmCloseEnough = false;
mbedalvaro 0:345b3bc7a0ea 97 }
mbedalvaro 0:345b3bc7a0ea 98 }
mbedalvaro 0:345b3bc7a0ea 99
mbedalvaro 0:345b3bc7a0ea 100 // ----------- (4) if so, update force
mbedalvaro 0:345b3bc7a0ea 101
mbedalvaro 0:345b3bc7a0ea 102 if (bAmCloseEnough == true){
mbedalvaro 0:345b3bc7a0ea 103 float pct = 1 - (length / radius); // stronger on the inside
mbedalvaro 0:345b3bc7a0ea 104 diff.normalize();
mbedalvaro 0:345b3bc7a0ea 105 totalForce += diff * scale * pct;
mbedalvaro 0:345b3bc7a0ea 106 }
mbedalvaro 0:345b3bc7a0ea 107 }
mbedalvaro 0:345b3bc7a0ea 108
mbedalvaro 0:345b3bc7a0ea 109 void pointMass::addInterSpringForce(pointMass &theOtherParticle, float radius, float scale){
mbedalvaro 0:345b3bc7a0ea 110
mbedalvaro 0:345b3bc7a0ea 111 // ----------- (1) make a vector of where this particle p is:
mbedalvaro 12:0de9cd2bced5 112 vector2Df posOfForce;
mbedalvaro 0:345b3bc7a0ea 113 posOfForce.set(theOtherParticle.pos);
mbedalvaro 0:345b3bc7a0ea 114
mbedalvaro 0:345b3bc7a0ea 115 // ----------- (2) calculate the difference & length
mbedalvaro 0:345b3bc7a0ea 116
mbedalvaro 12:0de9cd2bced5 117 vector2Df diff = pos - posOfForce;
mbedalvaro 0:345b3bc7a0ea 118 float length = diff.length();
mbedalvaro 0:345b3bc7a0ea 119
mbedalvaro 0:345b3bc7a0ea 120 // ----------- (3) check close enough
mbedalvaro 0:345b3bc7a0ea 121
mbedalvaro 0:345b3bc7a0ea 122 bool bAmCloseEnough = true;
mbedalvaro 0:345b3bc7a0ea 123 if (radius > 0){
mbedalvaro 0:345b3bc7a0ea 124 if (length > radius){
mbedalvaro 0:345b3bc7a0ea 125 bAmCloseEnough = false;
mbedalvaro 0:345b3bc7a0ea 126 }
mbedalvaro 0:345b3bc7a0ea 127 }
mbedalvaro 0:345b3bc7a0ea 128
mbedalvaro 0:345b3bc7a0ea 129 // ----------- (4) if so, update REPULSIVE force
mbedalvaro 0:345b3bc7a0ea 130
mbedalvaro 0:345b3bc7a0ea 131 if (bAmCloseEnough == true){
mbedalvaro 0:345b3bc7a0ea 132 float pct = 1 - (length / radius);
mbedalvaro 0:345b3bc7a0ea 133 diff.normalize();
mbedalvaro 0:345b3bc7a0ea 134 totalForce += diff * scale * pct;
mbedalvaro 0:345b3bc7a0ea 135 theOtherParticle.totalForce -= diff * scale * pct;
mbedalvaro 0:345b3bc7a0ea 136 //theOtherParticle.frc.x = p.frc.x - diff.x * scale * pct;
mbedalvaro 0:345b3bc7a0ea 137 //theOtherParticle.frc.y = p.frc.y - diff.y * scale * pct;
mbedalvaro 0:345b3bc7a0ea 138 }
mbedalvaro 0:345b3bc7a0ea 139 }
mbedalvaro 0:345b3bc7a0ea 140
mbedalvaro 0:345b3bc7a0ea 141
mbedalvaro 0:345b3bc7a0ea 142
mbedalvaro 0:345b3bc7a0ea 143 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 144 void pointMass::addDampingForce(){ // NOTE: use only in case of EULER intgration!
mbedalvaro 0:345b3bc7a0ea 145 totalForce-= speed* dampMotion;
mbedalvaro 0:345b3bc7a0ea 146 }
mbedalvaro 0:345b3bc7a0ea 147
mbedalvaro 0:345b3bc7a0ea 148 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 149 void pointMass::setIntegrationStep(float _dt){
mbedalvaro 0:345b3bc7a0ea 150 dt=_dt;
mbedalvaro 0:345b3bc7a0ea 151 }
mbedalvaro 0:345b3bc7a0ea 152
mbedalvaro 0:345b3bc7a0ea 153 //------------------------------------------------------------
mbedalvaro 12:0de9cd2bced5 154 void pointMass::setInitialCondition(vector2Df _pos, vector2Df _speed) {
mbedalvaro 4:f9d364f10335 155 setInitialCondition(_pos.x, _pos.y, _speed.x, _speed.y);
mbedalvaro 4:f9d364f10335 156 }
mbedalvaro 0:345b3bc7a0ea 157 void pointMass::setInitialCondition(float px, float py, float vx, float vy){
mbedalvaro 0:345b3bc7a0ea 158 #ifndef VERLET_METHOD
mbedalvaro 0:345b3bc7a0ea 159 pos.set(px,py);
mbedalvaro 0:345b3bc7a0ea 160 speed.set(vx,vy);
mbedalvaro 0:345b3bc7a0ea 161 #else
mbedalvaro 0:345b3bc7a0ea 162 // In case of Verlet method, setting the speed is a little more complicated. It involves in particular the integration step
mbedalvaro 0:345b3bc7a0ea 163 // through the approximation formula:
mbedalvaro 0:345b3bc7a0ea 164 // speed = (posNew-posOld)/(2*dt), or speed=(pos-posOld)/dt. Hence:
mbedalvaro 0:345b3bc7a0ea 165 posOld.set(px, py);
mbedalvaro 0:345b3bc7a0ea 166 setSpeed(vx, vy); // this assumes posOld known
mbedalvaro 0:345b3bc7a0ea 167 #endif
mbedalvaro 0:345b3bc7a0ea 168 }
mbedalvaro 0:345b3bc7a0ea 169
mbedalvaro 0:345b3bc7a0ea 170 //-------------------------------------------------------
mbedalvaro 12:0de9cd2bced5 171 vector2Df pointMass::getSpeed() {
mbedalvaro 0:345b3bc7a0ea 172 // this will give an estimate of the speed (not computed explicitly using the Verlet integration):
mbedalvaro 0:345b3bc7a0ea 173 //speed=(posNew-posOld)/(2*dt); // the variable speed is also updated (note: it is private)
mbedalvaro 1:a4050fee11f7 174 speed=(pos-posOld)/dt; // less approximate than the above, but we avoid having a global posNew variable (remember we will have many particles...)
mbedalvaro 0:345b3bc7a0ea 175 return(speed);
mbedalvaro 0:345b3bc7a0ea 176 }
mbedalvaro 0:345b3bc7a0ea 177
mbedalvaro 12:0de9cd2bced5 178 void pointMass::setSpeed(const vector2Df& vel) { // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
mbedalvaro 0:345b3bc7a0ea 179 speed.set(vel); // enough for EULER METHOD
mbedalvaro 2:34157ebbf56b 180
mbedalvaro 2:34157ebbf56b 181 // NECESSARY for VERLET METHOD (we assume posOld known):
mbedalvaro 2:34157ebbf56b 182 // pos=speed*dt+posOld; // when dampMotion=0 (no damping)
mbedalvaro 2:34157ebbf56b 183 // With damping:
mbedalvaro 2:34157ebbf56b 184 // we have: speed=(posNew-posOld)/(2*dt) and posNew=pos*(2.0-dampMotion)-posOld*(1.0-dampMotion), so:
mbedalvaro 2:34157ebbf56b 185 // pos=(speed*2*dt+posOld+posOld*(1.0-dampMotion))/(2.0-dampMotion);
mbedalvaro 2:34157ebbf56b 186 pos=speed*2*dt/(2.0-dampMotion)+posOld;
mbedalvaro 2:34157ebbf56b 187
mbedalvaro 0:345b3bc7a0ea 188 // no need to compute newPos
mbedalvaro 0:345b3bc7a0ea 189 }
mbedalvaro 0:345b3bc7a0ea 190
mbedalvaro 1:a4050fee11f7 191 void pointMass::setSpeed(float vx, float vy) { // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed.
mbedalvaro 0:345b3bc7a0ea 192 speed.set(vx, vy); // enough for EULER METHOD
mbedalvaro 2:34157ebbf56b 193
mbedalvaro 2:34157ebbf56b 194 // NECESSARY for VERLET METHOD (we assume posOld known):
mbedalvaro 2:34157ebbf56b 195 // pos=speed*dt+posOld; // when dampMotion=0 (no damping)
mbedalvaro 2:34157ebbf56b 196 // With damping:
mbedalvaro 2:34157ebbf56b 197 // we have: speed=(posNew-posOld)/(2*dt) and posNew=pos*(2.0-dampMotion)-posOld*(1.0-dampMotion), so:
mbedalvaro 2:34157ebbf56b 198 // pos=(speed*2*dt+posOld+posOld*(1.0-dampMotion))/(2.0-dampMotion);
mbedalvaro 2:34157ebbf56b 199 pos=speed*2*dt/(2.0-dampMotion)+posOld;
mbedalvaro 2:34157ebbf56b 200
mbedalvaro 0:345b3bc7a0ea 201 // no need to compute newPos
mbedalvaro 0:345b3bc7a0ea 202 }
mbedalvaro 0:345b3bc7a0ea 203
mbedalvaro 0:345b3bc7a0ea 204 void pointMass::setPos(float px, float py) { // assuming the speed is unchanged (must do some tweaking in case of Verlet integration)
mbedalvaro 0:345b3bc7a0ea 205 pos.set(px, py);
mbedalvaro 2:34157ebbf56b 206 posOld=pos-speed*dt;// no damping! attn...
mbedalvaro 0:345b3bc7a0ea 207 }
mbedalvaro 0:345b3bc7a0ea 208
mbedalvaro 0:345b3bc7a0ea 209 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 210 void pointMass::update(){
mbedalvaro 0:345b3bc7a0ea 211 if (bFixed == false){
mbedalvaro 0:345b3bc7a0ea 212 acc=totalForce/mass; // this is the acceleration at time t
mbedalvaro 1:a4050fee11f7 213
mbedalvaro 0:345b3bc7a0ea 214 #ifndef VERLET_METHOD
mbedalvaro 0:345b3bc7a0ea 215 // The following equations (Euler integration) assume acceleration constant during time dt:
mbedalvaro 0:345b3bc7a0ea 216 speed = speed + acc*dt;
mbedalvaro 0:345b3bc7a0ea 217 pos = pos + speed*dt ;//+acc*dt*dt*0.5;
mbedalvaro 0:345b3bc7a0ea 218 #else
mbedalvaro 3:b44ff6de81bd 219 // acc=0;//
mbedalvaro 0:345b3bc7a0ea 220 // The following equations are for VERLET integration with pseudo-damping:
mbedalvaro 2:34157ebbf56b 221 //Without damping this is just:
mbedalvaro 12:0de9cd2bced5 222 //vector2Df posNew=posOld*2 - pos + acc*dt*dt; // i.e., dampMotion=0;
mbedalvaro 2:34157ebbf56b 223 // With damping:
mbedalvaro 12:0de9cd2bced5 224 // vector2Df posNew=(pos*(2.0-dampMotion)-posOld*(1.0-dampMotion)+acc*dt*dt); // BAD!!! THIS NOTATION will introduce precision artefacts!!!
mbedalvaro 12:0de9cd2bced5 225 vector2Df posNew=pos+(pos-posOld)*(1-dampMotion)+acc*dt*dt;
mbedalvaro 0:345b3bc7a0ea 226
mbedalvaro 2:34157ebbf56b 227 // ATTENTION: because of the precision of the float or double used, it may be that (pos - posNew) is not 0, when it should ( this produces VERY strange motion artefacts).
mbedalvaro 2:34157ebbf56b 228 // So, I will test if that difference is smaller than an arbitrary tollerance in this numerical implementation (for each coordinate), and if so, will FORCE posNew to be equal to pos.
mbedalvaro 2:34157ebbf56b 229 // Note: no need to compute the norm of the difference (in fact, we need the abs difference for each component of the difference vector). Fortunately, nothing is needed here, because we can put the
mbedalvaro 2:34157ebbf56b 230 // expression in a better way that automatically makes 0 the contribution of something smaller than the precision (so the following is not really necessary)
mbedalvaro 12:0de9cd2bced5 231 // vector2Df diff=(pos-posOld)*(1-dampMotion);
mbedalvaro 2:34157ebbf56b 232 // Precision correction (separate axis or not):
mbedalvaro 2:34157ebbf56b 233 // if (abs(diff.x)<0.00001) diff.x=diff.x;
mbedalvaro 2:34157ebbf56b 234 // if (abs(diff.y)<0.00001) diff.y=diff.y;
mbedalvaro 2:34157ebbf56b 235 // if (posOld.match(pos, 0.001)==true) {posNew=pos; speed.set(0,0);}
mbedalvaro 12:0de9cd2bced5 236 // vector2Df posNew=pos+diff+acc*dt*dt;
mbedalvaro 2:34157ebbf56b 237
mbedalvaro 0:345b3bc7a0ea 238 posOld=pos;
mbedalvaro 0:345b3bc7a0ea 239 pos=posNew;
mbedalvaro 0:345b3bc7a0ea 240
mbedalvaro 0:345b3bc7a0ea 241 // NOTE: we can also estimate the speed if we want. But this may be unnecessary (call getSpeed() for that).
mbedalvaro 0:345b3bc7a0ea 242
mbedalvaro 0:345b3bc7a0ea 243 #endif
mbedalvaro 0:345b3bc7a0ea 244
mbedalvaro 5:73cd58b58f95 245 // Constrain speed to max speed (in norm):
mbedalvaro 5:73cd58b58f95 246 float normSpeed=getSpeed().length();
mbedalvaro 5:73cd58b58f95 247 if (normSpeed>100) {
mbedalvaro 5:73cd58b58f95 248 setSpeed(getSpeed()*100.0/normSpeed);
mbedalvaro 5:73cd58b58f95 249 }
mbedalvaro 5:73cd58b58f95 250
mbedalvaro 0:345b3bc7a0ea 251 }
mbedalvaro 0:345b3bc7a0ea 252 }
mbedalvaro 0:345b3bc7a0ea 253
mbedalvaro 0:345b3bc7a0ea 254 void pointMass::setWallLimits(float Minx, float Miny, float Maxx, float Maxy) {
mbedalvaro 0:345b3bc7a0ea 255 maxWall.set(Maxx, Maxy);
mbedalvaro 0:345b3bc7a0ea 256 minWall.set(Minx, Miny);
mbedalvaro 0:345b3bc7a0ea 257 }
mbedalvaro 0:345b3bc7a0ea 258
mbedalvaro 0:345b3bc7a0ea 259 //------------------------------------------------------------
mbedalvaro 0:345b3bc7a0ea 260 void pointMass::bounceOffWalls(){
mbedalvaro 0:345b3bc7a0ea 261 // NOTE: bounce is easy in case of EULER method; in case of VERLET, we need to do some hack on the positions.
mbedalvaro 12:0de9cd2bced5 262 //Note: the walls are in (vector2Dd) horizontalLimits and verticalLimits
mbedalvaro 0:345b3bc7a0ea 263
mbedalvaro 0:345b3bc7a0ea 264 bWallCollision=false;
mbedalvaro 0:345b3bc7a0ea 265 innerCollitionDirection.set(0,0);
mbedalvaro 0:345b3bc7a0ea 266 #ifndef VERLET_METHOD // EULER METHOD!!
mbedalvaro 0:345b3bc7a0ea 267
mbedalvaro 0:345b3bc7a0ea 268 if (pos.x > maxWall.x){
mbedalvaro 0:345b3bc7a0ea 269 pos.x = maxWall.x;
mbedalvaro 0:345b3bc7a0ea 270 speed.x *= -1;
mbedalvaro 0:345b3bc7a0ea 271 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 272 innerCollitionDirection.x=-1;
mbedalvaro 0:345b3bc7a0ea 273 } else if (pos.x < minWall.x){
mbedalvaro 0:345b3bc7a0ea 274 pos.x = minWall.x;
mbedalvaro 0:345b3bc7a0ea 275 speed.x *= -1;
mbedalvaro 0:345b3bc7a0ea 276 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 277 innerCollitionDirection.x=1;
mbedalvaro 0:345b3bc7a0ea 278 }
mbedalvaro 0:345b3bc7a0ea 279
mbedalvaro 0:345b3bc7a0ea 280 if (pos.y > maxWall.y){
mbedalvaro 0:345b3bc7a0ea 281 pos.y = maxWall.y;
mbedalvaro 0:345b3bc7a0ea 282 speed.y *= -1;
mbedalvaro 0:345b3bc7a0ea 283 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 284 innerCollitionDirection.y=-1;
mbedalvaro 0:345b3bc7a0ea 285 } else if (pos.y < minWall.y){
mbedalvaro 0:345b3bc7a0ea 286 pos.y = minWall.y;
mbedalvaro 0:345b3bc7a0ea 287 speed.y *= -1;
mbedalvaro 0:345b3bc7a0ea 288 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 289 innerCollitionDirection.y=1;
mbedalvaro 0:345b3bc7a0ea 290 }
mbedalvaro 0:345b3bc7a0ea 291
mbedalvaro 0:345b3bc7a0ea 292 if (bWallCollision) {
mbedalvaro 0:345b3bc7a0ea 293 // damping:
mbedalvaro 0:345b3bc7a0ea 294 speed *=(1-dampBorder);
mbedalvaro 0:345b3bc7a0ea 295 // normalization of collision direction:
mbedalvaro 0:345b3bc7a0ea 296 innerCollitionDirection.normalize();
mbedalvaro 0:345b3bc7a0ea 297 }
mbedalvaro 0:345b3bc7a0ea 298
mbedalvaro 0:345b3bc7a0ea 299 #else // THIS IS FOR VERLET METHOD:
mbedalvaro 0:345b3bc7a0ea 300 // we need to estimate the inverted, damped vector for bumping::
mbedalvaro 12:0de9cd2bced5 301 vector2Df bumpVector=getSpeed()*dt*(dampBorder-1.0); // assuming dampBorder<1 of course
mbedalvaro 0:345b3bc7a0ea 302 if (pos.x > maxWall.x){
mbedalvaro 0:345b3bc7a0ea 303 //posOld.x=pos.x;
mbedalvaro 0:345b3bc7a0ea 304 //pos.x=pos.x+bumpVector.x;
mbedalvaro 0:345b3bc7a0ea 305 posOld.x=maxWall.x;
mbedalvaro 0:345b3bc7a0ea 306 pos.x=maxWall.x+bumpVector.x;
mbedalvaro 0:345b3bc7a0ea 307 bWallCollision = true; // this is just computed here to detect bumps
mbedalvaro 0:345b3bc7a0ea 308 innerCollitionDirection.x=-1;
mbedalvaro 0:345b3bc7a0ea 309 } else if (pos.x < minWall.x){
mbedalvaro 0:345b3bc7a0ea 310 posOld.x=minWall.x;
mbedalvaro 0:345b3bc7a0ea 311 pos.x=minWall.x+bumpVector.x;
mbedalvaro 0:345b3bc7a0ea 312 innerCollitionDirection.x=1;
mbedalvaro 0:345b3bc7a0ea 313 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 314 }
mbedalvaro 0:345b3bc7a0ea 315
mbedalvaro 0:345b3bc7a0ea 316 if (pos.y > maxWall.y){
mbedalvaro 0:345b3bc7a0ea 317 posOld.y=maxWall.y;
mbedalvaro 0:345b3bc7a0ea 318 pos.y=maxWall.y+bumpVector.y;
mbedalvaro 0:345b3bc7a0ea 319 innerCollitionDirection.y=-1;
mbedalvaro 0:345b3bc7a0ea 320 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 321 } else if (pos.y < minWall.y){
mbedalvaro 0:345b3bc7a0ea 322 posOld.y=minWall.y;
mbedalvaro 0:345b3bc7a0ea 323 pos.y=minWall.y+bumpVector.y;
mbedalvaro 0:345b3bc7a0ea 324 innerCollitionDirection.y=1;
mbedalvaro 0:345b3bc7a0ea 325 bWallCollision = true;
mbedalvaro 0:345b3bc7a0ea 326 }
mbedalvaro 0:345b3bc7a0ea 327 #endif
mbedalvaro 0:345b3bc7a0ea 328
mbedalvaro 0:345b3bc7a0ea 329 }