Application of Quaternion in Industry
Code Link
0. Quaternion & Vec3
- quaternion Q
class Q(object):
def __init__(self, x=0.0, y=0.0, z=0.0, w=1.0) -> None:
self.x = x
self.y = y
self.z = z
self.w = w
...
def print(self):
print(self.x, self.y, self.z, self.w)
# operator override
def __mul__(self, num):
return Q(self.x * num, self.y * num, self.z * num, self.w * num)
def __truediv__(self, num):
assert np.abs(num) > 1e-10
return Q(self.x / num, self.y / num, self.z / num, self.w / num)
- Vec3
class Vec3(object):
def __init__(self, x=0.0, y=0.0, z=0.0) -> None:
self.x = x
self.y = y
self.z = z
def from_array(self, array):
self.x = array[0]
self.y = array[1]
self.z = array[2]
def to_array(self):
return np.array([self.x, self.y, self.z], dtype=np.float32)
def print(self):
print(self.x, self.y, self.z)
# operator override
def __add__(self, vec3):
return Vec3(self.x + vec3.x, self.y + vec3.y, self.z + vec3.z)
def __sub__(self, vec3):
return Vec3(self.x - vec3.x, self.y - vec3.y, self.z - vec3.z)
def __mul__(self, num):
return Vec3(self.x * num, self.y * num, self.z * num)
def __truediv(self, num):
assert np.abs(num) > 1e-10
return Vec3(self.x / num, self.y / num, self.z / num)
1. Definition
q = x i + y j + z k + w q = xi + yj + zk + w q=xi+yj+zk+w
其中 a , b , c a, b, c a,b,c是虚部, w w w是实部。 i 2 = j 2 = k 2 = i j k = − 1 i^2=j^2=k^2=ijk=-1 i2=j2=k2=ijk=−1.
class Q(object):
def __init__(self, x=0.0, y=0.0, z=0.0, w=1.0) -> None:
self.x = x
self.y = y
self.z = z
self.w = w
2. Property
2.1 Norm
- 四元数 q = x i + y j + z k + w q=xi+yj+zk+w q=xi+yj+zk+w的模长:
∣ ∣ q ∣ ∣ = x 2 + y 2 + z 2 + w 2 . ||q|| = \sqrt{x^2 + y^2 + z^2 + w^2}. ∣∣q∣∣=x2+y2+z2+w2.
class Q(object):
def __init__(self, x=0.0, y=0.0, z=0.0, w=1.0) -> None:
self.x = x
self.y = y
self.z = z
self.w = w
def get_norm(self) -> float:
return np.sqrt(self.x**2 + self.y**2 + self.z**2 + self.w**2)
2.2 Conjugation of quaternion
- 四元数 q = x i + y j + z k + w q=xi+yj+zk+w q=xi+yj+zk+w的共轭四元数 q ∗ q^* q∗
q ∗ = − x i − y j − z k + w q^* = -xi-yj-zk+w q∗=−xi−yj−zk+w
- 共轭四元数满足条件:
q q ∗ = q ∗ q = ∣ ∣ q ∣ ∣ 2 qq^*=q^*q=||q||^2 qq∗=q∗q=∣∣q∣∣2
class Q(object):
def __init__(self, x=0.0, y=0.0, z=0.0, w=1.0) -> None:
self.x = x
self.y = y
self.z = z
self.w = w
def get_conjugation(self):
return Q(-self.x, -self.y, -self.z, self.w)
2.3 Inverse of quaternion
- 四元数 q = x i + y j + z k + w q=xi+yj+zk+w q=xi+yj+zk+w的逆四元数 q − 1 q^{-1} q−1
q − 1 = q ∗ ∣ ∣ q ∣ ∣ 2 q^{-1}=\frac{q^*}{||q||^2} q−1=∣∣q∣∣2q∗
class Q(object):
def __init__(self, x=0.0, y=0.0, z=0.0, w=1.0) -> None:
self.x = x
self.y = y
self.z = z
self.w = w
def get_norm(self) -> float:
return np.sqrt(self.x**2 + self.y**2 + self.z**2 + self.w**2)
def get_conjugation(self):
return Q(-self.x, -self.y, -self.z, self.w)
def get_inverse(self):
q_conjugation = self.get_conjugation()
q_lengthSqr = self.get_norm()**2
return Q(q_conjugation.x/q_lengthSqr, q_conjugation.y/q_lengthSqr,\
q_conjugation.z/q_lengthSqr, q_conjugation.w/q_lengthSqr)
3. Operation
3.1 Make quaternion with axis and angle
- 绕轴 u \mathbf{u} u逆时针旋转 θ \theta θ角度的四元数表示:
q = sin θ 2 u . x i + sin θ 2 u . y j + sin θ 2 u . z k + cos θ 2 q = \sin{\frac{\theta}{2}}\mathbf{u}.x~i + \sin{\frac{\theta}{2}}\mathbf{u}.y~j + \sin{\frac{\theta}{2}}\mathbf{u}.z~k + \cos{\frac{\theta}{2}} q=sin2θu.x i+sin2θu.y j+sin2θu.z k+cos2θ
def MakeQuaternion(angle_radian: float, u: Vec3) -> Q:
q = Q()
halfAngle = 0.5 * angle_radian
sinHalf = np.sin(halfAngle)
q.w = np.cos(halfAngle)
q.x = sinHalf * u.x
q.y = sinHalf * u.y
q.z = sinHalf * u.z
return q
3.2 Multiplication with two quaternion
-
满足分配律和结合律,但不满足交换律, q A q B ≠ q A q B q_Aq_B \neq q_Aq_B qAqB=qAqB
-
q A q B q_Aq_B qAqB:
q A q B = ( q A . w ∗ q B . x + q A . x ∗ q B . w + q A . y ∗ q B . z − q A . z ∗ q B . y ) i + = ( q A . w ∗ q B . y + q A . y ∗ q B . w + q A . z ∗ q B . x − q A . x ∗ q B . z ) j + = ( q A . w ∗ q B . z + q A . z ∗ q B . w + q A . x ∗ q B . y − q A . y ∗ q B . x ) k + = ( q A . w ∗ q B . w − q A . x ∗ q B . x − q A . y ∗ q B . y − q A . z ∗ q B . z ) \begin{aligned} q_Aq_B &= (q_A.w * q_B.x + q_A.x * q_B.w + q_A.y * q_B.z - q_A.z * q_B.y)i + \\ &= (q_A.w * q_B.y + q_A.y * q_B.w + q_A.z * q_B.x - q_A.x * q_B.z)j + \\ &= (q_A.w * q_B.z + q_A.z * q_B.w + q_A.x * q_B.y - q_A.y * q_B.x)k + \\ &= (q_A.w * q_B.w - q_A.x * q_B.x - q_A.y * q_B.y - q_A.z * q_B.z) \end{aligned} qAqB=(qA.w∗qB.x+qA.x∗qB.w+qA.y∗qB.z−qA.z∗qB.y)i+=(qA.w∗qB.y+qA.y∗qB.w+qA.z∗qB.x−qA.x∗qB.z)j+=(qA.w∗qB.z+qA.z∗qB.w+qA.x∗qB.y−qA.y∗qB.x)k+=(qA.w∗qB.w−qA.x∗qB.x−qA.y∗qB.y−qA.z∗qB.z)
def MultQuaternionAndQuaternion(qA: Q, qB: Q) -> Q:
q = Q()
q.w = qA.w * qB.w - qA.x * qB.x - qA.y * qB.y - qA.z * qB.z
q.x = qA.w * qB.x + qA.x * qB.w + qA.y * qB.z - qA.z * qB.y
q.y = qA.w * qB.y + qA.y * qB.w + qA.z * qB.x - qA.x * qB.z
q.z = qA.w * qB.z + qA.z * qB.w + qA.x * qB.y - qA.y * qB.x
return q
3.3 Multiplication with quaternion and vec3
- 对三维向量 v \mathbf{v} v应用四元数 q q q:
v ′ = v + 2.0 ∗ q . w ∗ q . x y z × v + 2.0 ∗ q . x y z × ( q . x y z × v ) \mathbf{v}' = \mathbf{v} + 2.0 * q.w * q.xyz \times \mathbf{v} + 2.0 * q.xyz \times (q.xyz \times \mathbf{v}) v′=v+2.0∗q.w∗q.xyz×v+2.0∗q.xyz×(q.xyz×v)
def MultQuaternionAndVector(q: Q, v: Vec3) -> Vec3:
uv, uuv = Vec3(), Vec3()
qvec = Vec3(q.x, q.y, q.z)
uv.from_array(np.cross(qvec.to_array(), v.to_array()))
uuv.from_array(np.cross(qvec.to_array(), uv.to_array()) * 2.0)
uv *= (2.0 * q.w)
return v + uv + uuv
3.4 Normalize quaternion
- 四元数 q = x i + y j + z k + w q=xi+yj+zk+w q=xi+yj+zk+w的归一化结果为:
q ^ = q ∣ ∣ q ∣ ∣ \hat{q} = \frac{q}{||q||} q^=∣∣q∣∣q
def NormalizeQuaternion(q: Q) -> Q:
qq = Q(q.x, q.y, q.z, q.w)
lengthSqr = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w
if lengthSqr < 1e-10:
qq.w = 1.0
return qq
qq /= np.sqrt(lengthSqr)
return qq
3.5 Quaternion from u to v
- 向量 u \mathbf{u} u到向量 v \mathbf{v} v的四元数 q q q为:
q = N o r m a l i z e ( c r o s s ( u , v ) , 1.0 + d o t ( u , v ) ) q = Normalize(cross(u, v), 1.0 + dot(u, v)) q=Normalize(cross(u,v),1.0+dot(u,v))
def QuatFromTwoUnitVectors(u: Vec3, v: Vec3) -> Q:
r = 1.0 + np.dot(u.to_array(), v.to_array())
n = Vec3()
# if u and v are parallel
if r < 1e-7:
r = 0.0
n = Vec3(-u.y, u.x, 0.0) if np.abs(u.x) > np.abs(u.z) else Vec3(0.0, -u.z, u.y)
else:
n.from_array(np.cross(u.to_array(), v.to_array()))
q = Q(n.x, n.y, n.z, r)
return NormalizeQuaternion(q)