车辆信号完善，路边停车完善。

yy1717

2020-03-24 25777013517d1bd398a98504826a417236706af2

 lib/src/main/cpp/Geometry.cpp

@@ -16,10 +16,16 @@
using namespace std;

const double EPSILON = 1e-6;
const double EPSILON2 = 1e-3;

inline bool isEqual(double a, double b)
{
    return (fabs(a - b) <= EPSILON);
}

inline bool isEqual2(double a, double b)
{
    return (fabs(a - b) <= EPSILON2);
}

inline double toRadians(double degree)
@@ -434,3 +440,37 @@
        return -1;
    }
}

/***************************************************************
 * 得到p3于p1,p2组成的直线上的垂点
 * @param p1
 * @param p2
 * @param p3
 * @return
 */
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3)
{
    PointF p4;

    if (isEqual2(p1.X, p2.X)) {
        p4.Y = p3.Y;
        p4.X = p1.X;
        return p4;
    }
    if (isEqual2(p1.Y, p2.Y)) {
        p4.X = p3.X;
        p4.Y = p1.Y;
        return p4;
    }

    double k = (p2.Y - p1.Y) / (p2.X - p1.X);
    double b = p1.Y - k * p1.X;

    double k2 = (p2.X - p1.X) / (p1.Y - p2.Y);
    double b2 = p3.Y - p3.X * k2;

    p4.X = (b2 - b) / (k - k2);
    p4.Y = k2 * p4.X + b2;

    return p4;
}