常见几何计算问题及代码实现

计算向量叉乘

$$\begin{aligned} & \vec{a}=\left(x_1, y_1, z_1\right) \\ & \vec{b}=\left(x_2, y_2, z_2\right) \\ & \vec{c} = \vec{a} \times \vec{b}=\left|\begin{array}{ccc} \vec{i} & \vec{j} & \vec{k} \\ x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \end{array}\right| = \begin{bmatrix}y_1z_2-y_2z_1 & x_2z_1-x_1z_2 & x_1y_2-x_2y_1\end{bmatrix} \end{aligned}$$

C++

std::vector<double> CrossProduct3D(std::vector<double>& a,
                                   std::vector<double>& b) {
  if (a.size() != 3 || b.size() != 3) {
    std::cout << "Error: Vectors must be of size 3\n";
  }
  const double x_part = a[1] * b[2] - a[2] * b[1];
  const double y_part = a[2] * b[0] - a[0] * b[2];
  const double z_part = a[0] * b[1] - a[1] * b[0];
  return std::vector<double>{x_part, y_part, z_part};
}

Python

def cross_product(a, b):
    if len(a) != 3 or len(b) != 3:
        print("Error: Vectors must be of size 3")
    x = a[1] * b[2] - a[2] * b[1]
    y = a[2] * b[0] - a[0] * b[2]
    z = a[0] * b[1] - a[1] * b[0]
    return [x, y, z]

已知三点计算三角形的面积

思路

• 向量叉乘的几何意义是两个向量构成的平行四边形的面积，因此三角形的面积等于两个向量的叉乘的绝对值除以 2。

C++

double TriangleArea(std::vector<double>& point_A, std::vector<double>& point_B,
                    std::vector<double>& point_C) {
  if (point_A.size() != 2 || point_B.size() != 2 || point_C.size() != 2) {
    std::cout << "Error: Vectors must be of size 2\n";
  }
  const std::vector<double> ab = {point_B[0] - point_A[0],
                                  point_B[1] - point_A[1]};
  const std::vector<double> ac = {point_C[0] - point_A[0],
                                  point_C[1] - point_A[1]};
  return std::abs(ab[0] * ac[1] - ab[1] * ac[0]) / 2;
}

Python

判断点是否在三角形内

思路

• 点在三角形内，分割成的三个三角形面积之和等于原三角形的面积。

计算点到直线距离

思路

• 给定点$P = [x_0, y_0, z_0]$，和直线上的两个点$A = [x_1, y_1, z_1]$和$B = [x_2, y_2, z_2]$，
  直线的方向向量为$\vec{AB} = [x_2 - x_1, y_2 - y_1, z_2 - z_1]$，
  则点到直线的距离

  $$d =\frac{||AP \times AB||}{||AB||}$$

C++

std::vector<double> CrossProduct3D(const std::vector<double>& a,
                                   const std::vector<double>& b) {
  if (a.size() != 3 || b.size() != 3) {
    std::cout << "Error: Vectors must be of size 3\n";
  }
  const double x_part = a[1] * b[2] - a[2] * b[1];
  const double y_part = a[2] * b[0] - a[0] * b[2];
  const double z_part = a[0] * b[1] - a[1] * b[0];
  return std::vector<double>{x_part, y_part, z_part};
}

double GetDistanceFromPointToLine(std::vector<double>& point_P,
                                  std::vector<double>& point_A,
                                  std::vector<double>& point_B) {
  if (point_P.size() != 3 || point_A.size() != 3 || point_B.size() != 3) {
    std::cout << "Error: Vectors must be of size 2\n";
  }
  const std::vector<double> vector_ap = {point_P[0] - point_A[0],
                                         point_P[1] - point_A[1],
                                         point_P[2] - point_A[2]};
  const std::vector<double> vector_ab = {point_B[0] - point_A[0],
                                         point_B[1] - point_A[1],
                                         point_B[2] - point_A[2]};

  const auto cross_product = CrossProduct3D(vector_ab, vector_ap);
  const double vector_ap_length =
      std::sqrt(vector_ap[0] * vector_ap[0] + vector_ap[1] * vector_ap[1] +
                vector_ap[2] * vector_ap[2]);
  if (vector_ap_length == 0) {
    return 0;
  }
  const double area = std::sqrt(cross_product[0] * cross_product[0] +
                                cross_product[1] * cross_product[1] +
                                cross_product[2] * cross_product[2]);
  return area / vector_ap_length;
}

Python

def GetDistanceFromPointToLine(point_p, point_line_A, point_line_B):
    if (len(point_p) != len(point_line_A)) or (len(point_p) != len(point_line_B)):
        raise ValueError("Points must have the same dimension")
    vector_ap = np.array(point_p - point_line_A)
    vector_ab = np.array(point_line_B - point_line_A)
    ap_cross_bp = np.cross(vector_ap, vector_ab)
    return np.linalg.norm(ap_cross_bp) / np.linalg(vector_ab)

计算点到平面距离

思路

• 平面的标准方程

  $$Ax + By + Cz + D = 0$$

• 其法向量为$\vec{n} = [A, B, C]$，对于任一点$P = [x_0, y_0, z_0]$，到平面的距离为

  $$d = \frac{|Ax_0 + By_0 + Cz_0 + D|}{||\vec{n}||}$$

C++

double GetDistanceFromPointToPlane(const std::vector<double>& point_P,
                                   const std::vector<double>& plane_coeffs) {
  const double numerator =
      std::abs(plane_coeffs[0] * point_P[0] + plane_coeffs[1] * point_P[1] +
               plane_coeffs[2] * point_P[2] + plane_coeffs[3]);

  const double denominator = std::sqrt(plane_coeffs[0] * plane_coeffs[0] +
                                       plane_coeffs[1] * plane_coeffs[1] +
                                       plane_coeffs[2] * plane_coeffs[2]);

  return numerator / denominator;
}

Python

一组二维点拟合直线

• 给一组二维点$(x_i, y_i)$，希望找到一条直线$y = ax + b$，使得预测值和实际数据点的误差最小，即：

  $$\min_{a,b} \sum_{i=1}^n (y_i - (ax_i + b))^2$$

• 高斯牛顿法步骤：

  • 选择初始参数：选择直线参数$a_0$和$b_0$的初始猜测
  • 计算残差：计算每个点的残差值$r_i = y_i - (ax_i + b)$
  • 计算雅可比矩阵：残差对参数 a 和 b 的偏导数
  • 更新参数
  • 迭代直到收敛

C++

// 高斯牛顿法进行拟合
void gaussNewtonFit(const vector<pair<double, double>>& points, double& a,
                    double& b, int maxIterations = 100, double tol = 1e-6) {
  int n = points.size();
  double lambda = 1e-3;  // 正则化参数，防止矩阵不可逆
  for (int iter = 0; iter < maxIterations; ++iter) {
    // 构造残差向量和雅可比矩阵
    VectorXd residuals(n);
    MatrixXd J(n, 2);  // 雅可比矩阵

    for (int i = 0; i < n; ++i) {
      double x = points[i].first;
      double y = points[i].second;
      double prediction = a * x + b;

      residuals(i) = y - prediction;  // 计算残差
      J(i, 0) = -x;                   // a的偏导数
      J(i, 1) = -1;                   // b的偏导数
    }

    // 计算Hessian矩阵和梯度
    MatrixXd JTJ = J.transpose() * J;
    VectorXd JT_residual = J.transpose() * residuals;

    // 高斯牛顿更新
    // 增量
    VectorXd delta = (JTJ + lambda * MatrixXd::Identity(2, 2))
                         .ldlt()
                         .solve(-JT_residual);

    // 更新参数
    a += delta(0);
    b += delta(1);

    // 检查收敛
    if (delta.norm() < tol) {
      cout << "Converged after " << iter + 1 << " iterations." << endl;
      break;
    }
  }
}

一组二维点拟合圆

• 假设有一组二维点 $(x_i, y_i)(i=1,2,...,n)$，目标是找到一个圆的参数，使得这些点与圆的距离之和最小。一个圆的标准方程为：

  $$(x-a)^2 + (y-b)^2 = r^2$$

• 我们希望最小化以下误差函数的平方和

  $$F(a,b,r) = \sum^n_{i=1}(\sqrt{(x_i-a)^2 + (y_i-b)^2 }-r)^2$$

  这是个非线性方程，用高斯牛顿法迭代求解
• 高斯牛顿法步骤
  • 起始迭代值：选择圆心 $(a_0, b_0)$和半径 $r_0$的初始猜测
  • 计算残差：对每个点 $i$，计算残差函数 $r_i$：

    $$r_i(a,b,r) = \sqrt{(x_i-a)^2 + (y_i-b)^2 }-r$$

  • 计算雅可比矩阵：计算残差函数相对参数 $(a,b,r)$的偏导数，形成雅可比矩阵 $J$：

    $$J_{ij} = \frac{\partial r_i}{\partial \theta_i}$$

    $$J_{ia} = \frac{(a-x_i)}{\sqrt{(x_i-a)^2 + (y_i-b)^2 }}\\ J_{ib} = \frac{(b-y_i)}{\sqrt{(x_i-a)^2 + (y_i-b)^2 }} \\ J_{ir} = -1$$

  • 更新参数：根据高斯牛顿法的更新公式：

    $$\Delta \theta = -(J^T J)^{-1} J^Tr \\ \theta_{new} = \theta_{old} + \Delta \theta$$

  • 重复迭代，直到 $\Delta \theta$足够小

#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

struct Point {
    double x, y;
};

struct Circle {
    double a, b, r;
};

double residual(const Circle &circle, const Point &point) {
    return sqrt(pow(point.x - circle.a, 2) + pow(point.y - circle.b, 2)) - circle.r;
}

Circle fitCircle(const vector<Point> &points, Circle initial_guess, int max_iterations = 100, double tolerance = 1e-6) {
    Circle circle = initial_guess;

    for (int iter = 0; iter < max_iterations; ++iter) {
        int n = points.size();
        VectorXd r(n);
        MatrixXd J(n, 3);

        for (int i = 0; i < n; ++i) {
            double dx = points[i].x - circle.a;
            double dy = points[i].y - circle.b;
            double dist = sqrt(dx * dx + dy * dy);

            r(i) = dist - circle.r;

            J(i, 0) = -dx / dist;
            J(i, 1) = -dy / dist;
            J(i, 2) = -1.0;
        }

        VectorXd delta = (J.transpose() * J).ldlt().solve(-J.transpose() * r);

        circle.a += delta(0);
        circle.b += delta(1);
        circle.r += delta(2);

        if (delta.norm() < tolerance) break;
    }

    return circle;
}

int main() {
    vector<Point> points = { {1, 2}, {2, 3}, {3, 4}, {4, 5} };
    Circle initial_guess = {0, 0, 1};

    Circle fitted_circle = fitCircle(points, initial_guess);

    cout << "Fitted circle: center=(" << fitted_circle.a << ", " << fitted_circle.b << "), radius=" << fitted_circle.r << endl;

    return 0;
}

一个点和平面上三个点，计算点到平面的距离

• 根据平面上三个点，可以得到平面上的两个向量，向量叉乘得到平面的法向量。
• 法向量就对应平面的 ABC 参数，然后带入任意一个点，可以计算出 D 的参数值。
• 最终得到平面方程后，带入平面外的点，计算点到平面的距离。