sample
Diff: sample02.cpp
- Revision:
- 0:0b1d25a1682d
- Child:
- 1:85da508f8d7e
diff -r 000000000000 -r 0b1d25a1682d sample02.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sample02.cpp Mon Jul 08 07:17:33 2019 +0000 @@ -0,0 +1,52 @@ +#include "sample02.h" + +Sample::Sample(double accdis_, double decdis_, double maxspeed_): + accdis(accdis_), + decdis(decdis_), + maxspeed(maxspeed_) +{ +} + +void Sample::terget_xy(int tergetx_, int tergety_, int startx_, int starty_) +{ + tergetDis = hypot((float)(startx_-tergetx_), (float)(starty_-tergety_)); + if (tergetDis > accdis+decdis) { + startx = startx_; + starty = starty_; + tergetx = tergetx_; + tergety = tergety_; + accsec = 2.0 / maxspeed * accdis; + decsec = 2.0 / maxspeed * decdis; + s_vector = 0.1; + middledis = tergetDis - (accsec + decdis); + } +} + +void Sample::calculate(int now_x, int now_y) +{ + nowx = now_x; + nowy = now_y; + nowDis = hypot(nowx,nowy); + tergetRad = atan2((double)(tergety-nowy), (double)(tergetx-nowx)); + /***/ + if (nowDis < accdis) { + counter = sqrt(2.0*accsec/maxspeed*nowDis); + vector = s_vector + (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / (2.0*PI/(maxspeed-s_vector)); + } else if (nowDis > accdis && nowDis < accdis+middledis) { + vector = maxspeed; + } else if (nowDis > accdis+middledis) { + counter = sqrt(2.0*accsec/maxspeed*(tergetDis - nowDis)); + vector = (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter / (2.0*PI/maxspeed)); + } + /***/ +} + +double Sample::getvalue_x() +{ + return vector * cos(tergetRad); +} + +double Sample::getvalue_y() +{ + return vector * sin(tergetRad); +} \ No newline at end of file