科目三草案实现

yy1717

2020-03-17 4ff195404b21f74ca11e26a69cbf0418eaa4595f

 lib/src/main/cpp/Geometry.cpp

@@ -301,6 +301,52 @@
    return fabs(point.X*line.Y1 + point.Y*line.X2 + line.X1*line.Y2 - line.X2*line.Y1 - line.X1*point.Y - point.X*line.Y2) / c;
}

/*********************************************************
 * p2----------->p1 线端和Y轴的夹角
 * @param p1
 * @param p2
 * @return yaw
 */
double YawOf(PointF p1, PointF p2)
{
    double deg = 0.0;

    if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
        if (p1.X > p2.X) {
            deg = 90;
        } else {
            deg = 270;
        }
    } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
        if (p1.Y > p2.Y) {
            deg = 0;
        } else {
            deg = 180;
        }
    } else {
        deg = atan(fabs(p1.X - p2.X) /
                   fabs(p1.Y - p2.Y));

        deg = toDegree(deg);

        if (p1.X > p2.X &&
            p1.Y > p2.Y) {

        } else if (p1.X < p2.X &&
                   p1.Y > p2.Y) {
            deg = 360 - deg;
        } else if (p1.X < p2.X &&
                   p1.Y < p2.Y) {
            deg = 180 + deg;
        } else if (p1.X > p2.X &&
                   p1.Y < p2.Y) {
            deg = 180 - deg;
        }
    }

    return deg;
}

/**********************************************************
 * base 和 dest的第二点重合时形成的夹角
 * @param base
@@ -364,7 +410,7 @@
}

/***************************************************************
 * @brief p3位于由p1->p2构成的射线，左侧还是右侧，同向，反向
 * @brief p3位于由p1--------->p2构成的射线，左侧还是右侧，同向，反向
 * @param p1
 * @param p2
 * @param p3