Algorithm Computational Geometry

多边形的内缩算法（一）—全边内缩或外扩

Posted by Williamic on 2020-08-02

1.概述

   在无人机执行航线往复作业过程中，由于边界点或障碍点位置选取的精准度偏差（使用打点器打点时的定位精度问题）和无人机自身的定位精度及控制问题，往往需要在计算航点路径的时候，与边界区和障碍区预留出安全距离，防止因上述原因导致无人机飞出边界或者飞入障碍区发生事故。
   因此在实际计算过程中，可以通过对边界区内缩、对障碍区外扩的方式来预留安全距离，尽而使得航点生成可靠。下图是边界区内缩后和障碍区外扩后的航线（蓝色部分为边界线，白色部分为实际飞行航线，红色区域为障碍区）。

2.全边内缩外扩思路

   多边形的全边内缩和外扩，实际上是以原多边形的各边取平行边，按照多个平行边的相交点取作新多边形的端点P1，以此类推求出外扩或者内缩多边形的每一个端点，再将其连线成为新的多边形，如下图所示。
新多边形

3.实现步骤

1）取相邻两点做单位向量

   对于当前点P来说，P的左右相邻点P1，P2与点P构成两个向量，并且取PP1向量和PP2向量的单位向量。如下图所示。
单位向量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
public static List<LatLong> narrowAll(List<LatLong> oldLatLongs, float dis) {
if (oldLatLongs.size() >= 3) {
        for (int i = 0; i < oldLatLongs.size(); i++) {
            ......
            //当前点坐标
            LatLong center = oldLatLongs.get(i);
            LatLong loc1;
            LatLong loc2;
            loc1 = oldLatLongs.get((i + oldLatLongs.size() -1)%oldLatLongs.size());
            loc2 = oldLatLongs.get((i + 1)%oldLatLongs.size());
            //坐标点转向量
            Vector2D vec_center = loc2vector(center, center);
            Vector2D vec_loc1 = loc2vector(center, loc1);
            Vector2D vec_loc2 = loc2vector(center, loc2);
            Vector2D center2loc1 = vec_loc1.subtract(vec_center);//PP1向量
            Vector2D center2loc2 = vec_loc2.subtract(vec_center);//PP2向量
            
            //取单位向量D1，D2
            Vector2D dirLoc1 = center2loc1.divide(center2loc1.getLength());
            Vector2D dirLoc2 = center2loc2.divide(center2loc2.getLength());
            ......
        }
    }
    return oldLatLongs;
}

2）求出内缩外扩点上的向量

   求出单位向量后，因为两个方向的向量大小模长都是1，将D1和D2向量相加，求出内缩或者外扩上的向量的单位向量D。如下图所示。
外扩内缩上的单位向量

3）求当前点内缩和外扩的新点

   对于全边内缩外扩来说，两邻边的距离都是相同的距离dis，此时取向量D与PP1的弧度，根据弧度计算向量D在PP1向量的上的距离v。计算v和dis的比，在根据比来计算向量D方向上的目标向量d1和目标向量的反向向量d2。最后根据两个向量求出两个目标点L1和L2，如下图所示。
外扩内缩上的单位向量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
......
Vector2D point = dirLoc1.add(dirLoc2);//目标向量的平行向量
double v = Vector2D.radianBetween(dirLoc1, point);//目标向量与dirLoc1的夹角（弧度）
double v1 = dis / Math.sin(v);
double v2 = point.getLength() / v1;//求出目标向量的缩放比例

Vector2D divide1 = point.divide(v2);//目标向量
double lat = center.getLatitude() + (divide1.y / 100) * (89.83204953368922 / 1E7);
double lon = center.getLongitude() + (divide1.x / 100) * ((89.83204953368922 / 1E7) / longitude_scale(center.getLatitude()));
LatLong L1 = new LatLong(lat, lon);

Vector2D divide2 = new Vector2D(-divide1.x, -divide1.y);//目标向量的相反向量
double lat2 = center.getLatitude() + (divide2.y / 100) * (89.83204953368922 / 1E7);
double lon2 = center.getLongitude() + (divide2.x / 100) * ((89.83204953368922 / 1E7) / longitude_scale(center.getLatitude()));
LatLong L2 = new LatLong(lat2, lon2);
......

4）判断目标点位置

   求出两个点之后，根据L1和L2点的位置，判断此时点的位置位于多边形的内还是外，如果此时是内缩，目标点应该位于多边形的内部，反之则为外部。如何判断点在多边形的内外方法可以查看判断两个多边形的位置关系算法。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
LatLong L1;
LatLong L2;
if (dis >= 0) {//内缩
    if (isInPolygon(L1, oldLatLongs)) {
        newLatLong = L1;
    } else {
        newLatLong = L2;
    }
} else {//外扩
    if (isInPolygon(L1, oldLatLongs)) {
        newLatLong = L2;
    } else {
        newLatLong = L1;
    }
}

4.代码示例

1）完整代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
/**
 * 内缩全部边
 * @param oldLatLongs   原始边界点
 * @param dis           内缩距离（单位米）
 * @return              内缩后的边界点
 */
public static List<LatLong> narrowAll(List<LatLong> oldLatLongs, float dis) {
    if (oldLatLongs.size() >= 3) {
        List<LatLong> newLatLongs = new ArrayList<>();
        for (int i = 0; i < oldLatLongs.size(); i++) {
            LatLong newLatLong;
            if (dis == 0.0f) {
                newLatLong = oldLatLongs.get(i);
            } else {
                Pair<LatLong, LatLong> pair;
                LatLong center = oldLatLongs.get(i);
                int d = (int) (dis * 100);//以厘米单位计算

                LatLong loc1;
                LatLong loc2;
                loc1 = oldLatLongs.get((i + oldLatLongs.size() -1)%oldLatLongs.size());
                loc2 = oldLatLongs.get((i + 1)%oldLatLongs.size());
                pair = getShortenLocation(center, loc1, loc2, d);
                LatLong first = pair.first;
                LatLong second = pair.second;

                if (dis >= 0) {//内缩
                    if (isInPolygon(first, oldLatLongs)) {
                        newLatLong = first;
                    } else {
                        newLatLong = second;
                    }
                } else {//外扩
                    if (isInPolygon(first, oldLatLongs)) {
                        newLatLong = second;
                    } else {
                        newLatLong = first;
                    }
                }
            }
            newLatLongs.add(newLatLong);
        }
        return newLatLongs;
    }
    return oldLatLongs;
}



/**
 * 地块全边内缩外扩
 *
 * @param center
 * @param loc1
 * @param loc2
 * @param dis    内缩距离（厘米）
 * @return
 */
private static Pair<LatLong, LatLong> getShortenLocation(LatLong center, LatLong loc1, LatLong loc2, int dis) {
    Vector2D vec_center = loc2vector(center, center);
    Vector2D vec_loc1 = loc2vector(center, loc1);
    Vector2D vec_loc2 = loc2vector(center, loc2);
    Vector2D center2loc1 = vec_loc1.subtract(vec_center);//vec_loc1-vec_center
    Vector2D center2loc2 = vec_loc2.subtract(vec_center);//vec_loc2-vec_center
    Vector2D dirLoc1 = center2loc1.divide(center2loc1.getLength());//loc1的单位向量:center2loc1/center2loc1.getLength()
    Vector2D dirLoc2 = center2loc2.divide(center2loc2.getLength());//loc2的单位向量：center2loc2/center2loc2.getLength()
    Vector2D point = dirLoc1.add(dirLoc2);//目标向量的平行向量
    double v = Vector2D.radianBetween(dirLoc1, point);//目标向量与dirLoc1的夹角（弧度）
    double v1 = dis / Math.sin(v);//平移dis厘米，对应的point的模
    double v2 = point.getLength() / v1;//point缩放比例

    Vector2D divide1 = point.divide(v2);//目标向量
    double lat = center.getLatitude() + (divide1.y / 100) * (89.83204953368922 / 1E7);
    double lon = center.getLongitude() + (divide1.x / 100) * ((89.83204953368922 / 1E7) / longitude_scale(center.getLatitude()));
    LatLong L1 = new LatLong(lat, lon);

    Vector2D divide2 = new Vector2D(-divide1.x, -divide1.y);
    double lat2 = center.getLatitude() + (divide2.y / 100) * (89.83204953368922 / 1E7);
    double lon2 = center.getLongitude() + (divide2.x / 100) * ((89.83204953368922 / 1E7) / longitude_scale(center.getLatitude()));
    LatLong L2 = new LatLong(lat2, lon2);
    return Pair.create(L1, L2);
}

2）其他相关代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
public Vector2D(double _x, double _y) {
    x = _x;
    y = _y;
}

/**
 * 坐标点转向量
 *
 * @param origin
 * @param loc
 * @return
 */
public static Vector2D loc2vector(LatLong origin, LatLong loc) {
    Vector2D vector2D = new Vector2D(lon2cm(origin, loc), lat2cm(origin, loc));
    return vector2D;
}

//向量加
public Vector2D add(Vector2D v) {
    return new Vector2D(x + v.x, y + v.y);
}

//计算2个向量的夹角弧度
//参考点积公式:v1 * v2 = cos<v1,v2> * |v1| *|v2|
public static double radianBetween(Vector2D v1, Vector2D v2) {
    if (!v1.isNormalized())
        v1 = v1.clone().normalize(); // |v1| = 1
    if (!v2.isNormalized())
        v2 = v2.clone().normalize(); // |v2| = 1
    return Math.acos(v1.dotProduct(v2));
}

//是否已经标准化
public boolean isNormalized() {
    return getLength() == 1.0;
}

public double getLength() {
    return Math.sqrt(getLengthSQ());
}

/**
 * 纬度差转距离（厘米）
 *
 * @param origin
 * @param loc
 * @return
 */
private static double lat2cm(LatLong origin, LatLong loc) {
    double v = loc.getLatitude() - origin.getLatitude();//纬度差
    double v1 = v / (89.83204953368922 / 1E7) * 100;//厘米
    return v1;
}

/**
 * 经度差转距离（厘米）
 *
 * @param origin
 * @param loc
 * @return
 */
private static double lon2cm(LatLong origin, LatLong loc) {
    double v = loc.getLongitude() - origin.getLongitude();//经度差
    double v1 = (89.83204953368922 / 1E7) / longitude_scale(origin.getLatitude());//对应的比例  XX°/m
    double v2 = v / v1 * 100;//厘米
    return v2;
}

/**
 * 经度补偿
 *
 * @param latitude 纬度
 * @return
 */
public static double longitude_scale(double latitude) {
    double cos = Math.cos(latitude * 3.14159265358979323846 / 180.0);
    if (cos < 0.01) {
        cos = 0.01;
    } else if (cos > 1.0) {
        cos = 1.0;
    }
    return cos;
}

5.总结

   由于是全边内缩距离相同，所以可以直接计算内缩后的点的位置，全边内缩步骤思路：

  • 首先取相邻两个点与当前端点构成两个向量做单位向量
  • 计算两个单位向量的法线位置上单位向量，内缩和外扩点在这个向量的方向上
  • 根据内缩和外扩的距离，计算实际上的内缩和外扩点
  • 根据点的位置在多边形的内外来判断是否为需要的点（在多边形内部则为内缩点）



FEATURED TAGS
Algorithm Computational Geometry
FRIENDS