一、问题描述
给定空间不共线的三个点 A , B , C A,B,C A,B,C,推导空间有向圆弧路径 A B C ABC ABC关于路径标量 s ( s ∈ [ 0 , 1 ] ) s(s\in[0,1]) s(s∈[0,1])的参数方程。
![](https://img-blog.csdnimg.cn/20201028213433323.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21hcGxlXzIwMTQ=,size_16,color_FFFFFF,t_70#pic_center)
二、推导步骤
假设点
O
O
O为圆弧
A
B
C
ABC
ABC的圆心,如图,建立局部坐标系
{
X
′
O
Y
′
}
\{X'OY'\}
{X′OY′}。连结
A
B
AB
AB、
B
C
BC
BC,过点
O
O
O作平面
n
n
n垂直平分于线段
A
B
AB
AB于点
D
D
D,过点
O
O
O作平面
m
m
m垂直平分于线段
B
C
BC
BC于点
E
E
E。平面
n
n
n、平面
m
m
m、平面
A
B
C
ABC
ABC三个平面交点即为圆弧
A
B
C
ABC
ABC的圆心
O
(
x
0
,
y
0
,
z
0
)
O(x_0,y_0,z_0)
O(x0,y0,z0)。
设点坐标
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
)
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)
A(x1,y1,z1),B(x2,y2,z2),C(x3,y3,z3),D(x4,y4,z4),
E
(
x
5
,
y
5
,
z
5
)
E(x_5,y_5,z_5)
E(x5,y5,z5)。
联立平面
n
n
n、平面
m
m
m、平面
A
B
C
ABC
ABC的方程:
{
A
B
⃗
⋅
D
O
⃗
=
0
B
C
⃗
⋅
E
O
⃗
=
0
(
A
B
⃗
×
B
C
⃗
)
⋅
C
O
⃗
=
0
(1)
\begin{cases} \vec{AB}\cdot\vec{DO}=0\\ \vec{BC}\cdot\vec{EO}=0\\ (\vec{AB}\times\vec{BC})\cdot\vec{CO}=0\\ \tag 1 \end{cases}
⎩⎪⎨⎪⎧AB⋅DO=0BC⋅EO=0(AB×BC)⋅CO=0(1)
便可求得圆心
O
(
x
0
,
y
0
,
z
0
)
O(x_0,y_0,z_0)
O(x0,y0,z0)。
进而计算半径:
R
=
∣
∣
O
A
⃗
∣
∣
(2)
R =||\vec{OA }||\tag 2
R=∣∣OA∣∣(2)
设坐标系
{
X
′
O
Y
′
}
\{X'OY'\}
{X′OY′}到坐标系
{
X
Y
Z
}
\{XYZ\}
{XYZ}的旋转变换矩阵为:
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
[
u
⃗
v
⃗
w
⃗
]
(3)
\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{matrix} \right] =\left[ \begin{matrix} \vec{u} &\vec {v} & \vec{w }\\ \end{matrix} \right] \tag 3
⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤=[uvw](3)
根据旋转变换矩阵定义,容易得到:
u
⃗
=
O
A
⃗
/
R
(4)
\vec{u} =\vec{OA}/R\tag 4
u=OA/R(4)
w
w
w为平面
A
B
C
ABC
ABC的单位法向量:
w
⃗
=
(
A
B
⃗
×
B
C
⃗
)
/
∣
∣
A
B
⃗
×
B
C
⃗
∣
∣
(5)
\vec{w}=(\vec{AB}\times\vec{BC}) / ||\vec{AB}\times\vec{BC}||\tag 5
w=(AB×BC)/∣∣AB×BC∣∣(5)
根据右手法则:
v
⃗
=
w
⃗
×
u
⃗
(6)
\vec{v}=\vec{w}\times\vec{u}\tag 6
v=w×u(6)
圆弧圆心角:
θ
=
a
t
a
n
2
(
O
C
⃗
⋅
v
⃗
,
O
C
⃗
⋅
u
⃗
)
(7)
\theta=atan2(\vec{OC}\cdot\vec{v},\vec{OC}\cdot\vec{u})\tag 7
θ=atan2(OC⋅v,OC⋅u)(7)
若
θ
<
0
\theta<0
θ<0,则
θ
=
θ
+
2
π
\theta=\theta+2\pi
θ=θ+2π。
圆弧
A
B
C
ABC
ABC在局部坐标系
{
X
′
O
Y
′
}
\{X'OY'\}
{X′OY′}的参数方程:
{
x
t
e
m
p
=
R
c
o
s
(
s
θ
)
y
t
e
m
p
=
R
s
i
n
(
s
θ
)
z
t
e
m
p
=
0
(8)
\begin{cases} x_{temp}=Rcos(s\theta)\\ y_{temp}=Rsin(s\theta)\\ z_{temp}=0\\ \tag 8 \end{cases}
⎩⎪⎨⎪⎧xtemp=Rcos(sθ)ytemp=Rsin(sθ)ztemp=0(8)
其中,
s
∈
[
0
,
1
]
s\in[0,1]
s∈[0,1]。
利用齐次变换矩阵,将圆弧
A
B
C
ABC
ABC从局部坐标系
{
X
′
O
Y
′
}
\{X'OY'\}
{X′OY′}变换至坐标系
{
X
Y
Z
}
\{XYZ\}
{XYZ}:
[
x
y
z
1
]
=
[
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
]
[
x
t
e
m
p
y
t
e
m
p
z
t
e
m
p
1
]
(9)
\left[ \begin{matrix} x \\ y \\ z \\ 1\\ \end{matrix} \right] = \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} x_{temp}\\ y_{temp}\\ z_{temp}\\ 1\\ \end{matrix} \right] \tag 9
⎣⎢⎢⎡xyz1⎦⎥⎥⎤=⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330x0y0z01⎦⎥⎥⎤⎣⎢⎢⎡xtempytempztemp1⎦⎥⎥⎤(9)
进一步写成:
{
x
(
s
)
=
r
11
x
t
e
m
p
(
s
)
+
r
12
y
t
e
m
p
(
s
)
+
x
0
y
(
s
)
=
r
21
x
t
e
m
p
(
s
)
+
r
22
y
t
e
m
p
(
s
)
+
y
0
z
(
s
)
=
r
31
x
t
e
m
p
(
s
)
+
r
32
y
t
e
m
p
(
s
)
+
z
0
(10)
\begin{cases} 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\\ \tag {10} \end{cases}
⎩⎪⎨⎪⎧x(s)=r11xtemp(s)+r12ytemp(s)+x0y(s)=r21xtemp(s)+r22ytemp(s)+y0z(s)=r31xtemp(s)+r32ytemp(s)+z0(10)
![](https://img-blog.csdnimg.cn/20201028220517453.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21hcGxlXzIwMTQ=,size_16,color_FFFFFF,t_70#pic_center)
三、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