Diff: sample02.cpp

Revision:

0:0b1d25a1682d

Child:

1:85da508f8d7e

--- /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