File content as of revision 1:3be7b7d050f4:

/*
 *  pointMass.cpp
 *  laserBlob
 *
 *  Created by CASSINELLI ALVARO on 5/19/11.
 *  Copyright 2011 TOKYO UNIVERSITY. All rights reserved.
 *
 */

#include "classPointMass.h"

// Initialization of static member variables: 
vector2Df pointMass::maxWall(4095, 4095);
vector2Df pointMass::minWall(0, 0);

//------------------------------------------------------------
pointMass::pointMass(){
    setIntegrationStep(.01); // default in case we don't call integration step setting
    // NOTE: it is important to set dt before inital conditions in case of VERLET integration, because we need the integration
    // step for properly setting the initial speed.
    setInitialCondition(0,0,0,0);// default in case we don't call to initial conditions.
    setWallLimits(100,100,4000,4000);
    mass=1.0; 
    dampMotion = 0.0175;//0.025 was good for bigger blobs;//2f;//0.07f;
    dampBorder = 0.013f; //0.07f
    bFixed = false;
    bWallCollision=false;
}

//------------------------------------------------------------
void pointMass::resetForce(){
    totalForce.set(0,0);
}

//------------------------------------------------------------
void pointMass::addForce(float x, float y){
    totalForce.x = totalForce.x + x;
    totalForce.y = totalForce.y + y;
}

//------------------------------------------------------------
void pointMass::addForce(vector2Df forceToAdd){
    totalForce+=forceToAdd;
}

//------------------------------------------------------------
void pointMass::addInvSquareForce(float x, float y, float radiusMin, float radiusMax, float scale){
    
    vector2Df posOfForce;
    posOfForce.set(x,y);
        
    vector2Df diff    = pos - posOfForce; // note: we use the position AT TIME T, so this force is at time t
    float length    = diff.length();
    
    // check close enough and far enough (to avoid singularities for example):
    if ((length>radiusMin)&&(length<radiusMax)) {
        diff.normalize();
        totalForce += diff * scale * 1.0/(length*length+1);
    }
}

void pointMass::addInterInvSquareForce(pointMass &theOtherParticle, float radiusMin, float radiusMax, float scale){
    
    vector2Df posOfForce;
    posOfForce.set(theOtherParticle.pos);
    
    vector2Df diff    = pos - posOfForce; // note: we use the position AT TIME T, so this force is at time t
    float length    = diff.length();
    
    // check close enough and far enough (to avoid singularities for example):
    if ((length>radiusMin)&&(length<radiusMax)) {
        diff.normalize();
        totalForce += diff * scale * 1.0/(length*length+1);
        theOtherParticle.totalForce -= diff * scale * 1.0/(length*length+1);
    }
}


//------------------------------------------------------------
void pointMass::addSpringForce(float centerx, float centery, float radius, float scale){
    
    // ----------- (1) make a vector of where this particle p is: 
    vector2Df posOfForce;
    posOfForce.set(centerx, centery);
    
    // ----------- (2) calculate the difference & length 
    
    vector2Df diff    = pos - posOfForce;
    float length    = diff.length();
    
    // ----------- (3) check close enough
    
    bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
    
    // ----------- (4) if so, update force
    
    if (bAmCloseEnough == true){
        float pct = 1 - (length / radius);  // stronger on the inside
        diff.normalize();
        totalForce += diff * scale * pct;
    }
}

void pointMass::addInterSpringForce(pointMass &theOtherParticle, float radius, float scale){
    
    // ----------- (1) make a vector of where this particle p is: 
    vector2Df posOfForce;
    posOfForce.set(theOtherParticle.pos);
    
    // ----------- (2) calculate the difference & length 
    
    vector2Df diff    = pos - posOfForce;
    float length    = diff.length();
    
    // ----------- (3) check close enough
    
    bool bAmCloseEnough = true;
    if (radius > 0){
        if (length > radius){
            bAmCloseEnough = false;
        }
    }
    
    // ----------- (4) if so, update REPULSIVE force
    
    if (bAmCloseEnough == true){
        float pct = 1 - (length / radius);  
        diff.normalize();
        totalForce += diff * scale * pct;
        theOtherParticle.totalForce -= diff * scale * pct;
        //theOtherParticle.frc.x = p.frc.x - diff.x * scale * pct;
        //theOtherParticle.frc.y = p.frc.y - diff.y * scale * pct;
    }
}



//------------------------------------------------------------
void pointMass::addDampingForce(){ // NOTE: use only in case of EULER intgration!
    totalForce-= speed* dampMotion;
}

//------------------------------------------------------------
void pointMass::setIntegrationStep(float _dt){
    dt=_dt;
}

//------------------------------------------------------------
void pointMass::setInitialCondition(vector2Df _pos, vector2Df _speed) {
    setInitialCondition(_pos.x, _pos.y, _speed.x, _speed.y); 
}
void pointMass::setInitialCondition(float px, float py, float vx, float vy){
#ifndef VERLET_METHOD
    pos.set(px,py);
    speed.set(vx,vy);
#else
    // In case of Verlet method, setting the speed is a little more complicated. It involves in particular the integration step
    // through the approximation formula: 
    // speed = (posNew-posOld)/(2*dt), or speed=(pos-posOld)/dt. Hence:
    posOld.set(px, py); 
    setSpeed(vx, vy); // this assumes posOld known
#endif    
}

//-------------------------------------------------------
vector2Df pointMass::getSpeed() {
    // this will give an estimate of the speed (not computed explicitly using the Verlet integration):
    //speed=(posNew-posOld)/(2*dt); // the variable speed is also updated (note: it is private)
    speed=(pos-posOld)/dt; // less approximate than the above, but we avoid having a global posNew variable (remember we will have many particles...)
    return(speed); 
}

void pointMass::setSpeed(const vector2Df& vel) { // VERY IMPORTANT! in the case of verlet integration, we need to set dt BEFORE setting the initial speed. 
    speed.set(vel); // enough for EULER METHOD
   
   // NECESSARY for VERLET METHOD (we assume posOld known):
   // pos=speed*dt+posOld; // when dampMotion=0 (no damping)
    // With damping: 
    // we have: speed=(posNew-posOld)/(2*dt) and posNew=pos*(2.0-dampMotion)-posOld*(1.0-dampMotion), so:
   // pos=(speed*2*dt+posOld+posOld*(1.0-dampMotion))/(2.0-dampMotion);
    pos=speed*2*dt/(2.0-dampMotion)+posOld;
    
    // no need to compute newPos
}

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.
    speed.set(vx, vy); // enough for EULER METHOD
    
   // NECESSARY for VERLET METHOD (we assume posOld known):
   // pos=speed*dt+posOld; // when dampMotion=0 (no damping)
    // With damping: 
    // we have: speed=(posNew-posOld)/(2*dt) and posNew=pos*(2.0-dampMotion)-posOld*(1.0-dampMotion), so:
   // pos=(speed*2*dt+posOld+posOld*(1.0-dampMotion))/(2.0-dampMotion);
    pos=speed*2*dt/(2.0-dampMotion)+posOld; 
    
    // no need to compute newPos
}

void pointMass::setPos(float px, float py) { // assuming the speed is unchanged (must do some tweaking in case of Verlet integration)
    pos.set(px, py);
    posOld=pos-speed*dt;// no damping! attn...
}

//------------------------------------------------------------
void pointMass::update(){    
    if (bFixed == false){
        acc=totalForce/mass; // this is the acceleration at time t

#ifndef VERLET_METHOD
        // The following equations (Euler integration) assume acceleration constant during time dt:
        speed = speed + acc*dt;
        pos = pos + speed*dt ;//+acc*dt*dt*0.5;
#else
       // acc=0;//
        // The following equations are for VERLET integration with pseudo-damping:
          //Without damping this is just:
        //vector2Df posNew=posOld*2 - pos + acc*dt*dt; // i.e., dampMotion=0;
        // With damping: 
//         vector2Df posNew=(pos*(2.0-dampMotion)-posOld*(1.0-dampMotion)+acc*dt*dt); // BAD!!! THIS NOTATION will introduce precision artefacts!!!
        vector2Df posNew=pos+(pos-posOld)*(1-dampMotion)+acc*dt*dt;
        
        // 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).
        // 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. 
        // 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 
        // expression in a better way that automatically makes 0 the contribution of something smaller than the precision (so the following is not really necessary) 
//        vector2Df diff=(pos-posOld)*(1-dampMotion);
        // Precision correction (separate axis or not): 
       // if (abs(diff.x)<0.00001) diff.x=diff.x;
       // if (abs(diff.y)<0.00001) diff.y=diff.y;
       //   if (posOld.match(pos, 0.001)==true) {posNew=pos; speed.set(0,0);}
//        vector2Df posNew=pos+diff+acc*dt*dt;
     
        posOld=pos;
        pos=posNew;
        
        // NOTE: we can also estimate the speed if we want. But this may be unnecessary (call getSpeed() for that). 
        
#endif

    // Constrain speed to max speed (in norm):
    float normSpeed=getSpeed().length();
    if (normSpeed>MAX_PERMISSIBLE_SPEED) {
        setSpeed(getSpeed()*1.0*MAX_PERMISSIBLE_SPEED/normSpeed);
    }

    }
}

void pointMass::setWallLimits(float Minx, float Miny, float Maxx, float Maxy) {
    maxWall.set(Maxx, Maxy);
    minWall.set(Minx, Miny);
}

//------------------------------------------------------------
void pointMass::bounceOffWalls(){
    // NOTE: bounce is easy in case of EULER method; in case of VERLET, we need to do some hack on the positions.
    //Note: the walls are in (vector2Dd) horizontalLimits and verticalLimits 
 
    bWallCollision=false;
    innerCollitionDirection.set(0,0);
#ifndef VERLET_METHOD // EULER METHOD!!
    
    if (pos.x > maxWall.x){
        pos.x = maxWall.x; 
        speed.x *= -1;
        bWallCollision = true;
        innerCollitionDirection.x=-1; 
    } else if (pos.x < minWall.x){
        pos.x = minWall.x; 
        speed.x *= -1;
        bWallCollision = true;
        innerCollitionDirection.x=1; 
    }
    
    if (pos.y > maxWall.y){
        pos.y = maxWall.y; 
        speed.y *= -1;
        bWallCollision = true;
        innerCollitionDirection.y=-1; 
    } else if (pos.y < minWall.y){
        pos.y = minWall.y; 
        speed.y *= -1;
        bWallCollision = true;
        innerCollitionDirection.y=1;
    }
    
    if (bWallCollision) {
        // damping:
        speed *=(1-dampBorder);
        // normalization of collision direction:
       innerCollitionDirection.normalize();
    }
    
#else // THIS IS FOR VERLET METHOD:
    // we need to estimate the inverted, damped vector for bumping::
    vector2Df bumpVector=getSpeed()*dt*(dampBorder-1.0); // assuming dampBorder<1 of course
    if (pos.x > maxWall.x){
        //posOld.x=pos.x;
        //pos.x=pos.x+bumpVector.x;
        posOld.x=maxWall.x;
        pos.x=maxWall.x+bumpVector.x;
        bWallCollision = true; // this is just computed here to detect bumps
        innerCollitionDirection.x=-1; 
    } else if (pos.x < minWall.x){
        posOld.x=minWall.x;
        pos.x=minWall.x+bumpVector.x;
         innerCollitionDirection.x=1; 
        bWallCollision = true;
    }
    
    if (pos.y > maxWall.y){
        posOld.y=maxWall.y;
        pos.y=maxWall.y+bumpVector.y;    
         innerCollitionDirection.y=-1;     
        bWallCollision = true;
    } else if (pos.y < minWall.y){
        posOld.y=minWall.y;
        pos.y=minWall.y+bumpVector.y;
         innerCollitionDirection.y=1; 
        bWallCollision = true;
    }
#endif
    
}