CGAL中函数squared_distance使用细则

CGAL中函数squared_distance使用细则

📋 目录

引言

计算几何是计算机科学中的重要分支，在计算机图形学、机器人学、地理信息系统、数值控制加工等领域有着广泛应用。本文档深入分析了三个核心的计算几何算法：

• CGAL::squared_distance - 点到线段距离计算的高效实现
• 三点共线判定 - 几何图形分析的基础算法
• 向量投影参数 - 距离计算的数学理论基础

这些算法在NC路径生成、碰撞检测、路径优化等实际工程问题中发挥着关键作用。

---

CGAL::squared_distance 算法详解

📐 函数概述

CGAL::squared_distance(s, m) 计算线段 s 和点 m 之间的平方距离。这个函数是 CGAL 几何算法库中的核心距离计算函数。

🧮 数学原理

点到线段距离的几何分析

设线段 s 由两个端点 P1(x1, y1, z1) 和 P2(x2, y2, z2) 定义，点 m 的坐标为 M(x, y, z)。

算法步骤

1. 参数化线段表示

   线段上任意点: P(t) = P1 + t * (P2 - P1), t ∈ [0, 1]
   方向向量: v = P2 - P1
   

2. 投影计算

   从 P1 到 M 的向量: w = M - P1
   投影参数: t = (w · v) / (v · v)
   

3. 分情况讨论

   • t < 0: 点 M 投影在线段延长线上（P1 侧），最近点是 P1
   • t > 1: 点 M 投影在线段延长线上（P2 侧），最近点是 P2
   • 0 ≤ t ≤ 1: 点 M 投影在线段上，最近点是 P(t)

💻 内部实现逻辑

核心算法伪代码

template<typename Kernel>
FT squared_distance(const Segment_3<Kernel>& s, const Point_3<Kernel>& m) {// 获取线段端点Point_3 p1 = s.source();Point_3 p2 = s.target();// 计算向量Vector_3 v = p2 - p1;  // 线段方向向量Vector_3 w = m - p1;   // P1 到 M 的向量// 计算投影参数FT c1 = w * v;         // 点积 w·vFT c2 = v * v;         // 点积 v·v// 边界情况处理if (c1 <= 0) {// 投影在 P1 之前，最近点是 P1return squared_distance(m, p1);}if (c1 >= c2) {// 投影在 P2 之后，最近点是 P2return squared_distance(m, p2);}// 投影在线段内部FT t = c1 / c2;Point_3 closest = p1 + t * v;return squared_distance(m, closest);
}

🔧 CGAL 特定实现细节

1. 精确算术支持

// CGAL 使用精确算术避免浮点误差
using FT = typename Kernel::FT;  // 可能是 Exact_predicates_exact_constructions_kernel

2. 内核抽象

// 通过内核访问基本操作
typename Kernel::Compute_squared_distance_3 squared_distance_3 = kernel.compute_squared_distance_3_object();

3. 优化的点积计算

// CGAL 内部优化的向量运算
FT dot_product = kernel.compute_scalar_product_3_object()(v, w);

📊 具体计算示例

示例数据

// 线段 s: P1(1,1,1) 到 P2(10,10,10)
// 点 m(5,9,7)
Point_3 p1(1, 1, 1);
Point_3 p2(10, 10, 10);
Point_3 m(5, 9, 7);
Segment_3 s(p1, p2);

计算过程

// 1. 计算向量
Vector_3 v = p2 - p1;  // v = (9, 9, 9)
Vector_3 w = m - p1;   // w = (4, 8, 6)// 2. 计算点积
FT c1 = w * v;         // c1 = 4*9 + 8*9 + 6*9 = 162
FT c2 = v * v;         // c2 = 9*9 + 9*9 + 9*9 = 243// 3. 计算投影参数
FT t = c1 / c2;        // t = 162/243 = 2/3// 4. 计算最近点
Point_3 closest = p1 + t * v;  // closest = (1,1,1) + (2/3)*(9,9,9) = (7,7,7)// 5. 计算平方距离
Vector_3 diff = m - closest;   // diff = (5,9,7) - (7,7,7) = (-2,2,0)
FT result = diff * diff;       // result = 4 + 4 + 0 = 8

⚡ 性能优化技术

1. 避免开方运算

// 返回平方距离而不是距离，避免昂贵的开方运算
return squared_distance;  // 而不是 sqrt(squared_distance)

2. 早期退出优化

// 快速检查边界情况
if (c1 <= 0) return squared_distance(m, p1);  // 立即返回
if (c1 >= c2) return squared_distance(m, p2); // 立即返回

3. 内存访问优化

// 减少临时对象创建
FT dx = m.x() - closest_x;
FT dy = m.y() - closest_y;
FT dz = m.z() - closest_z;
return dx*dx + dy*dy + dz*dz;

🎯 数值稳定性考虑

1. 精确算术

// CGAL 支持多种数值类型
using Kernel = CGAL::Exact_predicates_exact_constructions_kernel;
// 或
using Kernel = CGAL::Simple_cartesian<double>;

2. 退化情况处理

// 处理零长度线段
if (v * v == 0) {// 线段退化为点，直接计算点到点距离return squared_distance(m, p1);
}

---

三点共线原理与实现

🎯 基本概念

三点共线是指三个点位于同一条直线上的几何性质。这是计算几何中的基础概念，在数值计算、图形学、路径规划等领域有广泛应用。

📊 数学判定方法

1. 向量叉积法（2D）

对于平面上三点 A(x₁, y₁)、B(x₂, y₂)、C(x₃, y₃)：

向量 AB = (x₂ - x₁, y₂ - y₁)
向量 AC = (x₃ - x₁, y₃ - y₁)叉积：AB × AC = (x₂ - x₁)(y₃ - y₁) - (y₂ - y₁)(x₃ - x₁)判定条件：
- 叉积 = 0  → 三点共线
- 叉积 ≠ 0  → 三点不共线

2. 行列式法

| x₁  y₁  1 |
| x₂  y₂  1 | = 0  ⟺  三点共线
| x₃  y₃  1 |展开：x₁(y₂ - y₃) + x₂(y₃ - y₁) + x₃(y₁ - y₂) = 0

3. 斜率法

斜率 k₁ = (y₂ - y₁)/(x₂ - x₁)  （AB的斜率）
斜率 k₂ = (y₃ - y₂)/(x₃ - x₂)  （BC的斜率）判定条件：k₁ = k₂  ⟺  三点共线注意：需要处理垂直线的特殊情况

4. 距离法

距离 d₁ = |AB|  （A到B的距离）
距离 d₂ = |BC|  （B到C的距离）
距离 d₃ = |AC|  （A到C的距离）判定条件：d₁ + d₂ = d₃  或  d₁ + d₃ = d₂  或  d₂ + d₃ = d₁

🔢 3D空间扩展

向量共面法

对于空间三点 A、B、C：

向量 AB = B - A
向量 AC = C - A叉积：AB × AC = (0, 0, 0)  ⟺  三点共线计算：
AB × AC = |i  j  k ||x₁ y₁ z₁||x₂ y₂ z₂|= i(y₁z₂ - z₁y₂) - j(x₁z₂ - z₁x₂) + k(x₁y₂ - y₁x₂)

💻 编程实现

C++ 实现（2D）

#include <cmath>
#include <iostream>struct Point2D {double x, y;Point2D(double x = 0, double y = 0) : x(x), y(y) {}
};// 方法1：叉积法
bool areCollinear_CrossProduct(const Point2D& A, const Point2D& B, const Point2D& C) {double cross = (B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x);return std::abs(cross) < 1e-9;  // 考虑浮点误差
}// 方法2：行列式法
bool areCollinear_Determinant(const Point2D& A, const Point2D& B, const Point2D& C) {double det = A.x * (B.y - C.y) + B.x * (C.y - A.y) + C.x * (A.y - B.y);return std::abs(det) < 1e-9;
}// 方法3：斜率法
bool areCollinear_Slope(const Point2D& A, const Point2D& B, const Point2D& C) {// 处理垂直线if (std::abs(B.x - A.x) < 1e-9 && std::abs(C.x - B.x) < 1e-9) {return true;  // 都是垂直线}if (std::abs(B.x - A.x) < 1e-9 || std::abs(C.x - B.x) < 1e-9) {return false; // 一个垂直一个不垂直}double slope1 = (B.y - A.y) / (B.x - A.x);double slope2 = (C.y - B.y) / (C.x - B.x);return std::abs(slope1 - slope2) < 1e-9;
}

C++ 实现（3D）

struct Point3D {double x, y, z;Point3D(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}
};struct Vector3D {double x, y, z;Vector3D(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}Vector3D(const Point3D& from, const Point3D& to) : x(to.x - from.x), y(to.y - from.y), z(to.z - from.z) {}
};// 向量叉积
Vector3D crossProduct(const Vector3D& a, const Vector3D& b) {return Vector3D(a.y * b.z - a.z * b.y,a.z * b.x - a.x * b.z,a.x * b.y - a.y * b.x);
}// 向量模长
double magnitude(const Vector3D& v) {return std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
}// 3D三点共线判定
bool areCollinear3D(const Point3D& A, const Point3D& B, const Point3D& C) {Vector3D AB(A, B);Vector3D AC(A, C);Vector3D cross = crossProduct(AB, AC);return magnitude(cross) < 1e-9;
}

🎮 CGAL 实现

#include <CGAL/Simple_cartesian.h>
#include <CGAL/collinear.h>typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_2 Point_2;
typedef Kernel::Point_3 Point_3;// 2D共线检测
bool checkCollinear2D() {Point_2 p1(0, 0);Point_2 p2(1, 1);Point_2 p3(2, 2);return CGAL::collinear(p1, p2, p3);
}// 3D共线检测
bool checkCollinear3D() {Point_3 p1(0, 0, 0);Point_3 p2(1, 1, 1);Point_3 p3(2, 2, 2);return CGAL::collinear(p1, p2, p3);
}

⚠️ 数值稳定性考虑

1. 浮点误差处理

const double EPSILON = 1e-9;bool isZero(double value) {return std::abs(value) < EPSILON;
}bool areCollinear_Robust(const Point2D& A, const Point2D& B, const Point2D& C) {double cross = (B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x);// 相对误差检查double maxCoord = std::max({std::abs(A.x), std::abs(A.y),std::abs(B.x), std::abs(B.y),std::abs(C.x), std::abs(C.y)});double relativeEpsilon = EPSILON * maxCoord;return std::abs(cross) < relativeEpsilon;
}

2. 精确算术

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>typedef CGAL::Exact_predicates_exact_constructions_kernel Exact_Kernel;
typedef Exact_Kernel::Point_2 Exact_Point_2;// 使用精确算术避免浮点误差
bool exactCollinearTest() {Exact_Point_2 p1(0, 0);Exact_Point_2 p2(1, 1);Exact_Point_2 p3(2, 2);return CGAL::collinear(p1, p2, p3);  // 精确结果
}

推荐选择

1. 精度要求高: 使用 CGAL 精确算术
2. 性能要求高: 使用叉积法
3. 理解简单: 使用斜率法（注意特殊情况）
4. 数值稳定: 使用相对误差检查

---

向量投影参数理论基础

🎯 基本概念

投影参数 t = (w · v) / (v · v) 是向量投影的核心，用于计算点到直线的最短距离问题。让我们逐步分析每个组成部分。

📊 几何背景

问题设定

• 线段: 由点 P1 和 P2 定义
• 目标点: M
• 目标: 找到线段上距离点 M 最近的点

向量定义

v = P2 - P1    // 线段的方向向量
w = M - P1     // 从线段起点到目标点的向量

🔍 投影参数 t 的几何意义

参数化表示

线段上任意一点可以表示为：

P(t) = P1 + t * v

其中：

• t = 0: 对应点 P1（线段起点）
• t = 1: 对应点 P2（线段终点）
• 0 < t < 1: 对应线段内部的点
• t < 0: 对应 P1 延长线上的点
• t > 1: 对应 P2 延长线上的点

📐 推导过程

步骤1: 最短距离条件

点 M 到线段上点 P(t) 的距离最短时，向量 (M - P(t)) 必须垂直于方向向量 v。

步骤2: 垂直条件

(M - P(t)) ⊥ v
⟺ (M - P(t)) · v = 0

步骤3: 展开计算

P(t) = P1 + t * v
M - P(t) = M - P1 - t * v = w - t * v垂直条件：
(w - t * v) · v = 0
w · v - t * (v · v) = 0
t * (v · v) = w · v
t = (w · v) / (v · v)

🧮 数学解释

点积的几何意义

w · v = |w| * |v| * cos(θ)

其中 θ 是向量 w 和 v 之间的夹角。

投影长度

投影长度 = |w| * cos(θ) = (w · v) / |v|

参数化系数

t = 投影长度 / |v| = (w · v) / |v|² = (w · v) / (v · v)

📈 可视化理解

      M|\| \|  \|   \|    \
P1----+-----P2↑投影点P(t)

• w: 从 P1 指向 M 的向量
• v: 从 P1 指向 P2 的向量
• 投影点: w 在 v 方向上的投影
• t: 投影点在线段上的相对位置

💻 具体计算示例

示例数据

Point P1(1, 1, 1);
Point P2(10, 10, 10);
Point M(5, 9, 7);// 计算向量
Vector v = P2 - P1;  // v = (9, 9, 9)
Vector w = M - P1;   // w = (4, 8, 6)

计算过程

// 1. 计算点积
double w_dot_v = w.x*v.x + w.y*v.y + w.z*v.z;
// w_dot_v = 4*9 + 8*9 + 6*9 = 36 + 72 + 54 = 162double v_dot_v = v.x*v.x + v.y*v.y + v.z*v.z;
// v_dot_v = 9*9 + 9*9 + 9*9 = 81 + 81 + 81 = 243// 2. 计算投影参数
double t = w_dot_v / v_dot_v;
// t = 162 / 243 = 2/3 ≈ 0.667// 3. 计算投影点
Point projection = P1 + t * v;
// projection = (1,1,1) + (2/3)*(9,9,9) = (1,1,1) + (6,6,6) = (7,7,7)

🔧 代码实现

C++ 实现

struct Vector3D {double x, y, z;Vector3D(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}// 点积运算double dot(const Vector3D& other) const {return x * other.x + y * other.y + z * other.z;}// 向量加法Vector3D operator+(const Vector3D& other) const {return Vector3D(x + other.x, y + other.y, z + other.z);}// 向量减法Vector3D operator-(const Vector3D& other) const {return Vector3D(x - other.x, y - other.y, z - other.z);}// 标量乘法Vector3D operator*(double scalar) const {return Vector3D(x * scalar, y * scalar, z * scalar);}
};// 计算投影参数
double calculateProjectionParameter(const Vector3D& P1, const Vector3D& P2, const Vector3D& M) {Vector3D v = P2 - P1;  // 方向向量Vector3D w = M - P1;   // 从起点到目标点的向量double w_dot_v = w.dot(v);double v_dot_v = v.dot(v);// 处理退化情况（零长度线段）if (v_dot_v == 0) {return 0;  // 线段退化为点，返回起点}return w_dot_v / v_dot_v;
}// 计算投影点
Vector3D calculateProjectionPoint(const Vector3D& P1, const Vector3D& P2, const Vector3D& M) {double t = calculateProjectionParameter(P1, P2, M);Vector3D v = P2 - P1;return P1 + v * t;
}

🎯 参数 t 的分类讨论

情况分析

void analyzeProjection(double t) {if (t < 0) {std::cout << "投影在线段外（P1侧），最近点是P1" << std::endl;} else if (t > 1) {std::cout << "投影在线段外（P2侧），最近点是P2" << std::endl;} else {std::cout << "投影在线段内，最近点是P(" << t << ")" << std::endl;}
}

完整的距离计算

double pointToSegmentDistance(const Vector3D& P1, const Vector3D& P2, const Vector3D& M) {double t = calculateProjectionParameter(P1, P2, M);Vector3D closestPoint;if (t < 0) {closestPoint = P1;  // 最近点是P1} else if (t > 1) {closestPoint = P2;  // 最近点是P2} else {Vector3D v = P2 - P1;closestPoint = P1 + v * t;  // 最近点在线段上}Vector3D diff = M - closestPoint;return std::sqrt(diff.dot(diff));
}

🔍 数值稳定性考虑

浮点精度处理

const double EPSILON = 1e-12;double robustProjectionParameter(const Vector3D& P1, const Vector3D& P2, const Vector3D& M) {Vector3D v = P2 - P1;Vector3D w = M - P1;double v_dot_v = v.dot(v);// 检查退化情况if (v_dot_v < EPSILON) {return 0;  // 线段长度接近零}double w_dot_v = w.dot(v);return w_dot_v / v_dot_v;
}

---

实际应用案例

🛠️ NC路径生成中的应用

1. 碰撞检测

// 检查刀具路径是否与障碍物发生碰撞
bool checkToolPathCollision(const std::vector<Point3D>& toolPath, const std::vector<Segment3D>& obstacles,double safetyDistance) {for (size_t i = 0; i < toolPath.size() - 1; ++i) {Segment3D pathSegment(toolPath[i], toolPath[i+1]);for (const auto& obstacle : obstacles) {double distance = CGAL::sqrt(CGAL::squared_distance(pathSegment, obstacle));if (distance < safetyDistance) {return true;  // 发生碰撞}}}return false;
}

2. 路径优化

// 使用三点共线原理简化刀具路径
std::vector<Point3D> optimizeToolPath(const std::vector<Point3D>& originalPath, double tolerance) {if (originalPath.size() <= 2) return originalPath;std::vector<Point3D> optimizedPath;optimizedPath.push_back(originalPath[0]);for (size_t i = 1; i < originalPath.size() - 1; ++i) {// 检查当前点是否可以被前后点的直线代替double distance = pointToSegmentDistance(originalPath[i-1], originalPath[i+1], originalPath[i]);if (distance > tolerance) {optimizedPath.push_back(originalPath[i]);}}optimizedPath.push_back(originalPath.back());return optimizedPath;
}

3. 干涉检查

// 检查刀具与工件的干涉
bool checkInterference(const Point3D& toolPosition, const std::vector<Triangle3D>& workpiece,double toolRadius) {for (const auto& triangle : workpiece) {// 检查点到三角形的距离for (int i = 0; i < 3; ++i) {Point3D p1 = triangle.vertex(i);Point3D p2 = triangle.vertex((i+1)%3);double distance = pointToSegmentDistance(p1, p2, toolPosition);if (distance < toolRadius) {return true;  // 发生干涉}}}return false;
}

🎮 计算机图形学应用

1. 线段简化

// Douglas-Peucker 算法实现
std::vector<Point2D> douglasPeucker(const std::vector<Point2D>& points, double epsilon) {if (points.size() <= 2) return points;// 找到距离首尾连线最远的点double maxDistance = 0;size_t maxIndex = 0;for (size_t i = 1; i < points.size() - 1; ++i) {double distance = pointToSegmentDistance(points[0], points.back(), points[i]);if (distance > maxDistance) {maxDistance = distance;maxIndex = i;}}std::vector<Point2D> result;if (maxDistance > epsilon) {// 递归处理两段std::vector<Point2D> left(points.begin(), points.begin() + maxIndex + 1);std::vector<Point2D> right(points.begin() + maxIndex, points.end());auto leftResult = douglasPeucker(left, epsilon);auto rightResult = douglasPeucker(right, epsilon);result.insert(result.end(), leftResult.begin(), leftResult.end());result.insert(result.end(), rightResult.begin() + 1, rightResult.end());} else {result.push_back(points[0]);result.push_back(points.back());}return result;
}

2. 最近邻搜索

// 找到距离查询点最近的线段
struct NearestSegmentResult {size_t segmentIndex;double distance;Point3D closestPoint;
};NearestSegmentResult findNearestSegment(const Point3D& queryPoint,const std::vector<Segment3D>& segments) {NearestSegmentResult result;result.distance = std::numeric_limits<double>::max();for (size_t i = 0; i < segments.size(); ++i) {double distance = CGAL::sqrt(CGAL::squared_distance(segments[i], queryPoint));if (distance < result.distance) {result.distance = distance;result.segmentIndex = i;// 计算最近点double t = calculateProjectionParameter(segments[i].source(), segments[i].target(), queryPoint);t = std::max(0.0, std::min(1.0, t));result.closestPoint = segments[i].source() + t * (segments[i].target() - segments[i].source());}}return result;
}

🤖 机器人路径规划

1. 路径平滑

// 使用三点共线原理平滑机器人路径
std::vector<Point2D> smoothPath(const std::vector<Point2D>& rawPath,double smoothingFactor) {if (rawPath.size() <= 2) return rawPath;std::vector<Point2D> smoothedPath;smoothedPath.push_back(rawPath[0]);for (size_t i = 1; i < rawPath.size() - 1; ++i) {// 检查是否需要保留当前点if (!areCollinear_CrossProduct(rawPath[i-1], rawPath[i], rawPath[i+1])) {// 应用平滑因子Point2D smoothedPoint;smoothedPoint.x = rawPath[i].x * (1 - smoothingFactor) +(rawPath[i-1].x + rawPath[i+1].x) * smoothingFactor / 2;smoothedPoint.y = rawPath[i].y * (1 - smoothingFactor) +(rawPath[i-1].y + rawPath[i+1].y) * smoothingFactor / 2;smoothedPath.push_back(smoothedPoint);}}smoothedPath.push_back(rawPath.back());return smoothedPath;
}

2. 障碍物避让

// 检查路径段是否与圆形障碍物相交
bool pathIntersectsCircle(const Point2D& start, const Point2D& end,const Point2D& circleCenter, double radius) {double distance = pointToSegmentDistance(start, end, circleCenter);return distance <= radius;
}// 生成避让路径
std::vector<Point2D> generateAvoidancePath(const Point2D& start, const Point2D& goal,const std::vector<Circle2D>& obstacles) {std::vector<Point2D> path;// 检查直线路径是否可行bool directPathClear = true;for (const auto& obstacle : obstacles) {if (pathIntersectsCircle(start, goal, obstacle.center, obstacle.radius)) {directPathClear = false;break;}}if (directPathClear) {path.push_back(start);path.push_back(goal);} else {// 实现更复杂的避让算法// ...}return path;
}