NFP碰撞检测算法实现

admin

实现一个完整的CalculateDistance函数来处理多边形边之间的所有可能情况（相交、不相交、重合、平行等）是一个相当复杂的任务，因为它需要处理多种几何情况和边界条件。不过，我可以给你一个简化的框架和示例代码，说明如何开始这样的实现。

请注意，下面的代码仅用于演示目的，并不涵盖所有可能的边界情况和优化。对于生产环境的应用，你可能需要使用更健壮和经过优化的库或算法。

首先，我们需要定义一些辅助函数来处理线段和点的基本几何操作。然后，我们可以实现CalculateDistance函数来处理不同的几何关系。

#include <cmath>
#include <vector>
#include <limits>

// ... (之前定义的Vector2d和AcGePoint2d类) ...

// 判断点是否在线段上（包括端点）
bool IsPointOnSegment(const AcGePoint2d& point, const AcGePoint2d& segmentStart, const AcGePoint2d& segmentEnd) {
    Vector2d vec1 = segmentStart.toVector(point);
    Vector2d vec2 = segmentStart.toVector(segmentEnd);
    return std::fabs(vec1.cross(vec2)) < 1e-6 && // 点到线段的垂直距离几乎为0
           vec1.dot(vec2) >= 0 && // 点在线段的投影点在线段上
           (vec1.lengthSquared() <= vec2.lengthSquared()); // 点到线段起点的距离不超过线段长度
}

// 计算点到线段的最近点（垂足）和距离
std::pair<AcGePoint2d, double> ClosestPointOnSegment(const AcGePoint2d& point, const AcGePoint2d& segmentStart, const AcGePoint2d& segmentEnd) {
    Vector2d segmentVec = segmentStart.toVector(segmentEnd);
    double segmentLengthSquared = segmentVec.lengthSquared();
    if (segmentLengthSquared < 1e-6) { // 线段退化为点
        return {segmentStart, (point - segmentStart).length()};
    }

    Vector2d pointVec = segmentStart.toVector(point);
    double t = pointVec.dot(segmentVec) / segmentLengthSquared;
    if (t < 0) { // 垂足在线段起点之外
        return {segmentStart, pointVec.length()};
    } else if (t > 1) { // 垂足在线段终点之外
        return {segmentEnd, (point - segmentEnd).length()};
    }

    // 垂足在线段上
    AcGePoint2d closestPoint = segmentStart + segmentVec * t;
    return {closestPoint, (point - closestPoint).length()};
}

// 计算两个线段之间的最小距离（简化版，不考虑平行且不相交的情况）
double CalculateDistance(const AcGePoint2d& a1, const AcGePoint2d& a2, const AcGePoint2d& b1, const AcGePoint2d& b2) {
    double minDistance = std::numeric_limits<double>::max();

    // 检查线段a的端点是否在线段b上
    for (const auto& point : {a1, a2}) {
        if (IsPointOnSegment(point, b1, b2)) {
            return 0.0; // 重合
        }
    }

    // 检查线段b的端点是否在线段a上
    for (const auto& point : {b1, b2}) {
        if (IsPointOnSegment(point, a1, a2)) {
            return 0.0; // 重合
        }
    }

    // 计算a的每个端点到线段b的最短距离
    for (const auto& point : {a1, a2}) {
        auto [closestPoint, distance] = ClosestPointOnSegment(point, b1, b2);
        minDistance = std::min(minDistance, distance);
    }

    // 计算b的每个端点到线段a的最短距离
    for (const auto& point : {b1, b2}) {
        auto [closestPoint, distance] = ClosestPointOnSegment(point, a1, a2);
        minDistance = std::min(minDistance, distance);
    }

    return minDistance;
}

// ... 其他代码 ...

// 注意：这个CalculateDistance函数没有处理平行且不相交的情况，也没有考虑浮点数的精度问题。
// 在实际应用中，你可能需要添加额外的逻辑来处理这些情况，并使用更健壮的方法来比较浮点数。

admin

1. nfp 临界多边形算法移动碰撞法的基本步骤如下： 首先根据多边形A和B当前时刻的靠接状态，得到B下一步的移动方向，在该移动方向上，计算出A和B之间的最小碰撞距离，从而得到移动距离，根据移动方向和移动距离，将B移动到新的位置，然后重复以上过程，直至绕完一圈，回到初始位置。 完整c++代码   static        vector<AcGePoint2d>  NoFitPolygon(vector<AcGePoint2d> py1, vector<AcGePoint2d> py2)
3. 实现NFP（Non-Fitting Polygon）临界多边形算法涉及多个步骤，包括计算多边形的凸包、确定接触点、计算最小碰撞距离等。下面是一个简化版的C++伪代码示例，用于演示如何使用NFP算法来处理两个多边形A和B的碰撞检测和响应：
5. Cpp
6. #include <vector>
7. #include <iostream>
9. struct AcGePoint2d {
10.     double x;
11.     double y;
12. };
14. // 简化版本的NFP算法实现
15. std::vector<AcGePoint2d> NoFitPolygon(std::vector<AcGePoint2d> poly1, std::vector<AcGePoint2d> poly2) {
16.     std::vector<AcGePoint2d> result;
18.     // 假设poly1和poly2已经经过预处理，确保它们是凸的且在原点附近
20.     while (true) {
21.         // 计算poly1和poly2之间的最小距离和相应的法线
22.         AcGePoint2d normal = calculateNormal(poly1, poly2);
23.         double minDistance = calculateMinDistance(poly1, poly2, normal);
25.         if (minDistance > 0) {
26.             // 如果最小距离大于0，则poly1和poly2之间存在碰撞
27.             // 根据碰撞响应调整poly2的位置
28.             movePolygon(poly2, -minDistance * normal);
29.         } else {
30.             // 如果最小距离小于等于0，则poly1和poly2没有碰撞
31.             break;
32.         }
34.         // 检查是否回到了初始位置
35.         if (isBackToStart(poly1, poly2)) {
36.             break;
37.         }
38.     }
40.     return result; // 返回结果，这里可能需要返回一些关于NFP的信息，如碰撞点或碰撞次数
41. }
43. AcGePoint2d calculateNormal(std::vector<AcGePoint2d> poly1, std::vector<AcGePoint2d> poly2) {
44.     // 计算poly1和poly2之间的最小距离的法线
45.     // 这里省略了具体的实现细节
46.     AcGePoint2d normal;
47.     return normal;
48. }
50. double calculateMinDistance(std::vector<AcGePoint2d> poly1, std::vector<AcGePoint2d> poly2, AcGePoint2d normal) {
51.     // 计算poly1和poly2之间的最小距离
52.     // 这里省略了具体的实现细节
53.     double minDistance = 0;
54.     return minDistance;
55. }
57. void movePolygon(std::vector<AcGePoint2d>& poly, AcGePoint2d direction) {
58.     // 根据方向移动多边形
59.     for (auto& point : poly) {
60.         point.x += direction.x;
61.         point.y += direction.y;
62.     }
63. }
65. bool isBackToStart(std::vector<AcGePoint2d> poly1, std::vector<AcGePoint2d> poly2) {
66.     // 检查是否回到了初始位置
67.     // 这里省略了具体的实现细节
68.     bool backToStart = false;
69.     return backToStart;
70. }
71. 请注意，上述代码中的calculateNormal、calculateMinDistance、movePolygon和isBackToStart函数都需要具体实现。此外，实际应用中可能还需要考虑多边形旋转、速度等因素。
73. 由于NFP算法的具体实现较为复杂，涉及到几何计算、线性代数等多个领域的知识，上述代码仅提供了一个框架性的示例，具体实现时需要根据实际情况进行详细设计和编程。
75. double computeCollisionDistance(vector<AcGePoint2d>& poly1, vector<AcGePoint2d>& poly2) {
76. // 遍历poly2边界，与poly1边界对比
77. // ...
78. }  完整的C++代码 给出
80. 计算两个多边形碰撞距离的过程通常涉及找到能够分离这两个多边形的轴，这个轴可以是其中一个多边形的边的法线。下面是一个基于分离轴定理（Separating Axis Theorem, SAT）的简化版C++函数，用于计算两个多边形之间的最小碰撞距离。请注意，为了简化问题，我们假设输入的多边形是凸的。
82. Cpp
83. #include <vector>
84. #include <algorithm>
85. #include <cmath>
87. struct AcGePoint2d {
88.     double x;
89.     double y;
90. };
92. // 向量点乘
93. double dotProduct(const AcGePoint2d& v1, const AcGePoint2d& v2) {
94.     return v1.x * v2.x + v1.y * v2.y;
95. }
97. // 向量叉乘
98. double crossProduct(const AcGePoint2d& v1, const AcGePoint2d& v2) {
99.     return v1.x * v2.y - v1.y * v2.x;
100. }
102. // 计算向量的长度
103. double vectorLength(const AcGePoint2d& v) {
104.     return std::sqrt(v.x * v.x + v.y * v.y);
105. }
107. // 计算向量的单位化
108. AcGePoint2d normalizeVector(const AcGePoint2d& v) {
109.     double length = vectorLength(v);
110.     return {v.x / length, v.y / length};
111. }
113. // 计算两个多边形之间的最小碰撞距离
114. double computeCollisionDistance(std::vector<AcGePoint2d>& poly1, std::vector<AcGePoint2d>& poly2) {
115.     double minDistance = std::numeric_limits<double>::max();
116.     AcGePoint2d normal;
118.     // 对于poly1的每条边
119.     for (size_t i = 0; i < poly1.size(); ++i) {
120.         size_t nextIndex = (i + 1) % poly1.size();
121.         AcGePoint2d edge = {poly1[nextIndex].x - poly1[i].x, poly1[nextIndex].y - poly1[i].y};
122.         AcGePoint2d edgeNormal = normalizeVector({-edge.y, edge.x});
124.         double minProjPoly1 = dotProduct(edgeNormal, poly1[i]);
125.         double maxProjPoly1 = minProjPoly1;
126.         for (size_t j = 0; j < poly1.size(); ++j) {
127.             double proj = dotProduct(edgeNormal, poly1[j]);
128.             minProjPoly1 = std::min(minProjPoly1, proj);
129.             maxProjPoly1 = std::max(maxProjPoly1, proj);
130.         }
132.         double minProjPoly2 = dotProduct(edgeNormal, poly2[0]);
133.         double maxProjPoly2 = minProjPoly2;
134.         for (size_t j = 0; j < poly2.size(); ++j) {
135.             double proj = dotProduct(edgeNormal, poly2[j]);
136.             minProjPoly2 = std::min(minProjPoly2, proj);
137.             maxProjPoly2 = std::max(maxProjPoly2, proj);
138.         }
140.         if (maxProjPoly1 < minProjPoly2 || maxProjPoly2 < minProjPoly1) {
141.             // 如果两个多边形在这个轴上的投影不重叠，则没有碰撞
142.             return std::numeric_limits<double>::max();
143.         }
145.         double overlap = std::max(0.0, std::min(maxProjPoly1, maxProjPoly2) - std::max(minProjPoly1, minProjPoly2));
146.         if (overlap < minDistance) {
147.             minDistance = overlap;
148.             normal = edgeNormal;
149.         }
150.     }
152.     // 对于poly2的每条边，重复上述过程
153.     for (size_t i = 0; i < poly2.size(); ++i) {
154.         size_t nextIndex = (i + 1) % poly2.size();
155.         AcGePoint2d edge = {poly2[nextIndex].x - poly2[i].x, poly2[nextIndex].y - poly2[i].y};
156.         AcGePoint2d edgeNormal = normalizeVector({-edge.y, edge.x});
158.         double minProjPoly1 = dotProduct(edgeNormal, poly1[0]);
159.         double maxProjPoly1 = minProjPoly1;
160.         for (size_t j = 0; j < poly1.size(); ++j) {
161.             double proj = dotProduct(edgeNormal, poly1[j]);
162.             minProjPoly1 = std::min(minProjPoly1, proj);
163.             maxProjPoly1 = std::max(maxProjPoly1, proj);
164.         }
166.         double minProjPoly2 = dotProduct(edgeNormal, poly2[i]);
167.         double maxProjPoly2 = minProjPoly2;
168.         for (size_t j = 0; j < poly2.size(); ++j) {
169.             double proj = dotProduct(edgeNormal, poly2[j]);
170.             minProjPoly2 = std::min(minProjPoly2, proj);
171.             maxProjPoly2 = std::max(maxProjPoly2, proj);
172.         }
174.         if (maxProjPoly1 < minProjPoly2 || maxProjPoly2 < minProjPoly1) {
175.             return std::numeric_limits<double>::max();
176.         }
178.         double overlap = std::max(0.0, std::min(maxProjPoly1, maxProjPoly2) - std::max(minProjPoly1, minProjPoly2));
179.         if (overlap < minDistance) {
180.             minDistance = overlap;
181.             normal = edgeNormal;
182.         }
183.     }
185.     return minDistance;
186. }
187. 这段代码实现了通过遍历每个多边形的所有边，并检查它们的法线方向是否能作为分离轴来判断两个多边形是否有碰撞，以及如果碰撞发生，计算最小碰撞距离。请注意，这个函数返回的是两个多边形之间的最小重叠距离，如果返回值为正，则表示两个多边形有碰撞；如果返回值为std::numeric_limits<double>::max()，则表示没有碰撞。
189. // 计算多边形之间的最小碰撞距离和碰撞方向 double CalculateCollisionDistanceAndDirection(const std::vector<AcGePoint2d>& polyA,                                              const std::vector<AcGePoint2d>& polyB,                                              Vector2d& collisionDirection) {     // 实现计算最小碰撞距离和碰撞方向的逻辑     // 这里需要使用到多边形顶点之间的距离计算、向量运算等     // ...     return minDistance; // 返回最小碰撞距离 }完整的C++代码
191. 在计算两个多边形之间的最小碰撞距离及其方向时，我们可以采用分离轴定理（Separating Axis Theorem, SAT）。以下是基于SAT的C++代码实现，它不仅计算了最小碰撞距离，还返回了碰撞方向。我们使用Vector2d作为collisionDirection的类型，这可以是一个自定义的结构体或者类来表示二维向量。
193. 首先，我们需要定义Vector2d类型：
195. Cpp
196. struct Vector2d {
197.     double x, y;
199.     Vector2d(double x = 0.0, double y = 0.0) : x(x), y(y) {}
201.     Vector2d operator-(const Vector2d& other) const {
202.         return Vector2d(x - other.x, y - other.y);
203.     }
205.     double dot(const Vector2d& other) const {
206.         return x * other.x + y * other.y;
207.     }
209.     Vector2d normalized() const {
210.         double len = sqrt(x * x + y * y);
211.         return Vector2d(x / len, y / len);
212.     }
213. };
214. 接下来是主要的函数实现：
216. Cpp
217. double CalculateCollisionDistanceAndDirection(
218.     const std::vector<AcGePoint2d>& polyA,
219.     const std::vector<AcGePoint2d>& polyB,
220.     Vector2d& collisionDirection)
221. {
222.     double minOverlap = std::numeric_limits<double>::max();
223.     Vector2d bestAxis;
225.     // 遍历polyA的边，计算其法线并作为分离轴
226.     for (size_t i = 0; i < polyA.size(); ++i) {
227.         size_t nextIndex = (i + 1) % polyA.size();
228.         Vector2d edge = Vector2d(polyA[nextIndex].x - polyA[i].x, polyA[nextIndex].y - polyA[i].y);
229.         Vector2d axis = edge.normalized().rotated90CCW();
231.         double minA = std::numeric_limits<double>::max();
232.         double maxA = std::numeric_limits<double>::lowest();
233.         for (const auto& p : polyA) {
234.             double projection = Vector2d(p.x, p.y).dot(axis);
235.             minA = std::min(minA, projection);
236.             maxA = std::max(maxA, projection);
237.         }
239.         double minB = std::numeric_limits<double>::max();
240.         double maxB = std::numeric_limits<double>::lowest();
241.         for (const auto& p : polyB) {
242.             double projection = Vector2d(p.x, p.y).dot(axis);
243.             minB = std::min(minB, projection);
244.             maxB = std::max(maxB, projection);
245.         }
247.         if (maxA < minB || maxB < minA) {
248.             return std::numeric_limits<double>::max(); // 不相交
249.         }
251.         double overlap = std::max(0.0, std::min(maxA, maxB) - std::max(minA, minB));
252.         if (overlap < minOverlap) {
253.             minOverlap = overlap;
254.             bestAxis = axis;
255.         }
256.     }
258.     // 遍历polyB的边，计算其法线并作为分离轴
259.     for (size_t i = 0; i < polyB.size(); ++i) {
260.         size_t nextIndex = (i + 1) % polyB.size();
261.         Vector2d edge = Vector2d(polyB[nextIndex].x - polyB[i].x, polyB[nextIndex].y - polyB[i].y);
262.         Vector2d axis = edge.normalized().rotated90CCW();
264.         double minA = std::numeric_limits<double>::max();
265.         double maxA = std::numeric_limits<double>::lowest();
266.         for (const auto& p : polyA) {
267.             double projection = Vector2d(p.x, p.y).dot(axis);
268.             minA = std::min(minA, projection);
269.             maxA = std::max(maxA, projection);
270.         }
272.         double minB = std::numeric_limits<double>::max();
273.         double maxB = std::numeric_limits<double>::lowest();
274.         for (const auto& p : polyB) {
275.             double projection = Vector2d(p.x, p.y).dot(axis);
276.             minB = std::min(minB, projection);
277.             maxB = std::max(maxB, projection);
278.         }
280.         if (maxA < minB || maxB < minA) {
281.             return std::numeric_limits<double>::max(); // 不相交
282.         }
284.         double overlap = std::max(0.0, std::min(maxA, maxB) - std::max(minA, minB));
285.         if (overlap < minOverlap) {
286.             minOverlap = overlap;
287.             bestAxis = axis;
288.         }
289.     }
291.     collisionDirection = bestAxis;
292.     return minOverlap;
293. }
294. 注意，上面的代码中，axis.rotated90CCW()函数用于计算一个向量逆时针旋转90度后的向量，这可以通过简单的坐标变换实现，例如：
296. Cpp
297. Vector2d Vector2d::rotated90CCW() const {
298.     return Vector2d(-y, x);
299. }
300. 这个函数实现了计算两个多边形之间的最小碰撞距离和碰撞方向的核心逻辑，其中collisionDirection参数用于输出碰撞方向。如果两个多边形不相交，则函数会返回一个非常大的数（通过std::numeric_limits<double>::max()表示），表示没有碰撞发生。

复制代码