坐标

yy1717

2020-08-25 0701276b4fec856d5427e4776eec3cc7c56ec065

 lib/src/main/cpp/test_common/Geometry.cpp

@@ -14,6 +14,7 @@
#include <iosfwd>
#include <iomanip>
#include <sstream>
#include <vector>

#include "../jni_log.h"

@@ -626,3 +627,43 @@
{
    return isEqual(p1.X, p2.X) && isEqual(p1.Y, p2.Y);
}

double AvgYaw(std::vector<double> &angles)
{
    double x = 0, y = 0;
    double deg = 0;

    for (int i = 0; i < angles.size(); ++i) {
        x += sin(toRadians(angles[i]));
        y += cos(toRadians(angles[i]));
    }

    if (fabs(y) <= EPSILON) {
        if (x > EPSILON) {
            deg = 90;
        } else {
            deg = 270;
        }
    }
    else if (fabs(x) <= EPSILON) {
        if (y > EPSILON) {
            deg = 0;
        } else {
            deg = 180;
        }
    } else {
        deg = toDegree(atan(fabs(x / y)));

        if (x < -EPSILON &&   y > EPSILON) {
            deg = 360 - deg;
        }
        else if (x < -EPSILON && y < -EPSILON) {
            deg = 180 + deg;
        }
        else if (x > EPSILON && y < -EPSILON) {
            deg = 180 - deg;
        }
    }

    return deg;
}