Ryu Anzawa
sample
sample02.cpp@1:85da508f8d7e, 2019-07-09 (annotated)
- Committer:
- piroro4560
- Date:
- Tue Jul 09 08:20:47 2019 +0000
- Revision:
- 1:85da508f8d7e
- Parent:
- 0:0b1d25a1682d
- Child:
- 2:b51bb221438a
sample ver-02
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| piroro4560 | 0:0b1d25a1682d | 1 | #include "sample02.h" |
| piroro4560 | 0:0b1d25a1682d | 2 | |
| piroro4560 | 1:85da508f8d7e | 3 | Sample::Sample(double accdis_, double decdis_, double maxspeed_) |
| piroro4560 | 0:0b1d25a1682d | 4 | { |
| piroro4560 | 1:85da508f8d7e | 5 | accdis =accdis_; |
| piroro4560 | 1:85da508f8d7e | 6 | decdis =decdis_; |
| piroro4560 | 1:85da508f8d7e | 7 | maxspeed=maxspeed_; |
| piroro4560 | 0:0b1d25a1682d | 8 | } |
| piroro4560 | 0:0b1d25a1682d | 9 | |
| piroro4560 | 0:0b1d25a1682d | 10 | void Sample::terget_xy(int tergetx_, int tergety_, int startx_, int starty_) |
| piroro4560 | 0:0b1d25a1682d | 11 | { |
| piroro4560 | 0:0b1d25a1682d | 12 | tergetDis = hypot((float)(startx_-tergetx_), (float)(starty_-tergety_)); |
| piroro4560 | 0:0b1d25a1682d | 13 | if (tergetDis > accdis+decdis) { |
| piroro4560 | 0:0b1d25a1682d | 14 | startx = startx_; |
| piroro4560 | 0:0b1d25a1682d | 15 | starty = starty_; |
| piroro4560 | 0:0b1d25a1682d | 16 | tergetx = tergetx_; |
| piroro4560 | 0:0b1d25a1682d | 17 | tergety = tergety_; |
| piroro4560 | 0:0b1d25a1682d | 18 | accsec = 2.0 / maxspeed * accdis; |
| piroro4560 | 0:0b1d25a1682d | 19 | decsec = 2.0 / maxspeed * decdis; |
| piroro4560 | 0:0b1d25a1682d | 20 | s_vector = 0.1; |
| piroro4560 | 1:85da508f8d7e | 21 | middledis = tergetDis - accdis - decdis; |
| piroro4560 | 0:0b1d25a1682d | 22 | } |
| piroro4560 | 0:0b1d25a1682d | 23 | } |
| piroro4560 | 0:0b1d25a1682d | 24 | |
| piroro4560 | 0:0b1d25a1682d | 25 | void Sample::calculate(int now_x, int now_y) |
| piroro4560 | 0:0b1d25a1682d | 26 | { |
| piroro4560 | 0:0b1d25a1682d | 27 | nowx = now_x; |
| piroro4560 | 0:0b1d25a1682d | 28 | nowy = now_y; |
| piroro4560 | 1:85da508f8d7e | 29 | nowDis = hypot((startx-nowx),(starty-nowy)); |
| piroro4560 | 0:0b1d25a1682d | 30 | tergetRad = atan2((double)(tergety-nowy), (double)(tergetx-nowx)); |
| piroro4560 | 0:0b1d25a1682d | 31 | /***/ |
| piroro4560 | 0:0b1d25a1682d | 32 | if (nowDis < accdis) { |
| piroro4560 | 1:85da508f8d7e | 33 | |
| piroro4560 | 0:0b1d25a1682d | 34 | counter = sqrt(2.0*accsec/maxspeed*nowDis); |
| piroro4560 | 1:85da508f8d7e | 35 | vector = s_vector + (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector)); |
| piroro4560 | 1:85da508f8d7e | 36 | |
| piroro4560 | 1:85da508f8d7e | 37 | } else if (nowDis >= accdis && nowDis < accdis+middledis) { |
| piroro4560 | 0:0b1d25a1682d | 38 | vector = maxspeed; |
| piroro4560 | 1:85da508f8d7e | 39 | } else if (nowDis > (accdis+middledis)) { |
| piroro4560 | 1:85da508f8d7e | 40 | |
| piroro4560 | 1:85da508f8d7e | 41 | counter = sqrt(2.0*accsec/maxspeed*fabs(tergetDis-nowDis)); |
| piroro4560 | 1:85da508f8d7e | 42 | vector = (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / (2.0*PI/maxspeed); |
| piroro4560 | 1:85da508f8d7e | 43 | |
| piroro4560 | 0:0b1d25a1682d | 44 | } |
| piroro4560 | 0:0b1d25a1682d | 45 | /***/ |
| piroro4560 | 0:0b1d25a1682d | 46 | } |
| piroro4560 | 0:0b1d25a1682d | 47 | |
| piroro4560 | 0:0b1d25a1682d | 48 | double Sample::getvalue_x() |
| piroro4560 | 0:0b1d25a1682d | 49 | { |
| piroro4560 | 0:0b1d25a1682d | 50 | return vector * cos(tergetRad); |
| piroro4560 | 0:0b1d25a1682d | 51 | } |
| piroro4560 | 0:0b1d25a1682d | 52 | |
| piroro4560 | 0:0b1d25a1682d | 53 | double Sample::getvalue_y() |
| piroro4560 | 0:0b1d25a1682d | 54 | { |
| piroro4560 | 0:0b1d25a1682d | 55 | return vector * sin(tergetRad); |
| piroro4560 | 0:0b1d25a1682d | 56 | } |