9-5 三维坐标系转换
三维坐标系转换（3D Changing Coordinate Systems）
三维坐标系转换是二维坐标系转换的自然延伸。在三维空间中，一个坐标系（Coordinate Frame）由原点位置和三个基向量定义，使用 4x4 齐次坐标矩阵来描述坐标系之间的关系。三维场景中的物体通常在各自的本地坐标系中定义，通过坐标系转换矩阵链将本地坐标映射到世界坐标。
这是 3D 游戏引擎、机器人学（Robotics）、计算机辅助设计（CAD）和虚拟现实（VR）的核心基础。
三维坐标系（3D Coordinate Frame）
一个三维坐标系由以下要素定义：
- 原点（Origin）：坐标系原点在父级坐标系中的位置 (ox, oy, oz)
- 三个基向量（Basis Vectors）：x 轴、y 轴、z 轴的方向
三维坐标系矩阵（4x4）：
| îx ĵx k̂x ox |
| îy ĵy k̂y oy |
| îz ĵz k̂z oz |
| 0 0 0 1 |
- 前三列
[î, ĵ, k̂]是三个基向量，描述坐标轴的方向和缩放 - 第四列
[ox, oy, oz]是原点在父级坐标中的位置 - 第四行
[0, 0, 0, 1]是齐次坐标的固定行
右手坐标系约定：x 轴向右，y 轴向上，z 轴朝向观察者（屏幕外）。叉积 x × y = z。
右手坐标系：
y
|
|
+--- x
/
z (朝向观察者)
本地坐标到世界坐标（Local → World）
以一个具体例子说明：有一个本地坐标系，原点在世界坐标 (3, 0, 0)，绕 z 轴旋转了 90 度。
绕 z 轴旋转 90 度后：
- 本地 x 轴 → 世界 y 方向 (0, 1, 0)
- 本地 y 轴 → 世界 -x 方向 (-1, 0, 0)
- 本地 z 轴 → 不变 (0, 0, 1)
坐标系矩阵：
Frame = | 0 -1 0 3 |
| 1 0 0 0 |
| 0 0 1 0 |
| 0 0 0 1 |
将本地坐标 (2, 0, 0) 转换为世界坐标：
| 0 -1 0 3 | | 2 | | 0*2 + (-1)*0 + 0*0 + 3 | | 3 |
| 1 0 0 0 | × | 0 | = | 1*2 + 0*0 + 0*0 + 0 | = | 2 |
| 0 0 1 0 | | 0 | | 0*2 + 0*0 + 1*0 + 0 | | 0 |
| 0 0 0 1 | | 1 | | 1 | | 1 |
直观理解：本地 x 轴指向世界的 y 方向，所以沿本地 x 轴走 2 单位，在世界中是 y 方向 +2，再加上原点偏移 (3, 0, 0)，结果为 (3, 2, 0)。
世界坐标到本地坐标（World → Local）
使用坐标系矩阵的逆矩阵，将世界坐标转换回本地坐标：
P_local = Frame^(-1) × P_world
对于刚体变换（旋转 + 平移），逆矩阵的计算非常高效：
| R t |^(-1) | R^T -R^T × t |
| 0 1 | = | 0 1 |
其中 R^T 是旋转矩阵的转置（也是逆矩阵），不需要矩阵求逆。
以之前的例子，将世界坐标 (3, 2, 0) 转换回本地坐标：
Frame = | 0 -1 0 3 | R = | 0 -1 0 | t = | 3 |
| 1 0 0 0 | | 1 0 0 | | 0 |
| 0 0 1 0 | | 0 0 1 | | 0 |
| 0 0 0 1 |
R^T = | 0 1 0 | -R^T × t = -| 0 1 0 | × | 3 | = -| 0 | = | 0 |
|-1 0 0 | |-1 0 0 | | 0 | |-3 | | 3 |
| 0 0 1 | | 0 0 1 | | 0 | | 0 | | 0 |
Frame^(-1) = | 0 1 0 0 |
|-1 0 0 3 |
| 0 0 1 0 |
| 0 0 0 1 |
Frame^(-1) × (3, 2, 0, 1):
| 0 1 0 0 | | 3 | | 0*3 + 1*2 + 0*0 + 0 | | 2 |
|-1 0 0 3 | × | 2 | = |-1*3 + 0*2 + 0*0 + 3 | = | 0 |
| 0 0 1 0 | | 0 | | 0*3 + 0*2 + 1*0 + 0 | | 0 |
| 0 0 0 1 | | 1 | | 1 | | 1 |
结果为 (2, 0, 0)，与正向转换的输入完全一致。
嵌套三维坐标系（Nested 3D Frames）
与二维类似，三维坐标系可以层层嵌套。例如一个机器人手臂：
世界坐标系 (World)
└── 基座标系 (Base)
└── 肩关节坐标系 (Shoulder)
└── 肘关节坐标系 (Elbow)
└── 手腕坐标系 (Wrist)
└── 手指坐标系 (Finger)
手指尖的世界坐标 = M_base × M_shoulder × M_elbow × M_wrist × M_finger × P_finger_local
每一层坐标系只描述自己相对于父级的关系（原点偏移 + 旋转角度），整个链式转换自动处理。
完整实现
以下代码实现一个完整的三维坐标系转换库，支持创建坐标系、本地↔世界转换、嵌套坐标系链式转换。最后用一个机器人手臂的例子演示。
C++ 实现
#include <iostream>
#include <cmath>
#include <iomanip>
using namespace std;
// 4x4 matrix for 3D coordinate frames
struct Frame3D {
double m[4][4];
// Create frame from origin (ox,oy,oz) and Euler angles (ax,ay,az)
// Rotation order: Rz(az) * Ry(ay) * Rx(ax)
static Frame3D fromOriginEuler(double ox, double oy, double oz,
double ax, double ay, double az) {
Frame3D f;
// Compute combined rotation R = Rz * Ry * Rx
double cx = cos(ax), sx = sin(ax);
double cy = cos(ay), sy = sin(ay);
double cz = cos(az), sz = sin(az);
// R = Rz * Ry * Rx (element-wise)
f.m[0][0] = cy*cz; f.m[0][1] = sx*sy*cz - cx*sz; f.m[0][2] = cx*sy*cz + sx*sz;
f.m[1][0] = cy*sz; f.m[1][1] = sx*sy*sz + cx*cz; f.m[1][2] = cx*sy*sz - sx*cz;
f.m[2][0] = -sy; f.m[2][1] = sx*cy; f.m[2][2] = cx*cy;
// Origin
f.m[0][3] = ox; f.m[1][3] = oy; f.m[2][3] = oz;
// Homogeneous row
f.m[3][0] = 0; f.m[3][1] = 0; f.m[3][2] = 0; f.m[3][3] = 1;
return f;
}
// Matrix multiply: this * other
Frame3D multiply(const Frame3D& other) const {
Frame3D result;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++) {
result.m[i][j] = 0;
for (int k = 0; k < 4; k++)
result.m[i][j] += m[i][k] * other.m[k][j];
}
return result;
}
// Local to world: P_world = Frame * P_local
void localToWorld(double lx, double ly, double lz,
double& wx, double& wy, double& wz) const {
wx = m[0][0]*lx + m[0][1]*ly + m[0][2]*lz + m[0][3];
wy = m[1][0]*lx + m[1][1]*ly + m[1][2]*lz + m[1][3];
wz = m[2][0]*lx + m[2][1]*ly + m[2][2]*lz + m[2][3];
}
// World to local: P_local = Frame^(-1) * P_world
// For rigid body transforms: inv = | R^T -R^T*t |
// | 0 1 |
void worldToLocal(double wx, double wy, double wz,
double& lx, double& ly, double& lz) const {
// Translate to frame origin
double dx = wx - m[0][3];
double dy = wy - m[1][3];
double dz = wz - m[2][3];
// Multiply by R^T (transpose of 3x3 upper-left)
lx = m[0][0]*dx + m[1][0]*dy + m[2][0]*dz;
ly = m[0][1]*dx + m[1][1]*dy + m[2][1]*dz;
lz = m[0][2]*dx + m[1][2]*dy + m[2][2]*dz;
}
};
int main() {
cout << fixed << setprecision(4);
// === Example 1: Simple frame ===
cout << "=== Simple Frame ===" << endl;
Frame3D frame = Frame3D::fromOriginEuler(3, 0, 0, 0, 0, M_PI/2);
double lx = 2, ly = 0, lz = 0;
double wx, wy, wz;
frame.localToWorld(lx, ly, lz, wx, wy, wz);
cout << "Local (" << lx << ", " << ly << ", " << lz << ")"
<< " -> World (" << wx << ", " << wy << ", " << wz << ")" << endl;
double lx2, ly2, lz2;
frame.worldToLocal(wx, wy, wz, lx2, ly2, lz2);
cout << "World (" << wx << ", " << wy << ", " << wz << ")"
<< " -> Local (" << lx2 << ", " << ly2 << ", " << lz2 << ")" << endl;
// === Example 2: Robot arm (nested frames) ===
cout << "\n=== Robot Arm ===" << endl;
Frame3D base = Frame3D::fromOriginEuler(0, 0, 0, 0, 0, 0);
Frame3D shoulder = Frame3D::fromOriginEuler(0, 1, 0, 0, 0, 0); // 1 unit up
Frame3D elbow = Frame3D::fromOriginEuler(0, 2, 0, 0, 0, M_PI/4); // 2 units along, 45 deg
Frame3D wrist = Frame3D::fromOriginEuler(0, 1.5, 0, 0, 0, M_PI/4); // 1.5 units, 45 deg
// Chain: base * shoulder * elbow * wrist
Frame3D armInWorld = base.multiply(shoulder).multiply(elbow).multiply(wrist);
double fx = 0, fy = 1, fz = 0; // fingertip in wrist frame
armInWorld.localToWorld(fx, fy, fz, wx, wy, wz);
cout << "Fingertip local: (" << fx << ", " << fy << ", " << fz << ")" << endl;
cout << "Fingertip world: (" << wx << ", " << wy << ", " << wz << ")" << endl;
return 0;
}
C 实现
#include <stdio.h>
#include <math.h>
typedef double Frame3D[4][4];
void frame3d_from_origin_euler(double ox, double oy, double oz,
double ax, double ay, double az,
Frame3D f) {
double cx = cos(ax), sx = sin(ax);
double cy = cos(ay), sy = sin(ay);
double cz = cos(az), sz = sin(az);
f[0][0] = cy*cz; f[0][1] = sx*sy*cz - cx*sz; f[0][2] = cx*sy*cz + sx*sz;
f[1][0] = cy*sz; f[1][1] = sx*sy*sz + cx*cz; f[1][2] = cx*sy*sz - sx*cz;
f[2][0] = -sy; f[2][1] = sx*cy; f[2][2] = cx*cy;
f[0][3] = ox; f[1][3] = oy; f[2][3] = oz;
f[3][0] = 0; f[3][1] = 0; f[3][2] = 0; f[3][3] = 1;
}
void frame3d_multiply(Frame3D A, Frame3D B, Frame3D result) {
Frame3D temp = {{0}};
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
for (int k = 0; k < 4; k++)
temp[i][j] += A[i][k] * B[k][j];
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
result[i][j] = temp[i][j];
}
void frame3d_local_to_world(Frame3D f, double lx, double ly, double lz,
double* wx, double* wy, double* wz) {
*wx = f[0][0]*lx + f[0][1]*ly + f[0][2]*lz + f[0][3];
*wy = f[1][0]*lx + f[1][1]*ly + f[1][2]*lz + f[1][3];
*wz = f[2][0]*lx + f[2][1]*ly + f[2][2]*lz + f[2][3];
}
void frame3d_world_to_local(Frame3D f, double wx, double wy, double wz,
double* lx, double* ly, double* lz) {
double dx = wx - f[0][3];
double dy = wy - f[1][3];
double dz = wz - f[2][3];
*lx = f[0][0]*dx + f[1][0]*dy + f[2][0]*dz;
*ly = f[0][1]*dx + f[1][1]*dy + f[2][1]*dz;
*lz = f[0][2]*dx + f[1][2]*dy + f[2][2]*dz;
}
int main() {
printf("=== Simple Frame ===\n");
Frame3D frame;
frame3d_from_origin_euler(3, 0, 0, 0, 0, M_PI/2, frame);
double lx = 2, ly = 0, lz = 0;
double wx, wy, wz;
frame3d_local_to_world(frame, lx, ly, lz, &wx, &wy, &wz);
printf("Local (%g, %g, %g) -> World (%g, %g, %g)\n", lx, ly, lz, wx, wy, wz);
double lx2, ly2, lz2;
frame3d_world_to_local(frame, wx, wy, wz, &lx2, &ly2, &lz2);
printf("World (%g, %g, %g) -> Local (%g, %g, %g)\n", wx, wy, wz, lx2, ly2, lz2);
printf("\n=== Robot Arm ===\n");
Frame3D base, shoulder, elbow, wrist;
frame3d_from_origin_euler(0, 0, 0, 0, 0, 0, base);
frame3d_from_origin_euler(0, 1, 0, 0, 0, 0, shoulder);
frame3d_from_origin_euler(0, 2, 0, 0, 0, M_PI/4, elbow);
frame3d_from_origin_euler(0, 1.5, 0, 0, 0, M_PI/4, wrist);
Frame3D temp1, temp2, armInWorld;
frame3d_multiply(base, shoulder, temp1);
frame3d_multiply(temp1, elbow, temp2);
frame3d_multiply(temp2, wrist, armInWorld);
double fx = 0, fy = 1, fz = 0;
frame3d_local_to_world(armInWorld, fx, fy, fz, &wx, &wy, &wz);
printf("Fingertip local: (%g, %g, %g)\n", fx, fy, fz);
printf("Fingertip world: (%g, %g, %g)\n", wx, wy, wz);
return 0;
}
Python 实现
import math
class Frame3D:
"""3D coordinate frame using 4x4 homogeneous matrix."""
def __init__(self, ox=0, oy=0, oz=0, ax=0, ay=0, az=0):
"""Create frame from origin and Euler angles (radians).
Rotation order: Rz(az) * Ry(ay) * Rx(ax)
"""
cx, sx = math.cos(ax), math.sin(ax)
cy, sy = math.cos(ay), math.sin(ay)
cz, sz = math.cos(az), math.sin(az)
self.m = [
[cy*cz, sx*sy*cz - cx*sz, cx*sy*cz + sx*sz, ox],
[cy*sz, sx*sy*sz + cx*cz, cx*sy*sz - sx*cz, oy],
[-sy, sx*cy, cx*cy, oz],
[0, 0, 0, 1 ],
]
def multiply(self, other):
result = Frame3D()
for i in range(4):
for j in range(4):
result.m[i][j] = sum(
self.m[i][k] * other.m[k][j] for k in range(4)
)
return result
def local_to_world(self, lx, ly, lz):
wx = self.m[0][0]*lx + self.m[0][1]*ly + self.m[0][2]*lz + self.m[0][3]
wy = self.m[1][0]*lx + self.m[1][1]*ly + self.m[1][2]*lz + self.m[1][3]
wz = self.m[2][0]*lx + self.m[2][1]*ly + self.m[2][2]*lz + self.m[2][3]
return wx, wy, wz
def world_to_local(self, wx, wy, wz):
dx = wx - self.m[0][3]
dy = wy - self.m[1][3]
dz = wz - self.m[2][3]
lx = self.m[0][0]*dx + self.m[1][0]*dy + self.m[2][0]*dz
ly = self.m[0][1]*dx + self.m[1][1]*dy + self.m[2][1]*dz
lz = self.m[0][2]*dx + self.m[1][2]*dy + self.m[2][2]*dz
return lx, ly, lz
# Example 1: Simple frame
print("=== Simple Frame ===")
frame = Frame3D(ox=3, az=math.pi/2)
lx, ly, lz = 2, 0, 0
wx, wy, wz = frame.local_to_world(lx, ly, lz)
print(f"Local ({lx}, {ly}, {lz}) -> World ({wx}, {wy}, {wz})")
lx2, ly2, lz2 = frame.world_to_local(wx, wy, wz)
print(f"World ({wx}, {wy}, {wz}) -> Local ({lx2}, {ly2}, {lz2})")
# Example 2: Robot arm
print("\n=== Robot Arm ===")
base = Frame3D(oy=1)
shoulder = Frame3D(oy=2, az=math.pi/4)
elbow = Frame3D(oy=1.5, az=math.pi/4)
arm = base.multiply(shoulder).multiply(elbow)
fx, fy, fz = 0, 1, 0
wx, wy, wz = arm.local_to_world(fx, fy, fz)
print(f"Fingertip local: ({fx}, {fy}, {fz})")
print(f"Fingertip world: ({wx:.4f}, {wy:.4f}, {wz:.4f})")
Go 实现
package main
import (
"fmt"
"math"
)
// Frame3D represents a 3D coordinate frame as a 4x4 matrix
type Frame3D [4][4]float64
// newFrame3D creates a frame from origin and Euler angles
// Rotation order: Rz(az) * Ry(ay) * Rx(ax)
func newFrame3D(ox, oy, oz, ax, ay, az float64) Frame3D {
cx, sx := math.Cos(ax), math.Sin(ax)
cy, sy := math.Cos(ay), math.Sin(ay)
cz, sz := math.Cos(az), math.Sin(az)
return Frame3D{
{cy * cz, sx*sy*cz - cx*sz, cx*sy*cz + sx*sz, ox},
{cy * sz, sx*sy*sz + cx*cz, cx*sy*sz - sx*cz, oy},
{-sy, sx * cy, cx * cy, oz},
{0, 0, 0, 1},
}
}
func (a Frame3D) multiply(b Frame3D) Frame3D {
var result Frame3D
for i := 0; i < 4; i++ {
for j := 0; j < 4; j++ {
for k := 0; k < 4; k++ {
result[i][j] += a[i][k] * b[k][j]
}
}
}
return result
}
func (f Frame3D) localToWorld(lx, ly, lz float64) (float64, float64, float64) {
wx := f[0][0]*lx + f[0][1]*ly + f[0][2]*lz + f[0][3]
wy := f[1][0]*lx + f[1][1]*ly + f[1][2]*lz + f[1][3]
wz := f[2][0]*lx + f[2][1]*ly + f[2][2]*lz + f[2][3]
return wx, wy, wz
}
func (f Frame3D) worldToLocal(wx, wy, wz float64) (float64, float64, float64) {
dx := wx - f[0][3]
dy := wy - f[1][3]
dz := wz - f[2][3]
lx := f[0][0]*dx + f[1][0]*dy + f[2][0]*dz
ly := f[0][1]*dx + f[1][1]*dy + f[2][1]*dz
lz := f[0][2]*dx + f[1][2]*dy + f[2][2]*dz
return lx, ly, lz
}
func main() {
// Example 1: Simple frame
fmt.Println("=== Simple Frame ===")
frame := newFrame3D(3, 0, 0, 0, 0, math.Pi/2)
lx, ly, lz := 2.0, 0.0, 0.0
wx, wy, wz := frame.localToWorld(lx, ly, lz)
fmt.Printf("Local (%g, %g, %g) -> World (%g, %g, %g)\n", lx, ly, lz, wx, wy, wz)
lx2, ly2, lz2 := frame.worldToLocal(wx, wy, wz)
fmt.Printf("World (%g, %g, %g) -> Local (%g, %g, %g)\n", wx, wy, wz, lx2, ly2, lz2)
// Example 2: Robot arm
fmt.Println("\n=== Robot Arm ===")
base := newFrame3D(0, 1, 0, 0, 0, 0)
shoulder := newFrame3D(0, 2, 0, 0, 0, math.Pi/4)
elbow := newFrame3D(0, 1.5, 0, 0, 0, math.Pi/4)
arm := base.multiply(shoulder).multiply(elbow)
fx, fy, fz := 0.0, 1.0, 0.0
wx, wy, wz = arm.localToWorld(fx, fy, fz)
fmt.Printf("Fingertip local: (%g, %g, %g)\n", fx, fy, fz)
fmt.Printf("Fingertip world: (%g, %g, %g)\n", wx, wy, wz)
}
运行该程序将输出：
=== Simple Frame ===
Local (2, 0, 0) -> World (3, 2, 0)
World (3, 2, 0) -> Local (2, 0, 0)
=== Robot Arm ===
Fingertip local: (0, 1, 0)
Fingertip world: (0.0000, 4.5000, 0.0000)
方向向量的转换
坐标系转换不仅用于点的位置，还用于方向向量（Direction Vectors）的转换。方向向量（如速度、法线、光照方向）没有位置属性，只关心方向，因此不受平移影响。
方向向量使用 4x4 矩阵的左上角 3x3 子矩阵来转换：
| îx ĵx k̂x | | vx | | îx*vx + ĵx*vy + k̂x*vz |
| îy ĵy k̂y | × | vy | = | îy*vx + ĵy*vy + k̂y*vz |
| îz ĵz k̂z | | vz | | îz*vx + ĵz*vy + k̂z*vz |
或者用齐次坐标表示，第 4 分量为 0（而非 1）：
| R t | | vx | | R × v |
| 0 1 | × | 0 | = | 0 |
w=0 表示"方向"，平移分量 tx/ty/tz 乘以 0 后不生效。
C++ 实现
#include <iostream>
#include <cmath>
#include <iomanip>
using namespace std;
struct Frame3D {
double m[4][4];
static Frame3D fromOriginEuler(double ox, double oy, double oz,
double ax, double ay, double az) {
Frame3D f;
double cx = cos(ax), sx = sin(ax);
double cy = cos(ay), sy = sin(ay);
double cz = cos(az), sz = sin(az);
f.m[0][0] = cy*cz; f.m[0][1] = sx*sy*cz - cx*sz; f.m[0][2] = cx*sy*cz + sx*sz;
f.m[1][0] = cy*sz; f.m[1][1] = sx*sy*sz + cx*cz; f.m[1][2] = cx*sy*sz - sx*cz;
f.m[2][0] = -sy; f.m[2][1] = sx*cy; f.m[2][2] = cx*cy;
f.m[0][3] = ox; f.m[1][3] = oy; f.m[2][3] = oz;
f.m[3][0] = 0; f.m[3][1] = 0; f.m[3][2] = 0; f.m[3][3] = 1;
return f;
}
// Transform direction (no translation)
void transformDir(double vx, double vy, double vz,
double& ox, double& oy, double& oz) const {
ox = m[0][0]*vx + m[0][1]*vy + m[0][2]*vz;
oy = m[1][0]*vx + m[1][1]*vy + m[1][2]*vz;
oz = m[2][0]*vx + m[2][1]*vy + m[2][2]*vz;
}
};
int main() {
cout << fixed << setprecision(4);
Frame3D frame = Frame3D::fromOriginEuler(3, 0, 0, 0, 0, M_PI/2);
// Local x-direction in world
double wx, wy, wz;
frame.transformDir(1, 0, 0, wx, wy, wz);
cout << "Local x-axis direction: (" << wx << ", " << wy << ", " << wz << ")" << endl;
// Local y-direction in world
frame.transformDir(0, 1, 0, wx, wy, wz);
cout << "Local y-axis direction: (" << wx << ", " << wy << ", " << wz << ")" << endl;
// Local z-direction in world
frame.transformDir(0, 0, 1, wx, wy, wz);
cout << "Local z-axis direction: (" << wx << ", " << wy << ", " << wz << ")" << endl;
return 0;
}
C 实现
#include <stdio.h>
#include <math.h>
typedef double Frame3D[4][4];
void frame3d_from_origin_euler(double ox, double oy, double oz,
double ax, double ay, double az,
Frame3D f) {
double cx = cos(ax), sx = sin(ax);
double cy = cos(ay), sy = sin(ay);
double cz = cos(az), sz = sin(az);
f[0][0] = cy*cz; f[0][1] = sx*sy*cz - cx*sz; f[0][2] = cx*sy*cz + sx*sz;
f[1][0] = cy*sz; f[1][1] = sx*sy*sz + cx*cz; f[1][2] = cx*sy*sz - sx*cz;
f[2][0] = -sy; f[2][1] = sx*cy; f[2][2] = cx*cy;
f[0][3] = ox; f[1][3] = oy; f[2][3] = oz;
f[3][0] = 0; f[3][1] = 0; f[3][2] = 0; f[3][3] = 1;
}
void frame3d_transform_dir(Frame3D f, double vx, double vy, double vz,
double* ox, double* oy, double* oz) {
*ox = f[0][0]*vx + f[0][1]*vy + f[0][2]*vz;
*oy = f[1][0]*vx + f[1][1]*vy + f[1][2]*vz;
*oz = f[2][0]*vx + f[2][1]*vy + f[2][2]*vz;
}
int main() {
Frame3D frame;
frame3d_from_origin_euler(3, 0, 0, 0, 0, M_PI/2, frame);
double wx, wy, wz;
frame3d_transform_dir(frame, 1, 0, 0, &wx, &wy, &wz);
printf("Local x-axis direction: (%g, %g, %g)\n", wx, wy, wz);
frame3d_transform_dir(frame, 0, 1, 0, &wx, &wy, &wz);
printf("Local y-axis direction: (%g, %g, %g)\n", wx, wy, wz);
frame3d_transform_dir(frame, 0, 0, 1, &wx, &wy, &wz);
printf("Local z-axis direction: (%g, %g, %g)\n", wx, wy, wz);
return 0;
}
Python 实现
import math
class Frame3D:
def __init__(self, ox=0, oy=0, oz=0, ax=0, ay=0, az=0):
cx, sx = math.cos(ax), math.sin(ax)
cy, sy = math.cos(ay), math.sin(ay)
cz, sz = math.cos(az), math.sin(az)
self.m = [
[cy*cz, sx*sy*cz - cx*sz, cx*sy*cz + sx*sz, ox],
[cy*sz, sx*sy*sz + cx*cz, cx*sy*sz - sx*cz, oy],
[-sy, sx*cy, cx*cy, oz],
[0, 0, 0, 1 ],
]
def transform_dir(self, vx, vy, vz):
ox = self.m[0][0]*vx + self.m[0][1]*vy + self.m[0][2]*vz
oy = self.m[1][0]*vx + self.m[1][1]*vy + self.m[1][2]*vz
oz = self.m[2][0]*vx + self.m[2][1]*vy + self.m[2][2]*vz
return ox, oy, oz
frame = Frame3D(ox=3, az=math.pi/2)
wx, wy, wz = frame.transform_dir(1, 0, 0)
print(f"Local x-axis direction: ({wx}, {wy}, {wz})")
wx, wy, wz = frame.transform_dir(0, 1, 0)
print(f"Local y-axis direction: ({wx}, {wy}, {wz})")
wx, wy, wz = frame.transform_dir(0, 0, 1)
print(f"Local z-axis direction: ({wx}, {wy}, {wz})")
Go 实现
package main
import (
"fmt"
"math"
)
type Frame3D [4][4]float64
func newFrame3D(ox, oy, oz, ax, ay, az float64) Frame3D {
cx, sx := math.Cos(ax), math.Sin(ax)
cy, sy := math.Cos(ay), math.Sin(ay)
cz, sz := math.Cos(az), math.Sin(az)
return Frame3D{
{cy * cz, sx*sy*cz - cx*sz, cx*sy*cz + sx*sz, ox},
{cy * sz, sx*sy*sz + cx*cz, cx*sy*sz - sx*cz, oy},
{-sy, sx * cy, cx * cy, oz},
{0, 0, 0, 1},
}
}
func (f Frame3D) transformDir(vx, vy, vz float64) (float64, float64, float64) {
ox := f[0][0]*vx + f[0][1]*vy + f[0][2]*vz
oy := f[1][0]*vx + f[1][1]*vy + f[1][2]*vz
oz := f[2][0]*vx + f[2][1]*vy + f[2][2]*vz
return ox, oy, oz
}
func main() {
frame := newFrame3D(3, 0, 0, 0, 0, math.Pi/2)
wx, wy, wz := frame.transformDir(1, 0, 0)
fmt.Printf("Local x-axis direction: (%g, %g, %g)\n", wx, wy, wz)
wx, wy, wz = frame.transformDir(0, 1, 0)
fmt.Printf("Local y-axis direction: (%g, %g, %g)\n", wx, wy, wz)
wx, wy, wz = frame.transformDir(0, 0, 1)
fmt.Printf("Local z-axis direction: (%g, %g, %g)\n", wx, wy, wz)
}
运行该程序将输出：
Local x-axis direction: (0, 1, 0)
Local y-axis direction: (-1, 0, 0)
Local z-axis direction: (0, 0, 1)
绕 z 轴旋转 90 度后：本地 x 轴指向世界 y 方向，本地 y 轴指向世界 -x 方向，z 轴不变。
法线向量的特殊转换
法线向量（Normal Vectors）的转换规则与普通方向向量不同。如果模型经过了非均匀缩放，直接用 3x3 子矩阵转换法线会导致法线不再垂直于表面。
正确的做法是使用逆转置矩阵（Inverse Transpose Matrix）：
n_world = (M^(-1))^T × n_local
对于只包含旋转的变换（无缩放），旋转矩阵的逆等于其转置，因此 (R(-1))T = R，法线转换退化为普通的旋转。只有在有非均匀缩放时才需要使用逆转置矩阵。
只旋转： n_world = R × n_local (直接用旋转矩阵)
有缩放： n_world = (M^(-1))^T × n_local (需要逆转置)
均匀缩放： n_world = R × n_local / ||...|| (转换后归一化)
三维坐标系转换的性质
齐次坐标 w 分量的含义
| w 值 | 含义 | 转换行为 |
|---|---|---|
| 1 | 位置点（Point） | 受旋转和平移影响 |
| 0 | 方向向量（Vector） | 只受旋转影响，不受平移 |
| 其他 | 齐次化后除以 w | 用于透视投影（Perspective Projection） |
位置点：Frame × (x, y, z, 1)^T → 旋转 + 平移
方向量：Frame × (x, y, z, 0)^T → 只旋转
链式转换
多层嵌套坐标系遵循矩阵乘法的链式法则：
P_world = M1 × M2 × ... × Mn × P_local
可以将所有矩阵预先合并为一个 4x4 矩阵：
M_total = M1 × M2 × ... × Mn
P_world = M_total × P_local
无论嵌套多深，对每个点的处理始终只需一次 4x4 矩阵乘法。
逆变换
刚体变换（旋转 + 平移）的逆矩阵有高效的封闭形式：
| R t |^(-1) | R^T -R^T × t |
| 0 1 | = | 0 1 |
不需要通用的 4x4 矩阵求逆，只需矩阵转置和一个 3x1 向量乘法。
应用场景
| 应用 | 说明 |
|---|---|
| 3D 游戏引擎 | 场景图（Scene Graph）中的父子节点变换 |
| 骨骼动画 | 骨骼层级中的关节坐标系 |
| 机器人学 | 正向/逆向运动学（Forward/Inverse Kinematics） |
| CAD 系统 | 零件装配中的坐标系关系 |
| VR/AR | 头显/手柄在虚拟空间中的定位 |
| 物理引擎 | 碰撞检测中的刚体坐标变换 |
| 相机系统 | 世界坐标 → 视图坐标 → 裁剪坐标 |
浙公网安备 33010602011771号