classPointMass.cpp

00001 /*
00002  *  pointMass.cpp
00003  *  laserBlob
00004  *
00005  *  Created by CASSINELLI ALVARO on 5/19/11.
00006  *  Copyright 2011 TOKYO UNIVERSITY. All rights reserved.
00007  *
00008  */
00009 
00010 #include "classPointMass.h"
00011 
00012 // Initialization of static member variables: 
00013 vector2Df pointMass::maxWall(4095, 4095);
00014 vector2Df pointMass::minWall(0, 0);
00015 
00016 //------------------------------------------------------------
00017 pointMass::pointMass(){
00018     setIntegrationStep(.01); // default in case we don't call integration step setting
00019     // NOTE: it is important to set dt before inital conditions in case of VERLET integration, because we need the integration
00020     // step for properly setting the initial speed.
00021     setInitialCondition(0,0,0,0);// default in case we don't call to initial conditions.
00022     setWallLimits(100,100,4000,4000);
00023     mass=1.0; 
00024     dampMotion = 0.0175;//0.025 was good for bigger blobs;//2f;//0.07f;
00025     dampBorder = 0.013f; //0.07f
00026     bFixed = false;
00027     bWallCollision=false;
00028 }
00029 
00030 //------------------------------------------------------------
00031 void pointMass::resetForce(){
00032     totalForce.set(0,0);
00033 }
00034 
00035 //------------------------------------------------------------
00036 void pointMass::addForce(float x, float y){
00037     totalForce.x = totalForce.x + x;
00038     totalForce.y = totalForce.y + y;
00039 }
00040 
00041 //------------------------------------------------------------
00042 void pointMass::addForce(vector2Df forceToAdd){
00043     totalForce+=forceToAdd;
00044 }
00045 
00046 //------------------------------------------------------------
00047 void pointMass::addInvSquareForce(float x, float y, float radiusMin, float radiusMax, float scale){
00048     
00049     vector2Df posOfForce;
00050     posOfForce.set(x,y);
00051         
00052     vector2Df diff    = pos - posOfForce; // note: we use the position AT TIME T, so this force is at time t
00053     float length    = diff.length();
00054     
00055     // check close enough and far enough (to avoid singularities for example):
00056     if ((length>radiusMin)&&(length<radiusMax)) {
00057         diff.normalize();
00058         totalForce += diff * scale * 1.0/(length*length+1);
00059     }
00060 }
00061 
00062 void pointMass::addInterInvSquareForce(pointMass &theOtherParticle, float radiusMin, float radiusMax, float scale){
00063     
00064     vector2Df posOfForce;
00065     posOfForce.set(theOtherParticle.pos);
00066     
00067     vector2Df diff    = pos - posOfForce; // note: we use the position AT TIME T, so this force is at time t
00068     float length    = diff.length();
00069     
00070     // check close enough and far enough (to avoid singularities for example):
00071     if ((length>radiusMin)&&(length<radiusMax)) {
00072         diff.normalize();
00073         totalForce += diff * scale * 1.0/(length*length+1);
00074         theOtherParticle.totalForce -= diff * scale * 1.0/(length*length+1);
00075     }
00076 }
00077 
00078 
00079 //------------------------------------------------------------
00080 void pointMass::addSpringForce(float centerx, float centery, float radius, float scale){
00081     
00082     // ----------- (1) make a vector of where this particle p is: 
00083     vector2Df posOfForce;
00084     posOfForce.set(centerx, centery);
00085     
00086     // ----------- (2) calculate the difference & length 
00087     
00088     vector2Df diff    = pos - posOfForce;
00089     float length    = diff.length();
00090     
00091     // ----------- (3) check close enough
00092     
00093     bool bAmCloseEnough = true;
00094     if (radius > 0){
00095         if (length > radius){
00096             bAmCloseEnough = false;
00097         }
00098     }
00099     
00100     // ----------- (4) if so, update force
00101     
00102     if (bAmCloseEnough == true){
00103         float pct = 1 - (length / radius);  // stronger on the inside
00104         diff.normalize();
00105         totalForce += diff * scale * pct;
00106     }
00107 }
00108 
00109 void pointMass::addInterSpringForce(pointMass &theOtherParticle, float radius, float scale){
00110     
00111     // ----------- (1) make a vector of where this particle p is: 
00112     vector2Df posOfForce;
00113     posOfForce.set(theOtherParticle.pos);
00114     
00115     // ----------- (2) calculate the difference & length 
00116     
00117     vector2Df diff    = pos - posOfForce;
00118     float length    = diff.length();
00119     
00120     // ----------- (3) check close enough
00121     
00122     bool bAmCloseEnough = true;
00123     if (radius > 0){
00124         if (length > radius){
00125             bAmCloseEnough = false;
00126         }
00127     }
00128     
00129     // ----------- (4) if so, update REPULSIVE force
00130     
00131     if (bAmCloseEnough == true){
00132         float pct = 1 - (length / radius);  
00133         diff.normalize();
00134         totalForce += diff * scale * pct;
00135         theOtherParticle.totalForce -= diff * scale * pct;
00136         //theOtherParticle.frc.x = p.frc.x - diff.x * scale * pct;
00137         //theOtherParticle.frc.y = p.frc.y - diff.y * scale * pct;
00138     }
00139 }
00140 
00141 
00142 
00143 //------------------------------------------------------------
00144 void pointMass::addDampingForce(){ // NOTE: use only in case of EULER intgration!
00145     totalForce-= speed* dampMotion;
00146 }
00147 
00148 //------------------------------------------------------------
00149 void pointMass::setIntegrationStep(float _dt){
00150     dt=_dt;
00151 }
00152 
00153 //------------------------------------------------------------
00154 void pointMass::setInitialCondition(vector2Df _pos, vector2Df _speed) {
00155     setInitialCondition(_pos.x, _pos.y, _speed.x, _speed.y); 
00156 }
00157 void pointMass::setInitialCondition(float px, float py, float vx, float vy){
00158 #ifndef VERLET_METHOD
00159     pos.set(px,py);
00160     speed.set(vx,vy);
00161 #else
00162     // In case of Verlet method, setting the speed is a little more complicated. It involves in particular the integration step
00163     // through the approximation formula: 
00164     // speed = (posNew-posOld)/(2*dt), or speed=(pos-posOld)/dt. Hence:
00165     posOld.set(px, py); 
00166     setSpeed(vx, vy); // this assumes posOld known
00167 #endif    
00168 }
00169 
00170 //-------------------------------------------------------
00171 vector2Df pointMass::getSpeed() {
00172     // this will give an estimate of the speed (not computed explicitly using the Verlet integration):
00173     //speed=(posNew-posOld)/(2*dt); // the variable speed is also updated (note: it is private)
00174     speed=(pos-posOld)/dt; // less approximate than the above, but we avoid having a global posNew variable (remember we will have many particles...)
00175     return(speed); 
00176 }
00177 
00178 void pointMass::setSpeed(const vector2Df& vel) { // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed. 
00179     speed.set(vel); // enough for EULER METHOD
00180    
00181    // NECESSARY for VERLET METHOD (we assume posOld known):
00182    // pos=speed*dt+posOld; // when dampMotion=0 (no damping)
00183     // With damping: 
00184     // we have: speed=(posNew-posOld)/(2*dt) and posNew=pos*(2.0-dampMotion)-posOld*(1.0-dampMotion), so:
00185    // pos=(speed*2*dt+posOld+posOld*(1.0-dampMotion))/(2.0-dampMotion);
00186     pos=speed*2*dt/(2.0-dampMotion)+posOld;
00187     
00188     // no need to compute newPos
00189 }
00190 
00191 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.
00192     speed.set(vx, vy); // enough for EULER METHOD
00193     
00194    // NECESSARY for VERLET METHOD (we assume posOld known):
00195    // pos=speed*dt+posOld; // when dampMotion=0 (no damping)
00196     // With damping: 
00197     // we have: speed=(posNew-posOld)/(2*dt) and posNew=pos*(2.0-dampMotion)-posOld*(1.0-dampMotion), so:
00198    // pos=(speed*2*dt+posOld+posOld*(1.0-dampMotion))/(2.0-dampMotion);
00199     pos=speed*2*dt/(2.0-dampMotion)+posOld; 
00200     
00201     // no need to compute newPos
00202 }
00203 
00204 void pointMass::setPos(float px, float py) { // assuming the speed is unchanged (must do some tweaking in case of Verlet integration)
00205     pos.set(px, py);
00206     posOld=pos-speed*dt;// no damping! attn...
00207 }
00208 
00209 //------------------------------------------------------------
00210 void pointMass::update(){    
00211     if (bFixed == false){
00212         acc=totalForce/mass; // this is the acceleration at time t
00213 
00214 #ifndef VERLET_METHOD
00215         // The following equations (Euler integration) assume acceleration constant during time dt:
00216         speed = speed + acc*dt;
00217         pos = pos + speed*dt ;//+acc*dt*dt*0.5;
00218 #else
00219        // acc=0;//
00220         // The following equations are for VERLET integration with pseudo-damping:
00221           //Without damping this is just:
00222         //vector2Df posNew=posOld*2 - pos + acc*dt*dt; // i.e., dampMotion=0;
00223         // With damping: 
00224 //         vector2Df posNew=(pos*(2.0-dampMotion)-posOld*(1.0-dampMotion)+acc*dt*dt); // BAD!!! THIS NOTATION will introduce precision artefacts!!!
00225         vector2Df posNew=pos+(pos-posOld)*(1-dampMotion)+acc*dt*dt;
00226         
00227         // 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).
00228         // 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. 
00229         // 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 
00230         // expression in a better way that automatically makes 0 the contribution of something smaller than the precision (so the following is not really necessary) 
00231 //        vector2Df diff=(pos-posOld)*(1-dampMotion);
00232         // Precision correction (separate axis or not): 
00233        // if (abs(diff.x)<0.00001) diff.x=diff.x;
00234        // if (abs(diff.y)<0.00001) diff.y=diff.y;
00235        //   if (posOld.match(pos, 0.001)==true) {posNew=pos; speed.set(0,0);}
00236 //        vector2Df posNew=pos+diff+acc*dt*dt;
00237      
00238         posOld=pos;
00239         pos=posNew;
00240         
00241         // NOTE: we can also estimate the speed if we want. But this may be unnecessary (call getSpeed() for that). 
00242         
00243 #endif
00244 
00245     // Constrain speed to max speed (in norm):
00246     float normSpeed=getSpeed().length();
00247     if (normSpeed>MAX_PERMISSIBLE_SPEED) {
00248         setSpeed(getSpeed()*1.0*MAX_PERMISSIBLE_SPEED/normSpeed);
00249     }
00250 
00251     }
00252 }
00253 
00254 void pointMass::setWallLimits(float Minx, float Miny, float Maxx, float Maxy) {
00255     maxWall.set(Maxx, Maxy);
00256     minWall.set(Minx, Miny);
00257 }
00258 
00259 //------------------------------------------------------------
00260 void pointMass::bounceOffWalls(){
00261     // NOTE: bounce is easy in case of EULER method; in case of VERLET, we need to do some hack on the positions.
00262     //Note: the walls are in (vector2Dd) horizontalLimits and verticalLimits 
00263  
00264     bWallCollision=false;
00265     innerCollitionDirection.set(0,0);
00266 #ifndef VERLET_METHOD // EULER METHOD!!
00267     
00268     if (pos.x > maxWall.x){
00269         pos.x = maxWall.x; 
00270         speed.x *= -1;
00271         bWallCollision = true;
00272         innerCollitionDirection.x=-1; 
00273     } else if (pos.x < minWall.x){
00274         pos.x = minWall.x; 
00275         speed.x *= -1;
00276         bWallCollision = true;
00277         innerCollitionDirection.x=1; 
00278     }
00279     
00280     if (pos.y > maxWall.y){
00281         pos.y = maxWall.y; 
00282         speed.y *= -1;
00283         bWallCollision = true;
00284         innerCollitionDirection.y=-1; 
00285     } else if (pos.y < minWall.y){
00286         pos.y = minWall.y; 
00287         speed.y *= -1;
00288         bWallCollision = true;
00289         innerCollitionDirection.y=1;
00290     }
00291     
00292     if (bWallCollision) {
00293         // damping:
00294         speed *=(1-dampBorder);
00295         // normalization of collision direction:
00296        innerCollitionDirection.normalize();
00297     }
00298     
00299 #else // THIS IS FOR VERLET METHOD:
00300     // we need to estimate the inverted, damped vector for bumping::
00301     vector2Df bumpVector=getSpeed()*dt*(dampBorder-1.0); // assuming dampBorder<1 of course
00302     if (pos.x > maxWall.x){
00303         //posOld.x=pos.x;
00304         //pos.x=pos.x+bumpVector.x;
00305         posOld.x=maxWall.x;
00306         pos.x=maxWall.x+bumpVector.x;
00307         bWallCollision = true; // this is just computed here to detect bumps
00308         innerCollitionDirection.x=-1; 
00309     } else if (pos.x < minWall.x){
00310         posOld.x=minWall.x;
00311         pos.x=minWall.x+bumpVector.x;
00312          innerCollitionDirection.x=1; 
00313         bWallCollision = true;
00314     }
00315     
00316     if (pos.y > maxWall.y){
00317         posOld.y=maxWall.y;
00318         pos.y=maxWall.y+bumpVector.y;    
00319          innerCollitionDirection.y=-1;     
00320         bWallCollision = true;
00321     } else if (pos.y < minWall.y){
00322         posOld.y=minWall.y;
00323         pos.y=minWall.y+bumpVector.y;
00324          innerCollitionDirection.y=1; 
00325         bWallCollision = true;
00326     }
00327 #endif
00328     
00329 }