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

说在前面

• golang版本： go1.20.5 windows/386
• gonum版本：gonum.org/v1/gonum v0.14.0

计算旋转矩阵

• 四元数→旋转矩阵

  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

• 旋转矩阵→四元数

  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=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
  其中$R$为上文计算的旋转矩阵，而$t$为3row x 1col的位移变换（其值为物体的三维坐标）
• 如果知道了World Space中某个物体的位置以及朝向(欧拉角或四元数)，就能计算出它(相对于世界坐标系)的变换矩阵。

Inverse Quaternion

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

  // 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库也提供了接口：

  // 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实现

  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$进行计算：
  $$P_{local}=T_{parent}^{\prime }{P}_{world}$$
  其中$T_{parent}^{\prime }$为父对象的变换矩阵的逆，
  而$P_{world}$为世界坐标，其形式为：
  P w o r l d = [ x y z 1 ] P_{world}=\begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} Pworld​= ​xyz1​ ​
• 代码

  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_{world}=Q_{parent}{Q}_{local}$$
  那么：
  $$Q_{parent}^{\prime }{Q}_{world}=Q_{local}$$
  其中$Q_{parent}^{\prime }$为父对象的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.

  # 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_{world}=T_{parent}{P}_{local}$$
  代码就不贴了，基本一致

Rotation

• 基本上反着来就行
• unity
  $$Q_{world}=Q_{parent}{Q}_{local}$$
• 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_{\text{local}}=R_{\text{world\_to\_local}}\times Q_{\text{world}}\times R_{\text{world\_to\_local}}^T$$
  Here, $$R_{\text{world\_to\_local}}^T$$ 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:

  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]])