【go/方法记录】局部坐标与世界坐标间的相互转换(位置/方向)

文章目录

说在前面
计算旋转矩阵
计算变换矩阵
[Inverse Quaternion](#Inverse Quaternion)
[Euler To Quaternion](#Euler To Quaternion)
[World to Local](#World to Local)
- Position
- Rotation
[Local to World](#Local to World)
- Position
- Rotation
参考

说在前面

golang版本： go1.20.5 windows/386

gonum版本：gonum.org/v1/gonum v0.14.0

计算旋转矩阵

四元数→旋转矩阵

go 复制代码

import (
	"gonum.org/v1/gonum/mat"
	"gonum.org/v1/gonum/num/quat"
)
func QuaternionToMatrix(qua *quat.Number) *mat.Dense {
	x, y, z, w := qua.Imag, qua.Jmag, qua.Kmag, qua.Real
	return mat.NewDense(3, 3, []float64{
		1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w,
		2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w,
		2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y,
	})
}

详见：Maths - Conversion Quaternion to Matrix

旋转矩阵→四元数

go 复制代码

import (
	"gonum.org/v1/gonum/mat"
	"gonum.org/v1/gonum/num/quat"
)
func MatrixToQuaternion(m *mat.Dense) *quat.Number {
	var m00, m01, m02,
		m10, m11, m12,
		m20, m21, m22 = m.At(0, 0), m.At(0, 1), m.At(0, 2),
		m.At(1, 0), m.At(1, 1), m.At(1, 2),
		m.At(2, 0), m.At(2, 1), m.At(2, 2)

	tr := m00 + m11 + m22

	var x, y, z, w float64
	if tr > 0 {
		S := math.Sqrt(tr+1.0) * 2 // S=4*qw
		w = 0.25 * S
		x = (m21 - m12) / S
		y = (m02 - m20) / S
		z = (m10 - m01) / S
	} else if (m00 > m11) && (m00 > m22) {
		S := math.Sqrt(1.0+m00-m11-m22) * 2 // S=4*qx
		w = (m21 - m12) / S
		x = 0.25 * S
		y = (m01 + m10) / S
		z = (m02 + m20) / S
	} else if m11 > m22 {
		S := math.Sqrt(1.0+m11-m00-m22) * 2 // S=4*qy
		w = (m02 - m20) / S
		x = (m01 + m10) / S
		y = 0.25 * S
		z = (m12 + m21) / S
	} else {
		S := math.Sqrt(1.0+m22-m00-m11) * 2 // S=4*qz
		w = (m10 - m01) / S
		x = (m02 + m20) / S
		y = (m12 + m21) / S
		z = 0.25 * S
	}
	return &quat.Number{Real: w, Imag: x, Jmag: y, Kmag: z}
}

计算变换矩阵

变换矩阵的基本形式为：
T = [ R t 0 1 ] T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} T=[R0t1]
其中 R R R为上文计算的旋转矩阵，而 t t t为3row x 1col的位移变换（其值为物体的三维坐标）
如果知道了World Space中某个物体的位置以及朝向(欧拉角或四元数)，就能计算出它(相对于世界坐标系)的变换矩阵。

Inverse Quaternion

对于一个单位四元数，直接取共轭（conjugate），gonum库有提供接口：

go 复制代码

// Conj returns the quaternion conjugate of q.
func Conj(q Number) Number {
	return Number{Real: q.Real, Imag: -q.Imag, Jmag: -q.Jmag, Kmag: -q.Kmag}
}

否则，需要归一，gonum库也提供了接口：

go 复制代码

// Inv returns the quaternion inverse of q.
func Inv(q Number) Number {
	if IsInf(q) {
		return zero
	}
	a := Abs(q)
	return Scale(1/(a*a), Conj(q))
}

Euler To Quaternion

欧拉角转四元数时，不同的旋转顺序对应的结果不一样，需要先确定旋顺序，Unity默认的顺序为ZXY

参考Three.js实现

js 复制代码

setFromEuler( euler, update ) {
	const x = euler._x, y = euler._y, z = euler._z, order = euler._order;

	// http://www.mathworks.com/matlabcentral/fileexchange/
	// 	20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
	//	content/SpinCalc.m

	const cos = Math.cos;
	const sin = Math.sin;

	const c1 = cos( x / 2 );
	const c2 = cos( y / 2 );
	const c3 = cos( z / 2 );

	const s1 = sin( x / 2 );
	const s2 = sin( y / 2 );
	const s3 = sin( z / 2 );

	switch ( order ) {
		case 'XYZ':
			this._x = s1 * c2 * c3 + c1 * s2 * s3;
			this._y = c1 * s2 * c3 - s1 * c2 * s3;
			this._z = c1 * c2 * s3 + s1 * s2 * c3;
			this._w = c1 * c2 * c3 - s1 * s2 * s3;
			break;
		case 'YXZ':
			this._x = s1 * c2 * c3 + c1 * s2 * s3;
			this._y = c1 * s2 * c3 - s1 * c2 * s3;
			this._z = c1 * c2 * s3 - s1 * s2 * c3;
			this._w = c1 * c2 * c3 + s1 * s2 * s3;
			break;
		case 'ZXY':
			this._x = s1 * c2 * c3 - c1 * s2 * s3;
			this._y = c1 * s2 * c3 + s1 * c2 * s3;
			this._z = c1 * c2 * s3 + s1 * s2 * c3;
			this._w = c1 * c2 * c3 - s1 * s2 * s3;
			break;
		case 'ZYX':
			this._x = s1 * c2 * c3 - c1 * s2 * s3;
			this._y = c1 * s2 * c3 + s1 * c2 * s3;
			this._z = c1 * c2 * s3 - s1 * s2 * c3;
			this._w = c1 * c2 * c3 + s1 * s2 * s3;
			break;
		case 'YZX':
			this._x = s1 * c2 * c3 + c1 * s2 * s3;
			this._y = c1 * s2 * c3 + s1 * c2 * s3;
			this._z = c1 * c2 * s3 - s1 * s2 * c3;
			this._w = c1 * c2 * c3 - s1 * s2 * s3;
			break;
		case 'XZY':
			this._x = s1 * c2 * c3 - c1 * s2 * s3;
			this._y = c1 * s2 * c3 - s1 * c2 * s3;
			this._z = c1 * c2 * s3 + s1 * s2 * c3;
			this._w = c1 * c2 * c3 + s1 * s2 * s3;
			break;
		default:
			console.warn( 'THREE.Quaternion: .setFromEuler() encountered an unknown order: ' + order );
	}
	if ( update !== false ) this._onChangeCallback();
	return this;
}

World to Local

Position

对于某个父对象下的某个子对象，如果知道了父、子对象的世界坐标、朝向，就可以计算出子对象相对于父对象的局部坐标。通过父对象的变换矩阵 T T T进行计算：
P l o c a l = T p a r e n t ′ P w o r l d P_{local}=T_{parent}'P_{world} Plocal=Tparent′Pworld
其中 T p a r e n t ′ T_{parent}' Tparent′为父对象的变换矩阵的逆，
而 P w o r l d P_{world} Pworld为世界坐标，其形式为：
P w o r l d = [ x y z 1 ] P_{world}=\begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} Pworld= xyz1

代码

go 复制代码

func WorldToLocal(x, y, z float64) {
	pWorld := mat.NewDense(4, 1, []float64{x, y, z, 1})
	T := mat.NewDense(4, 4, []float64{
		// ... 旋转矩阵+位置的组合 4X4矩阵
	})
	T.Inverse(T)
	pLocal := mat.NewDense(4, 1, nil)
	pLocal.Mul(T, pWorld)
}

Rotation

目前这里记录的是四元数的转换，并且由于网上找不到啥资料，两个转换是unity以及chat gpt提供的
unity，参考这里
Q w o r l d = Q p a r e n t Q l o c a l Q_{world}=Q_{parent}Q_{local} Qworld=QparentQlocal
那么：
Q p a r e n t ′ Q w o r l d = Q l o c a l Q_{parent}'Q_{world}=Q_{local} Qparent′Qworld=Qlocal
其中 Q p a r e n t ′ Q_{parent}' Qparent′为父对象的rotation的逆
chat gpt4提供的方式是： Prompt：
how to convert quaternion from world to space

GPT：

Converting a quaternion from world space to local space (also referred to as "object space" or "model space") typically involves using the inverse of the transformation quaternion that represents the orientation of the object in world space. This process is used in 3D graphics and physics simulations to understand the orientation of an object relative to its own coordinate system rather than the global coordinate system.

Here's a general step-by-step guide:
1. Understand Quaternion Representation: Quaternions are a way to represent rotations. They have four components: ( x, y, z ) (the imaginary components) and ( w ) (the real component).
2. Determine the World Space Quaternion: This is the quaternion that represents the rotation of your object in world space.
3. Compute the Inverse of the World Space Quaternion: To convert a quaternion from world space to local space, you need the inverse of the world space quaternion. The inverse of a quaternion ( q = (x, y, z, w) ) is given by ( q^{-1} = (-x, -y, -z, w) ), assuming the quaternion is normalized (i.e., its length is 1).
4. Apply the Inverse Quaternion to the Point or Quaternion of Interest: If you have a point or another quaternion that you want to convert from world space to local space, you multiply it by the inverse quaternion you computed in step 3. This operation is typically a quaternion multiplication.
5. Normalize the Result (if needed): After the multiplication, it might be necessary to normalize the resulting quaternion to ensure that its length is 1.
python 复制代码
```
# Here is an example in Python pseudocode:
# Assuming you have a quaternion class with necessary operations
class Quaternion:
    def __init__(self, x, y, z, w):
        self.x = x
        self.y = y
        self.z = z
        self.w = w

    def normalize(self):
        # Normalize the quaternion
        pass

    def inverse(self):
        # Compute and return the inverse of the quaternion
        return Quaternion(-self.x, -self.y, -self.z, self.w)

    def __mul__(self, other):
        # Define quaternion multiplication
        pass

# Your world space quaternion
world_space_quaternion = Quaternion(x, y, z, w)
world_space_quaternion.normalize()

# Compute the inverse
inverse_quaternion = world_space_quaternion.inverse()

# Multiply with the point/quaternion you want to convert
# For example, converting another quaternion from world to local space
local_space_quaternion = inverse_quaternion * other_quaternion
```
和unity基本一致（但是我问3.5又是另一种结果）

Local to World

Position

基本上反着来就行：
P w o r l d = T p a r e n t P l o c a l P_{world}=T_{parent}P_{local} Pworld=TparentPlocal
代码就不贴了，基本一致

Rotation

基本上反着来就行
unity
Q w o r l d = Q p a r e n t Q l o c a l Q_{world}=Q_{parent}Q_{local} Qworld=QparentQlocal
chat gpt
公式和上面一致

参考

3D Rotation Converter
Maths - Quaternion Functions
how to convert world space transform to object space transform?
3.5的结果
Converting a quaternion from world space to another space (e.g., local space or object space) involves a transformation that depends on the orientation of the reference spaces. Here's a general approach:
Let's say you have a quaternion Q_world representing an orientation in world space, and you want to convert it to a quaternion Q_local in local space. Also, assume that you have the rotation matrix R_world_to_local that transforms vectors from world space to local space.
The conversion can be done using the following formula:
Q local = R world_to_local × Q world × R world_to_local T Q_{\text{local}} = R_{\text{world\to\local}} \times Q{\text{world}} \times R{\text{world\_to\local}}^T Qlocal=Rworld_to_local×Qworld×Rworld_to_localT
Here, R world_to_local T R{\text{world\_to\_local}}^T Rworld_to_localT is the transpose of the rotation matrix. If the rotation matrix is a pure rotation matrix (i.e., it doesn't include translation or scaling), then the transpose is the same as the inverse.
If you have the rotation matrix in terms of a 3x3 matrix, you can convert it to a quaternion using a conversion function. The quaternion multiplication is typically implemented as Hamilton product.
Here's some pseudocode in Python:
python 复制代码
```
import numpy as np
from scipy.spatial.transform import Rotation

# Assuming Q_world is your quaternion in world space
# Assuming R_world_to_local is your rotation matrix from world to local space

# Convert rotation matrix to quaternion
r_world_to_local = Rotation.from_matrix(R_world_to_local)
q_world_to_local = r_world_to_local.as_quat()

# Convert quaternion from world to local space
q_world = np.quaternion(Q_world[0], Q_world[1], Q_world[2], Q_world[3])
q_local = q_world_to_local * q_world * q_world_to_local.conj()

# Extract the components of the resulting quaternion
Q_local = np.array([q_local.real, q_local.imag[0], q_local.imag[1], q_local.imag[2]])
```