#include "wagon.h" #define SLOWVELOCITY 0.1 #define WAGONLENGTH 5 #define EDGE_WALL //#define EDGE_BOUNCE //#define EDGE_WRAP uint8_t GammaE[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16, 17, 17, 18, 18, 19, 19, 20, 21, 21, 22, 22, 23, 23, 24, 25, 25, 26, 27, 27, 28, 29, 29, 30, 31, 31, 32, 33, 34, 34, 35, 36, 37, 37, 38, 39, 40, 40, 41, 42, 43, 44, 45, 46, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 76, 77, 78, 79, 80, 81, 83, 84, 85, 86, 88, 89, 90, 91, 93, 94, 95, 96, 98, 99,100,102,103,104,106,107,109,110, 111,113,114,116,117,119,120,121,123,124,126,128,129,131,132,134, 135,137,138,140,142,143,145,146,148,150,151,153,155,157,158,160, 162,163,165,167,169,170,172,174,176,178,179,181,183,185,187,189, 191,193,194,196,198,200,202,204,206,208,210,212,214,216,218,220, 222,224,227,229,231,233,235,237,239,241,244,246,248,250,252,255}; Wagon::Wagon(int id,int numpixels, Adafruit_NeoPixel *strip,uint8_t *height,float pos, float trainlength,float startvel,float startacc, float wagonmass, uint32_t wagoncolor) { _id = id; _numpixels=numpixels; _pos = pos; _trainlength = trainlength; _strip=strip; _height=height; _vel=startvel; _acc=startacc; _wagonmass=wagonmass; //mass of whole train _spawntime=millis(); _lastvel=startvel; _lastpositivedirchangepos=_numpixels+1000; _lastpositivedirchangePosDifference=1000; _wagoncolor=wagoncolor; _health=1.0; } bool Wagon::operator==(const Wagon &r) const { return (r._id == _id); } void Wagon::updatePhysics(float updatedelayms) { /* float acceleration=0; for (int cpos=int(_pos);cpos>int(_pos-_trainlength);cpos--){ acceleration=(_height[(int)cpos]-_height[(int)cpos+1])*0.03; } acceleration/=int(_trainlength); _vel+= acceleration; //Acceleration from height difference _vel*=1-0.001; //resistance float airresistance=_vel*_vel *0.005;//air resistance if (_vel>=0){ _vel-=airresistance; }else{ _vel+=airresistance; }*/ #define CONST_G 9.81 #define PIXELDISTANCE 1.6666667 // 1/60.0 * 100 #define C_ROLL 0.001 // = Croll * G https://de.wikipedia.org/wiki/Rollwiderstand 0.001 (zug) #define AIRRESFIRST 0.18 //C_w*A*0.5*rho Air resistance: C_w * A * 0.5 * rho (.... *v^2) #define AIRRES 0.01 //for slipstream at second wagon #define AIRRESMUL 0.7 //how much of air resistance the next wagon has //http://www.kfz-tech.de/Biblio/Formelsammlung/Luftwiderstand.htm C_w 0.6 //rho Massendichte luft 1,2041 //A = 1m^2 float m=_wagonmass; //mass of part of a wagon _acc=0; int cpos=(int)_pos; uint8_t wagonnumber=0; for (int cpos=(int)_pos;cpos>(int)(_pos-_trainlength);cpos--){ //for each wagon if (wagonnumber%WAGONLENGTH==0) { //every n-th pixel is a wagon float hdiff=getHeight((int) (cpos-0.5)) - getHeight((int)(cpos+0.5)); //Serial.print("hdiff="); //Serial.print(hdiff); float beta=atan2(PIXELDISTANCE, hdiff); //Serial.print(" beta="); //Serial.println(beta); //_acc += CONST_G * cos(beta) - C_ROLLG*sin(beta) - AIRRES/m*_vel*_vel; float aa=CONST_G * cos(beta) *updatedelayms/1000; //Gravity and m/s^2 time correction //Roll Resistance float bb=C_ROLL*CONST_G*updatedelayms/1000*sin(beta); //roll resistance if (_vel<0){ bb*=-1; } //Air Resistance float cc=0; if (wagonnumber==0){ //first wagon cc=AIRRESFIRST/m*pow(_vel,2); //air resistance for first wagon }else { cc=AIRRES/m*pow(_vel,2) *pow(AIRRESMUL,wagonnumber-1); //air resistance } if (_vel<0){ cc*=-1; } //Serial.print("aa="); Serial.print(aa); //Serial.print(" bb="); Serial.print(bb); //Serial.print(" cc="); Serial.println(cc); _acc += aa - bb - cc; } wagonnumber++; } _acc*=updatedelayms/1000; _vel += _acc; _pos += _vel/PIXELDISTANCE; /*Serial.print(" Vel="); Serial.print(_vel); Serial.print(" Acc="); Serial.println(_acc);*/ if (_lastvel*_vel<0 && _lastvel>=0){ //direction changed, positive _lastpositivedirchangePosDifference=_lastpositivedirchangepos-_pos; Serial.print("DC pos="); Serial.print(_pos); Serial.print(" last="); Serial.print(_lastpositivedirchangepos); Serial.print(" diff="); Serial.println(_lastpositivedirchangePosDifference); _lastpositivedirchangepos=_pos; } _lastvel=_vel; if (_pos>=_numpixels){ #ifdef EDGE_WRAP _pos-=_numpixels; //Wrap around edges #endif #ifdef EDGE_BOUNCE _vel*=-1; //bounce at edges #endif #ifdef EDGE_WALL //nothing #endif } if (_pos<0){ #ifdef EDGE_WRAP _pos=_numpixels+_pos; //warp around edges #endif #ifdef EDGE_BOUNCE _vel*=-1;; //bounce at edges #endif #ifdef EDGE_WALL //nothing #endif } //fade out if health got low if (_health<1.0 && _health>0.0){ _health-=0.0001; //reduce until 0 } } float Wagon::getHeight(int p){ if (p<0){ #ifdef EDGE_WRAP p=numpixels+p; //wrap edge #endif #ifdef EDGE_WALL return _height[0]+p*-100.0; //edges as wall #endif }else if(p>=_numpixels){ #ifdef EDGE_WRAP p=p-numpixels; //wrap edge #endif #ifdef EDGE_WALL return _height[_numpixels-1]+(p-_numpixels)*100.0; //edges as wall #endif } return _height[p]; } void Wagon::updateGraphics() { float wagonfeathering=2; for(int i=_pos+wagonfeathering;i>_pos-_trainlength-wagonfeathering;i--){ float featherbrightness=1; if (i>_pos){ //in front of wagon featherbrightness=1 - (i-_pos)/wagonfeathering; }else if (i<_pos-_trainlength){ //behind of wagon featherbrightness=1 - (_pos-_trainlength -i)/wagonfeathering; } if (featherbrightness<=0){ //distpercent between 0 and 1. 1-> full brightness, 0-> feathering distance away featherbrightness=0; } //uint32_t c=_strip->Color(0,255*featherbrightness,0); //uint32_t c=Wheel(_height[i]/45.0*255,featherbrightness); //uint8_t b=_height[i]*255/45; //uint8_t b=abs(_vel)*255.0; //uint32_t c=_strip->Color(b*featherbrightness,(255-b)*featherbrightness,0); float healtpositive=_health; if(healtpositive<0){ //only positive values for color healtpositive=0; } uint32_t c=_wagoncolor; uint8_t _r = (uint8_t)(c >> 16)*healtpositive; uint8_t _g = (uint8_t)(c >> 8)*healtpositive; uint8_t _b = (uint8_t)c*healtpositive; _r*=featherbrightness; _g*=featherbrightness; _b*=featherbrightness; _r=GammaE[_r]; _g=GammaE[_g]; _b=GammaE[_b]; uint32_t _pxcolor=_strip->getPixelColor(i); //get current color of that pixel uint8_t _pxr = _pxcolor >> 16; uint8_t _pxg = _pxcolor >> 8; uint8_t _pxb = _pxcolor; uint16_t _tmpr=_pxr+_r; //add colors uint16_t _tmpg=_pxg+_g; uint16_t _tmpb=_pxb+_b; if (_tmpr>255){ //clamp _tmpr=255; } if (_tmpg>255){ _tmpg=255; } if (_tmpb>255){ _tmpb=255; } _strip->setPixelColor(i,_tmpr,_tmpg,_tmpb); //draw pixel } } int Wagon::pos() { return _pos; } int Wagon::id() { return _id; } long Wagon::spawntime() { return _spawntime; } bool Wagon::alive() { /*if (_pos>_numpixels){ return false; }*/ /*if (millis()>_spawntime+30*1000){ //too old return false; }*/ if (_lastpositivedirchangePosDifference<=5){ //reduce health //return false; _health-=0.1; } if (_health<=0) { return false; } return true; } uint32_t Wagon::Wheel(byte WheelPos,float brightness) { WheelPos = 255 - WheelPos; if(WheelPos < 85) { return _strip->Color(255 - WheelPos * 3*brightness, 0, WheelPos * 3*brightness); } if(WheelPos < 170) { WheelPos -= 85; return _strip->Color(0, WheelPos * 3*brightness, 255 - WheelPos * 3*brightness); } WheelPos -= 170; return _strip->Color(WheelPos * 3*brightness, 255 - WheelPos * 3*brightness, 0); }