Alvaro Cassinelli / Mbed 2 deprecated skinGames_II

save loops

Dependencies:   mbed

classPointMass.cpp@1:3be7b7d050f4, 2014-12-02 (annotated)

Committer:
mbedalvaro
Date:
Tue Dec 02 08:29:59 2014 +0000
Revision:
1:3be7b7d050f4
Parent:
0:df6fdd9b99f0 
updated

Who changed what in which revision?

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