Diff: proto01.cpp

Revision:

3:c46f084010aa

Parent:

1:ad711893a3a7

Child:

4:40c5ac00bb5b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/proto01.cpp	Thu Aug 22 14:07:24 2019 +0000
@@ -0,0 +1,62 @@
+#include "proto01.h"
+
+Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_)
+{
+    accdis   = accdis_;
+    decdis   = decdis_;
+    maxspeed = maxspeed_;
+    s_vector = s_vector_;
+    accdis2 = accdis;
+    decdis2 = decdis;
+}
+
+void Proto1::target_xy(int targetx_, int targety_, int startx_, int starty_)
+{
+    targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_));
+    startx  = startx_;
+    starty  = starty_;
+    targetx = targetx_;
+    targety = targety_;
+    if (targetDis > accdis+decdis) {
+        accdis    = accdis2;
+        decdis    = decdis2;
+        middledis = targetDis - accdis - decdis;
+    } else {
+        maxspeed /= 2;
+        accdis    = targetDis / 2;
+        decdis    = targetDis / 2;
+        middledis = 0;
+    }
+    accsec = 2.0 / maxspeed * accdis;
+    decsec = 2.0 / maxspeed * decdis;
+}
+
+void Proto1::calculate(int now_x, int now_y)
+{
+    nowx = now_x;
+    nowy = now_y;
+    nowDis = hypot((startx-nowx),(starty-nowy));
+    targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx));
+
+    if (nowDis < accdis) {
+        counter = sqrt(2.0*accsec/maxspeed*nowDis);
+        vector = s_vector + (-1 * 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*decsec/maxspeed*fabs(targetDis-nowDis));
+        vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed);
+    } else {
+        vector = 0;
+    }
+}
+
+double Proto1::getvalue_x()
+{
+    return vector * cos(targetRad);
+}
+
+double Proto1::getvalue_y()
+{
+    return vector * sin(targetRad);
+}
\ No newline at end of file