科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

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

@@ -78,7 +78,7 @@
    polygon->num = 0;
}

Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
relation_t IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
{
    bool inside = false, outside = false, tangent = false;

@@ -87,7 +87,7 @@
    }

    for (int idx = 0; idx < polygon1->num; ++idx) {
        Relation relation = IntersectionOf(polygon1->point[idx], polygon2);
        relation_t relation = IntersectionOf(polygon1->point[idx], polygon2);

        if (relation == GM_Containment) {
            inside = true;
@@ -112,7 +112,7 @@
    return GM_None;
}

Relation IntersectionOf(Line line, const Polygon *polygon)
relation_t IntersectionOf(Line line, const Polygon *polygon)
{
    if (polygon->num == 0) {
        return GM_None;
@@ -135,7 +135,7 @@
//        LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.X1, line.Y1, line.X2, line.Y2,
//             line2.X1, line2.Y1, line2.X2, line2.Y2);

        Relation relation = IntersectionOf(line, line2);
        relation_t relation = IntersectionOf(line, line2);

//        LOGD("relation = %d", relation);

@@ -155,7 +155,7 @@
    return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}

Relation IntersectionOf(PointF point, const Polygon *polygon)
relation_t IntersectionOf(PointF point, const Polygon *polygon)
{
    switch (polygon->num)
    {
@@ -222,7 +222,7 @@
    return (counter % 2 == 1) ? GM_Containment : GM_None;
}

Relation IntersectionOf(PointF point, Line line)
relation_t IntersectionOf(PointF point, Line line)
{
    double bottomY = fmin(line.Y1, line.Y2);
    double topY = fmax(line.Y1, line.Y2);
@@ -247,7 +247,7 @@
    }
}

Relation IntersectionOf(Line line1, Line line2)
relation_t IntersectionOf(Line line1, Line line2)
{
    //  Fail if either line segment is zero-length.
    if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
@@ -313,7 +313,7 @@
}

/*********************************************************
 * p2----------->p1 线端和Y轴的夹角
 * p1----------->p2 线端和Y轴的夹角
 * @param p1
 * @param p2
 * @return yaw
@@ -322,40 +322,102 @@
{
    double deg = 0.0;

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

        deg = toDegree(deg);

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

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

    return deg;
}

double YawOf(Line &line)
{
    PointF p1 = {
            .X = line.X1,
            .Y = line.Y1
    };

    PointF p2 = {
            .X = line.X2,
            .Y = line.Y2
    };

    return YawOf(p1, p2);
}

/**************************************************
 * p1----->p3 与 p1----->p2 两射线的逆时针夹角
 * 左侧小于180，右侧大于180
 * @param p1
 * @param p2
 * @param p3
 * @return
 */
double AngleOf(PointF p1, PointF p2, PointF p3)
{
    int rel = IntersectionOfLine(p1, p2, p3);

    switch (rel) {
        case RELATION_LEFT:
        case RELATION_RIGHT: {
            double a = DistanceOf(p2, p3);
            double b = DistanceOf(p1, p2);
            double c = DistanceOf(p1, p3);

            double deg = toDegree(acos((pow(b, 2) + pow(c, 2) - pow(a, 2)) / (2 * b * c)));

            return (rel == RELATION_LEFT) ? deg : (360-deg);
        }
        case RELATION_BACK:
            return 180;
        default:
            return 0;
    };

    return 0;
}

/************************************************
 * dest相对于base的逆时针夹角
 * @param base
 * @param dest
 * @return
 */
double AngleOfTowLine(Line base, Line dest)
{
    PointF p1 = {.X = base.X1, .Y = base.Y1};
    PointF p2 = {.X = base.X2, .Y = base.Y2};
    PointF p3 = {.X = dest.X2 + (base.X1 - dest.X1), .Y = dest.Y2 + (base.Y1 - dest.Y1)};

    return AngleOf(p1, p2, p3);
}

double DeltaYaw(double yaw1, double yaw2)
@@ -449,27 +511,27 @@
 * @param p3
 * @return 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
 */
int IntersectionOfLine(PointF p1, PointF p2, PointF p3)
relational_position_t IntersectionOfLine(PointF p1, PointF p2, PointF p3)
{
    double lr = (p1.X-p3.X)*(p2.Y-p3.Y) - (p1.Y-p3.Y)*(p2.X-p3.X);

    if (fabs(lr) <= EPSILON2) {
        double fb = (p2.X-p1.X)*(p3.X-p1.X) + (p2.Y-p1.Y)*(p3.Y-p1.Y);
        if (fabs(fb) <= EPSILON2)
            return 0;
            return REL_POS_ON;
        else if (fb > 0)
            return 2;
            return REL_POS_FRONT;
        else
            return -2;
            return REL_POS_REAR;
    } else if (lr > 0) {
        return 1;
        return REL_POS_LEFT;
    } else {
        return -1;
        return REL_POS_RIGHT;
    }
}

// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
int IntersectionOfLine(PointF p, Line line)
relational_position_t IntersectionOfLine(PointF p, Line line)
{
    PointF p1, p2;

@@ -688,4 +750,30 @@
    }

    return deg;
}
}

bool Calc3PointCircle(PointF p1, PointF p2, PointF p3, Circle &circle)
{
    double a = p1.X - p2.X;
    double b = p1.Y - p2.Y;
    double c = p1.X - p3.X;
    double d = p1.Y - p3.Y;
    double e = ((pow(p1.X,2) - pow(p2.X,2)) - (pow(p2.Y,2) - pow(p1.Y,2))) / 2;
    double f = ((pow(p1.X,2) - pow(p3.X,2)) - (pow(p3.Y,2) - pow(p1.Y,2))) / 2;

    // 三点不能共线
    if (isEqual2(a*d, b*c)) {
        circle.centre.X = circle.centre.Y = circle.radius = 0;
        return false;
    }

    double x0 = (d*e - b*f)/(a*d - b*c);
    double y0 = (a*f - c*e)/(a*d - b*c);
    double r = sqrt(pow(x0-p1.X, 2) + pow(y0-p1.Y, 2));

    circle.centre.X = x0;
    circle.centre.Y = y0;
    circle.radius = r;

    return true;
}