258 lines
6.1 KiB
C++
258 lines
6.1 KiB
C++
#include "wagon.h"
|
|
|
|
#define SLOWVELOCITY 0.1
|
|
|
|
#define EDGE_WALL
|
|
//#define EDGE_WRAP
|
|
|
|
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();
|
|
_lasttimefast=millis();
|
|
_wagoncolor=wagoncolor;
|
|
|
|
}
|
|
|
|
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
|
|
#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/_trainlength; //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
|
|
|
|
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
|
|
float bb=C_ROLL*CONST_G*updatedelayms/1000*sin(beta); //roll resistance
|
|
if (_vel<0){
|
|
bb*=-1;
|
|
}
|
|
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);*/
|
|
|
|
float _testvel=_vel;
|
|
if (_testvel<0){
|
|
_testvel*=-1;
|
|
}
|
|
if (_testvel>SLOWVELOCITY){ //for despawn if slow
|
|
_lasttimefast=millis();
|
|
}
|
|
|
|
|
|
if (_pos>=_numpixels){
|
|
#ifdef EDGE_WRAP
|
|
_pos-=_numpixels; //Wrap around edges
|
|
#endif
|
|
#ifdef EDGE_WALL
|
|
_vel*=-1; //wall at edges
|
|
#endif
|
|
}
|
|
if (_pos<0){
|
|
#ifdef EDGE_WRAP
|
|
_pos=_numpixels+_pos; //warp around edges
|
|
#endif
|
|
#ifdef EDGE_WALL
|
|
_vel*=-1;; //wall at edges
|
|
#endif
|
|
}
|
|
|
|
}
|
|
|
|
float Wagon::getHeight(int p){
|
|
|
|
|
|
if (p<0){
|
|
#ifdef EDGE_WRAP
|
|
p=numpixels+p; //wrap edge
|
|
#endif
|
|
#ifdef EDGE_WALL
|
|
return _height[0]+p*-10; //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)*10; //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);
|
|
|
|
uint32_t c=_wagoncolor;
|
|
uint8_t _r = (uint8_t)(c >> 16);
|
|
uint8_t _g = (uint8_t)(c >> 8);
|
|
uint8_t _b = (uint8_t)c;
|
|
|
|
_r*=featherbrightness;
|
|
_g*=featherbrightness;
|
|
_b*=featherbrightness;
|
|
|
|
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 (millis()>_lasttimefast+5000 ){ //too long too slow
|
|
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);
|
|
}
|