空间圆弧路径参数化

一、问题描述

  给定空间不共线的三个点$A,B,C$，推导空间有向圆弧路径$ABC$关于路径标量$s(s\in [0,1])$的参数方程。

二、推导步骤

  假设点$O$为圆弧$ABC$的圆心，如图，建立局部坐标系 { X ′ O Y ′ } \{X'OY'\} { X′OY′}。连结$AB$、$BC$，过点$O$作平面$n$垂直平分于线段$AB$于点$D$，过点$O$作平面$m$垂直平分于线段$BC$于点$E$。平面$n$、平面$m$、平面$ABC$三个平面交点即为圆弧$ABC$的圆心$O(x_0,y_0,z_0)$。
  设点坐标$A(x_1,y_1,z_1),B(x_2,y_2,z_2),C(x_3,y_3,z_3),D(x_4,y_4,z_4)$,$E(x_5,y_5,z_5)$。
  联立平面$n$、平面$m$、平面$ABC$的方程：
$$\{\begin{array}{l}\overrightarrow{AB}\cdot \overrightarrow{DO}=0\\ \overrightarrow{BC}\cdot \overrightarrow{EO}=0\\ (\overrightarrow{AB}\times \overrightarrow{BC})\cdot \overrightarrow{CO}=0\end{array}\text{\hspace{1em}(1)}$$
  便可求得圆心$O(x_0,y_0,z_0)$。
  进而计算半径：
$$R=||\overrightarrow{OA}||\text{\hspace{1em}(2)}$$
  设坐标系 { X ′ O Y ′ } \{X'OY'\} { X′OY′}到坐标系 { X Y Z } \{XYZ\} { XYZ}的旋转变换矩阵为：
$$\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}\vec{u}& \vec{v}& \vec{w}\end{array}\right]\text{\hspace{1em}(3)}$$
  根据旋转变换矩阵定义，容易得到：
$$\vec{u}=\overrightarrow{OA}/R\text{\hspace{1em}(4)}$$
  $w$为平面$ABC$的单位法向量：
$$\vec{w}=(\overrightarrow{AB}\times \overrightarrow{BC})/||\overrightarrow{AB}\times \overrightarrow{BC}||\text{\hspace{1em}(5)}$$
  根据右手法则：
$$\vec{v}=\vec{w}\times \vec{u}\text{\hspace{1em}(6)}$$
  圆弧圆心角：
$$\theta =atan2(\overrightarrow{OC}\cdot \vec{v},\overrightarrow{OC}\cdot \vec{u})\text{\hspace{1em}(7)}$$
  若$\theta <0$，则$\theta =\theta +2\pi$。
  圆弧$ABC$在局部坐标系 { X ′ O Y ′ } \{X'OY'\} { X′OY′}的参数方程：
$$\{\begin{array}{l}{x}_{temp}=Rcos(s\theta )\\ y_{temp}=Rsin(s\theta )\\ z_{temp}=0\end{array}\text{\hspace{1em}(8)}$$
  其中，$s\in [0,1]$。
  利用齐次变换矩阵，将圆弧$ABC$从局部坐标系 { X ′ O Y ′ } \{X'OY'\} { X′OY′}变换至坐标系 { X Y Z } \{XYZ\} { XYZ}：
$$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& x_0\\ r_{21}& r_{22}& r_{23}& y_0\\ r_{31}& r_{32}& r_{33}& z_0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{temp}\\ y_{temp}\\ z_{temp}\\ 1\end{array}\right]\text{\hspace{1em}(9)}$$
  进一步写成：
$$\{\begin{array}{l}x(s)=r_{11}{x}_{temp}(s)+r_{12}{y}_{temp}(s)+x_0\\ y(s)=r_{21}{x}_{temp}(s)+r_{22}{y}_{temp}(s)+y_0\\ z(s)=r_{31}{x}_{temp}(s)+r_{32}{y}_{temp}(s)+z_0\end{array}\text{\hspace{1em}(10)}$$

三、MATLAB代码

%{
    
    
Function: solve_spatial_arc_params
Description: 求三点空间圆弧路径参数
Input: 空间三个点startPos, midPos, endPos
Output: 空间圆弧圆心arcCenter, 半径radius, 圆心角arcCentralAngle, 
        旋转变换矩阵rotateMatrix, 求解状态status(1表示成功，0表示失败)
Author: Marc Pony(marc_pony@163.com)
%}
function [arcCenter, radius, arcCentralAngle, rotateMatrix, status] = solve_spatial_arc_params(startPos, midPos, endPos)
x1 = startPos(1);
y1 = startPos(2);
z1 = startPos(3);
x2 = midPos(1);
y2 = midPos(2);
z2 = midPos(3);
x3 = endPos(1);
y3 = endPos(2);
z3 = endPos(3);

arcCenter = zeros(3, 1);
radius = 0.0;
arcCentralAngle = 0.0;
rotateMatrix = zeros(3, 3);
status = 1;

if abs((y1 - y2) * (z2 - z3) - (y2 - y3) * (z1 - z2)) < 1.0e-8 ...
   && abs((x2 - x3) * (z1 - z2) - (x1 - x2) * (z2 - z3)) < 1.0e-8 ...
   && abs((x1 - x2) * (y2 - y3) - (x2 - x3) * (y1 - y2)) < 1.0e-8
    status = 0;
    return;
else
    x4 = 0.5 * (x1 + x2);
    y4 = 0.5 * (y1 + y2);
    z4 = 0.5 * (z1 + z2);
    
    x5 = 0.5 * (x2 + x3);
    y5 = 0.5 * (y2 + y3);
    z5 = 0.5 * (z2 + z3);
    
    a11 = x2 - x1;
    a12 = y2 - y1;
    a13 = z2 - z1;
    b1 = x4 * a11 + y4 * a12 + z4 * a13;
    
    a21 = x3 - x2;
    a22 = y3 - y2;
    a23 = z3 - z2;
    b2 = x5 * a21 + y5 * a22 + z5 * a23;
    
    a31 = (y1 - y2) * (z2 - z3) - (y2 - y3) * (z1 - z2);
    a32 = (x2 - x3) * (z1 - z2) - (x1 - x2) * (z2 - z3);
    a33 = (x1 - x2) * (y2 - y3) - (x2 - x3) * (y1 - y2);
    b3 = x3 * a31 + y3 * a32 + z3 * a33;
    
    temp = a11 * (a22 * a33 - a23 * a32) + a12 * (a23 * a31 - a21 * a33) + a13 * (a21 * a32 - a22 * a31);
    x0 = ((a12 * a23 - a13 * a22) * b3 + (a13 * a32 - a12 * a33) * b2 + (a22 * a33 - a23 * a32) * b1) / temp;
    y0 = -((a11 * a23 - a13 * a21) * b3 + (a13 * a31 - a11 * a33) * b2 + (a21 * a33 - a23 * a31) * b1) / temp;
    z0 = ((a11 * a22 - a12 * a21) * b3 + (a12 * a31 - a11 * a32) * b2 + (a21 * a32 - a22 * a31) * b1) / temp;
    radius = sqrt((x0 - x1)^2 + (y0 - y1)^2 + (z0 - z1)^2);
    
    r11 = (x1 - x0) / radius;
    r21 = (y1 - y0) / radius;
    r31 = (z1 - z0) / radius;
    
    temp1 = (y1 - y2) * (z2 - z3) - (y2 - y3) * (z1 - z2);
    temp2 = (x2 - x3) * (z1 - z2) - (x1 - x2) * (z2 - z3);
    temp3 = (x1 - x2) * (y2 - y3) - (x2 - x3) * (y1 - y2);
    vectorZLength = sqrt(temp1 * temp1 + temp2 * temp2 + temp3 * temp3);
    r13 = temp1 / vectorZLength;
    r23 = temp2 / vectorZLength;
    r33 = temp3 / vectorZLength;
    
    r12 = r23 * r31 - r21 * r33;
    r22 = r11 * r33 - r13 * r31;
    r32 = r13 * r21 - r11 * r23;
    
    arcCentralAngle = atan2((x3 - x0) * r12 + (y3 - y0) * r22 + (z3 - z0) * r32, ...
                      (x3 - x0) * r11 + (y3 - y0) * r21 + (z3 - z0) * r31);
    if  arcCentralAngle < 0.0
        arcCentralAngle = arcCentralAngle + 2.0 * pi;
    end
    arcCenter = [x0; y0; z0];
    rotateMatrix = [r11, r12, r13; r21, r22, r23; r31, r32, r33];
end
end

clc
clear
close all

startPos = [12, 7, 20]'; %圆弧起点
midPos = [0, 12, 10]';   %圆弧中间点
endPos = [8, 0, 10]';    %圆弧终点

[arcCenter, radius, arcCentralAngle, rotateMatrix, status] = solve_spatial_arc_params(startPos, midPos, endPos);

count = 1000;
s = linspace(0, 1, count)';
xs = radius * cos(s * arcCentralAngle);
ys = radius * sin(s * arcCentralAngle);
x = zeros(count, 1);
y = zeros(count, 1);
z = zeros(count, 1);
for i = 1 : count
    x(i) = rotateMatrix(1, 1) * xs(i) + rotateMatrix(1, 2) * ys(i) + arcCenter(1);
    y(i) = rotateMatrix(2, 1) * xs(i) + rotateMatrix(2, 2) * ys(i) + arcCenter(2);
    z(i) = rotateMatrix(3, 1) * xs(i) + rotateMatrix(3, 2) * ys(i) + arcCenter(3);
end

figure('color', 'w')
plot3([startPos(1), midPos(1), endPos(1)], [startPos(2), midPos(2), endPos(2)], [startPos(3), midPos(3), endPos(3)], 'o')
hold on
plot3(x, y, z, 'r')
xlabel('x')
ylabel('y')
zlabel('z')
axis equal