坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

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

@@ -43,15 +43,13 @@
    return (radians * 180.0) / M_PI;
}

void MakeLine(Line *line, const PointF *p1, const PointF *p2)
void MAKE_LINE(Line &a, PointF &b, PointF &c)
{
    line->X1 = p1->X;
    line->Y1 = p1->Y;
    line->X2 = p2->X;
    line->Y2 = p2->Y;
    a.p1 = b;
    a.p2 = c;
}

void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set)
/*void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set)
{
    int n = 0;

@@ -61,12 +59,12 @@
        polygon->point[n++] = ptr;
    }
    polygon->num = n;
}
}*/

void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl)
{
    point->X = (bl->X1 + bl->X2) - bp->X;
    point->Y = (bl->Y1 + bl->Y2) - bp->Y;
    point->X = (bl->p1.X + bl->p2.X) - bp->X;
    point->Y = (bl->p1.Y + bl->p2.Y) - bp->Y;
}

void CleanPolygon(Polygon *polygon)
@@ -127,13 +125,13 @@
        int index2 = (index + 1) % polygon->num;
        Line line2;

        line2.X1 = polygon->point[index].X;
        line2.Y1 = polygon->point[index].Y;
        line2.X2 = polygon->point[index2].X;
        line2.Y2 = polygon->point[index2].Y;
        line2.p1.X = polygon->point[index].X;
        line2.p1.Y = polygon->point[index].Y;
        line2.p2.X = polygon->point[index2].X;
        line2.p2.Y = polygon->point[index2].Y;

//        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);
//        LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.p1.X, line.p1.Y, line.p2.X, line.p2.Y,
//             line2.p1.X, line2.p1.Y, line2.p2.X, line2.p2.Y);

        relation_t relation = IntersectionOf(line, line2);

@@ -149,8 +147,8 @@

    PointF point2;

    point2.X = line.X1;
    point2.Y = line.Y1;
    point2.X = line.p1.X;
    point2.Y = line.p1.Y;

    return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}
@@ -172,10 +170,10 @@
        case 2: {
            Line line2;

            line2.X1 = polygon->point[0].X;
            line2.Y1 = polygon->point[0].Y;
            line2.X2 = polygon->point[1].X;
            line2.Y2 = polygon->point[1].Y;
            line2.p1.X = polygon->point[0].X;
            line2.p1.Y = polygon->point[0].Y;
            line2.p2.X = polygon->point[1].X;
            line2.p2.Y = polygon->point[1].Y;
            return IntersectionOf(point, line2);
        }
        default:
@@ -224,21 +222,21 @@

relation_t IntersectionOf(PointF point, Line line)
{
    double bottomY = fmin(line.Y1, line.Y2);
    double topY = fmax(line.Y1, line.Y2);
    double bottomY = fmin(line.p1.Y, line.p2.Y);
    double topY = fmax(line.p1.Y, line.p2.Y);
    bool heightIsRight = point.Y >= bottomY &&
                         point.Y <= topY;
    //Vertical line, slope is divideByZero error!
    if (isEqual(line.X1, line.X2)) {
        if (isEqual(point.X, line.X1) && heightIsRight) {
    if (isEqual(line.p1.X, line.p2.X)) {
        if (isEqual(point.X, line.p1.X) && heightIsRight) {
            return GM_Tangent;
        } else {
            return GM_None;
        }
    }

    double slope = (line.X2 - line.X1) / (line.Y2 - line.Y1);
    bool onLine = isEqual(line.Y1 - point.Y, slope * (line.X1 - point.X));
    double slope = (line.p2.X - line.p1.X) / (line.p2.Y - line.p1.Y);
    bool onLine = isEqual(line.p1.Y - point.Y, slope * (line.p1.X - point.X));

    if (onLine && heightIsRight) {
        return GM_Tangent;
@@ -250,41 +248,41 @@
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)))
    if ((isEqual(line1.p1.X, line1.p2.X) && isEqual(line1.p1.Y, line1.p2.Y)) || (isEqual(line2.p1.X, line2.p2.X) && isEqual(line2.p1.Y, line2.p2.Y)))
        return GM_None;

    if ((isEqual(line1.X1, line2.X1) && isEqual(line1.Y1, line2.Y1)) || (isEqual(line1.X2, line2.X1) && isEqual(line1.Y2, line2.Y1)))
    if ((isEqual(line1.p1.X, line2.p1.X) && isEqual(line1.p1.Y, line2.p1.Y)) || (isEqual(line1.p2.X, line2.p1.X) && isEqual(line1.p2.Y, line2.p1.Y)))
        return GM_Intersection;

    if ((isEqual(line1.X1, line2.X2) && isEqual(line1.Y1, line2.Y2)) || (isEqual(line1.X2, line2.X2) && isEqual(line1.Y2, line2.Y2)))
    if ((isEqual(line1.p1.X, line2.p2.X) && isEqual(line1.p1.Y, line2.p2.Y)) || (isEqual(line1.p2.X, line2.p2.X) && isEqual(line1.p2.Y, line2.p2.Y)))
        return GM_Intersection;

    //  (1) Translate the system so that point A is on the origin.
    line1.X2 -= line1.X1; line1.Y2 -= line1.Y1;
    line2.X1 -= line1.X1; line2.Y1 -= line1.Y1;
    line2.X2 -= line1.X1; line2.Y2 -= line1.Y1;
    line1.p2.X -= line1.p1.X; line1.p2.Y -= line1.p1.Y;
    line2.p1.X -= line1.p1.X; line2.p1.Y -= line1.p1.Y;
    line2.p2.X -= line1.p1.X; line2.p2.Y -= line1.p1.Y;

    //  Discover the length of segment A-B.
    double distAB = sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2);
    double distAB = sqrt(line1.p2.X * line1.p2.X + line1.p2.Y * line1.p2.Y);

    //  (2) Rotate the system so that point B is on the positive X axis.
    double theCos = line1.X2 / distAB;
    double theSin = line1.Y2 / distAB;
    double newX = line2.X1 * theCos + line2.Y1 * theSin;
    double theCos = line1.p2.X / distAB;
    double theSin = line1.p2.Y / distAB;
    double newX = line2.p1.X * theCos + line2.p1.Y * theSin;

    line2.Y1 = line2.Y1 * theCos - line2.X1 * theSin;
    line2.X1 = newX;
    newX = line2.X2 * theCos + line2.Y2 * theSin;
    line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin;
    line2.X2 = newX;
    line2.p1.Y = line2.p1.Y * theCos - line2.p1.X * theSin;
    line2.p1.X = newX;
    newX = line2.p2.X * theCos + line2.p2.Y * theSin;
    line2.p2.Y = line2.p2.Y * theCos - line2.p2.X * theSin;
    line2.p2.X = newX;

    //  Fail if segment C-D doesn't cross line A-B.
    if ((line2.Y1 < 0 && line2.Y2 < 0) || (line2.Y1 >= 0 && line2.Y2 >= 0)) {
    if ((line2.p1.Y < 0 && line2.p2.Y < 0) || (line2.p1.Y >= 0 && line2.p2.Y >= 0)) {
        return GM_None;
    }

    //  (3) Discover the position of the intersection point along line A-B.
    double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1);
    double posAB = line2.p2.X + (line2.p1.X - line2.p2.X) * line2.p2.Y / (line2.p2.Y - line2.p1.Y);

    //  Fail if segment C-D crosses line A-B outside of segment A-B.
    if (posAB < 0 || posAB > distAB) {
@@ -301,19 +299,19 @@

double DistanceOf(PointF point, Line line)
{
//    float a = sqrt((point.X-line.X1)*(point.X-line.X1) + (point.Y-line.Y1)*(point.Y-line.Y1));
//    float b = sqrt((point.X-line.X2)*(point.X-line.X2) + (point.Y-line.Y2)*(point.Y-line.Y2));
    double c = sqrt((line.X1-line.X2)*(line.X1-line.X2) + (line.Y1-line.Y2)*(line.Y1-line.Y2));
//    float a = sqrt((point.X-line.p1.X)*(point.X-line.p1.X) + (point.Y-line.p1.Y)*(point.Y-line.p1.Y));
//    float b = sqrt((point.X-line.p2.X)*(point.X-line.p2.X) + (point.Y-line.p2.Y)*(point.Y-line.p2.Y));
    double c = sqrt((line.p1.X-line.p2.X)*(line.p1.X-line.p2.X) + (line.p1.Y-line.p2.Y)*(line.p1.Y-line.p2.Y));

//    float p = (a+b+c)/2;

//    dis = 2 * sqrt(p*(p-a)*(p-b)*(p-c)) / c;

    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;
    return fabs(point.X*line.p1.Y + point.Y*line.p2.X + line.p1.X*line.p2.Y - line.p2.X*line.p1.Y - line.p1.X*point.Y - point.X*line.p2.Y) / c;
}

/*********************************************************
 * p1----------->p2 线端和Y轴的夹角
 * p1----------->p2 线端和Y轴的夹角(顺时针)
 * @param p1
 * @param p2
 * @return yaw
@@ -361,13 +359,13 @@
double YawOf(Line &line)
{
    PointF p1 = {
            .X = line.X1,
            .Y = line.Y1
            .X = line.p1.X,
            .Y = line.p1.Y
    };

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

    return YawOf(p1, p2);
@@ -383,20 +381,20 @@
 */
double AngleOf(PointF p1, PointF p2, PointF p3)
{
    int rel = IntersectionOfLine(p1, p2, p3);
    relational_position_t rel = IntersectionOfLine(p1, p2, p3);

    switch (rel) {
        case RELATION_LEFT:
        case RELATION_RIGHT: {
        case REL_POS_LEFT:
        case REL_POS_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);
            return (rel == REL_POS_LEFT) ? deg : (360-deg);
        }
        case RELATION_BACK:
        case REL_POS_REAR:
            return 180;
        default:
            return 0;
@@ -413,9 +411,9 @@
 */
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)};
    PointF p1 = {.X = base.p1.X, .Y = base.p1.Y};
    PointF p2 = {.X = base.p2.X, .Y = base.p2.Y};
    PointF p3 = {.X = dest.p2.X + (base.p1.X - dest.p1.X), .Y = dest.p2.Y + (base.p1.Y - dest.p1.Y)};

    return AngleOf(p1, p2, p3);
}
@@ -443,15 +441,15 @@
{
    double angle = 0;

    double dx = base.X2 - dest.X2;
    double dy = base.Y2 - dest.Y2;
    double dx = base.p2.X - dest.p2.X;
    double dy = base.p2.Y - dest.p2.Y;

    dest.X1 += dx;
    dest.Y1 += dy;
    dest.p1.X += dx;
    dest.p1.Y += dy;

    double c2 = pow((dest.X1 - base.X1), 2) + pow((dest.Y1 - base.Y1), 2);
    double a2 = pow((base.X1 - base.X2), 2) + pow((base.Y1 - base.Y2), 2);
    double b2 = pow((dest.X1 - base.X2), 2) + pow((dest.Y1 - base.Y2), 2);
    double c2 = pow((dest.p1.X - base.p1.X), 2) + pow((dest.p1.Y - base.p1.Y), 2);
    double a2 = pow((base.p1.X - base.p2.X), 2) + pow((base.p1.Y - base.p2.Y), 2);
    double b2 = pow((dest.p1.X - base.p2.X), 2) + pow((dest.p1.Y - base.p2.Y), 2);

    angle = acos((a2 + b2 - c2) / (2 * sqrt(a2) * sqrt(b2)));

@@ -535,10 +533,10 @@
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;
    p2.X = line.X2;
    p2.Y = line.Y2;
    p1.X = line.p1.X;
    p1.Y = line.p1.Y;
    p2.X = line.p2.X;
    p2.Y = line.p2.Y;

    return IntersectionOfLine(p1, p2, p);
}
@@ -588,11 +586,11 @@
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;
    p1.X = line.p1.X;
    p1.Y = line.p1.Y;

    p2.X = line.X2;
    p2.Y = line.Y2;
    p2.X = line.p2.X;
    p2.Y = line.p2.Y;

    PointF pv = GetVerticalPoint(p1, p2, point);

@@ -607,11 +605,11 @@
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;
    p1.X = line.p1.X;
    p1.Y = line.p1.Y;

    p2.X = line.X2;
    p2.Y = line.Y2;
    p2.X = line.p2.X;
    p2.Y = line.p2.Y;

    PointF pv = GetVerticalPoint(p1, p2, point);
    vp = pv;
@@ -625,7 +623,7 @@

/****************************************************************
 *                      p3
 *                      |  'L'
 *                      |  'length'
 *                      |
 * p1------------------>p2
 *                      |
@@ -633,11 +631,11 @@
 *                      p3
 * @param p1
 * @param p2
 * @param L
 * @param length
 * @param dir
 * @return
 */
PointF Calc3Point(PointF p1, PointF p2, double L, char dir)
PointF Calc3Point(PointF p1, PointF p2, double length, char dir)
{
    PointF p3;

@@ -649,18 +647,18 @@
    if (isEqual(p1.X, p2.X)) {
        p3.Y = p2.Y;
        if (p2.Y > p1.Y) {
            p3.X = p2.X + ((dir == 'L')? -L:L);
            p3.X = p2.X + ((dir == 'L') ? -length : length);
        } else {
            p3.X = p2.X + ((dir=='L')?L:-L);
            p3.X = p2.X + ((dir=='L') ? length : -length);
        }
        return p3;
    }
    if (isEqual(p1.Y, p2.Y)) {
        p3.X = p2.X;
        if (p2.X > p1.X) {
            p3.Y = p2.Y + ((dir == 'L')? L:-L);
            p3.Y = p2.Y + ((dir == 'L') ? length : -length);
        } else {
            p3.Y = p2.Y + ((dir == 'L')? -L:L);
            p3.Y = p2.Y + ((dir == 'L') ? -length : length);
        }
        return p3;
    }
@@ -670,7 +668,7 @@

    double A = 1 + pow(k, 2);
    double B = 2*k*(b - p2.Y) - 2*p2.X;
    double C = pow(b - p2.Y, 2) + pow(p2.X, 2) - pow(L,2);
    double C = pow(b - p2.Y, 2) + pow(p2.X, 2) - pow(length, 2);

    double x3, y3;