Advertisement
Bunny83

Quaternion Multiplication

Sep 21st, 2019
496
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 5.51 KB | None | 0 0
  1. [Quaternion rules]
  2. === i*j*k = -1
  3.  
  4. i*j = k     j*i = -k
  5. j*k = i     k*j = -i
  6. k*i = j     i*k = -j
  7.  
  8. [Example quaternion "q" and point "p"]
  9. q(x,y,z,w) = q.w + q.x*i + q.y*j + q.z*k
  10. q'(x,y,z,w) = q.w - q.x*i - q.y*j - q.z*k
  11. p(x,y,z)   = p.x*i + p.y*j + p.z*k
  12.  
  13.  
  14. p' = q * p * q'
  15.  
  16. p' = (q.w + q.x*i + q.y*j + q.z*k) * (p.x*i + p.y*j + p.z*k) * (q.w - q.x*i - q.y*j - q.z*k)
  17.  
  18. // first and second bracket combined
  19. p' = (
  20. q.w*p.x*i + q.x*i*p.x*i + q.y*j*p.x*i + q.z*k*p.x*i +
  21. q.w*p.y*j + q.x*i*p.y*j + q.y*j*p.y*j + q.z*k*p.y*j +
  22. q.w*p.z*k + q.x*i*p.z*k + q.y*j*p.z*k + q.z*k*p.z*k
  23. ) * (q.w - q.x*i - q.y*j - q.z*k)
  24.  
  25. // all brackets combined
  26. p' = (
  27. q.w*p.x*i*q.w + q.x*i*p.x*i*q.w + q.y*j*p.x*i*q.w + q.z*k*p.x*i*q.w +
  28. q.w*p.y*j*q.w + q.x*i*p.y*j*q.w + q.y*j*p.y*j*q.w + q.z*k*p.y*j*q.w +
  29. q.w*p.z*k*q.w + q.x*i*p.z*k*q.w + q.y*j*p.z*k*q.w + q.z*k*p.z*k*q.w +
  30.  
  31. q.w*p.x*i*(-q.x*i) + q.x*i*p.x*i*(-q.x*i) + q.y*j*p.x*i*(-q.x*i) + q.z*k*p.x*i*(-q.x*i) +
  32. q.w*p.y*j*(-q.x*i) + q.x*i*p.y*j*(-q.x*i) + q.y*j*p.y*j*(-q.x*i) + q.z*k*p.y*j*(-q.x*i) +
  33. q.w*p.z*k*(-q.x*i) + q.x*i*p.z*k*(-q.x*i) + q.y*j*p.z*k*(-q.x*i) + q.z*k*p.z*k*(-q.x*i) +
  34.  
  35. q.w*p.x*i*(-q.y*j) + q.x*i*p.x*i*(-q.y*j) + q.y*j*p.x*i*(-q.y*j) + q.z*k*p.x*i*(-q.y*j) +
  36. q.w*p.y*j*(-q.y*j) + q.x*i*p.y*j*(-q.y*j) + q.y*j*p.y*j*(-q.y*j) + q.z*k*p.y*j*(-q.y*j) +
  37. q.w*p.z*k*(-q.y*j) + q.x*i*p.z*k*(-q.y*j) + q.y*j*p.z*k*(-q.y*j) + q.z*k*p.z*k*(-q.y*j) +
  38.  
  39. q.w*p.x*i*(-q.z*k) + q.x*i*p.x*i*(-q.z*k) + q.y*j*p.x*i*(-q.z*k) + q.z*k*p.x*i*(-q.z*k) +
  40. q.w*p.y*j*(-q.z*k) + q.x*i*p.y*j*(-q.z*k) + q.y*j*p.y*j*(-q.z*k) + q.z*k*p.y*j*(-q.z*k) +
  41. q.w*p.z*k*(-q.z*k) + q.x*i*p.z*k*(-q.z*k) + q.y*j*p.z*k*(-q.z*k) + q.z*k*p.z*k*(-q.z*k)
  42. )  
  43.  
  44.  
  45. // applying multiplication rules from above
  46.  
  47. p' = (
  48. q.w*p.x*q.w*i - q.x*p.x*q.w   - q.y*p.x*q.w*k + q.z*p.x*q.w*j
  49. +q.w*p.y*q.w*j + q.x*p.y*q.w*k - q.y*p.y*q.w   - q.z*p.y*q.w*i
  50. +q.w*p.z*q.w*k - q.x*p.z*q.w*j + q.y*p.z*q.w*i - q.z*p.z*q.w
  51.  
  52. +q.w*p.x*q.x   + q.x*p.x*q.x*i + q.y*p.x*q.x*j + q.z*p.x*q.x*k
  53. +q.w*p.y*q.x*k - q.x*p.y*q.x*j + q.y*p.y*q.x*i - q.z*p.y*q.x
  54. -q.w*p.z*q.x*j - q.x*p.z*q.x*k + q.y*p.z*q.x   + q.z*p.z*q.x*i
  55.  
  56. -q.w*p.x*q.y*k + q.x*p.x*q.y*j - q.y*p.x*q.y*i + q.z*p.x*q.y
  57. +q.w*p.y*q.y   + q.x*p.y*q.y*i + q.y*p.y*q.y*j + q.z*p.y*q.y*k
  58. +q.w*p.z*q.y*i - q.x*p.z*q.y   - q.y*p.z*q.y*k + q.z*p.z*q.y*j
  59.  
  60. +q.w*p.x*q.z*j + q.x*p.x*q.z*k - q.y*p.x*q.z   - q.z*p.x*q.z*i
  61. -q.w*p.y*q.z*i + q.x*p.y*q.z   + q.y*p.y*q.z*k - q.z*p.y*q.z*j
  62. +q.w*p.z*q.z   + q.x*p.z*q.z*i + q.y*p.z*q.z*j + q.z*p.z*q.z*k
  63. )  
  64.  
  65. // regroup according to imaginary units:
  66.  
  67. p' = (
  68. - q.x*p.x*q.w   - q.y*p.y*q.w   - q.z*p.z*q.w   + q.w*p.x*q.x   - q.z*p.y*q.x   + q.y*p.z*q.x   + q.z*p.x*q.y   + q.w*p.y*q.y   - q.x*p.z*q.y   - q.y*p.x*q.z   + q.x*p.y*q.z   + q.w*p.z*q.z
  69. + q.w*p.x*q.w*i - q.z*p.y*q.w*i + q.y*p.z*q.w*i + q.x*p.x*q.x*i + q.y*p.y*q.x*i + q.z*p.z*q.x*i - q.y*p.x*q.y*i + q.x*p.y*q.y*i + q.w*p.z*q.y*i - q.z*p.x*q.z*i - q.w*p.y*q.z*i + q.x*p.z*q.z*i
  70. + q.z*p.x*q.w*j + q.w*p.y*q.w*j - q.x*p.z*q.w*j + q.y*p.x*q.x*j - q.x*p.y*q.x*j - q.w*p.z*q.x*j + q.x*p.x*q.y*j + q.y*p.y*q.y*j + q.z*p.z*q.y*j + q.w*p.x*q.z*j - q.z*p.y*q.z*j + q.y*p.z*q.z*j
  71. - q.y*p.x*q.w*k + q.x*p.y*q.w*k + q.w*p.z*q.w*k + q.z*p.x*q.x*k + q.w*p.y*q.x*k - q.x*p.z*q.x*k - q.w*p.x*q.y*k + q.z*p.y*q.y*k - q.y*p.z*q.y*k + q.x*p.x*q.z*k + q.y*p.y*q.z*k + q.z*p.z*q.z*k
  72. )  
  73.  
  74. // factor out the imaginary units
  75.  
  76. p' = (
  77. (- q.x*p.x*q.w + q.w*p.x*q.x - q.y*p.y*q.w + q.w*p.y*q.y - q.z*p.z*q.w + q.w*p.z*q.z - q.z*p.y*q.x+ q.x*p.y*q.z + q.y*p.z*q.x - q.x*p.z*q.y + q.z*p.x*q.y - q.y*p.x*q.z )+
  78. (+ q.w*p.x*q.w - q.z*p.y*q.w + q.y*p.z*q.w + q.x*p.x*q.x + q.y*p.y*q.x + q.z*p.z*q.x - q.y*p.x*q.y + q.x*p.y*q.y + q.w*p.z*q.y - q.z*p.x*q.z - q.w*p.y*q.z + q.x*p.z*q.z)*i+
  79. (+ q.z*p.x*q.w + q.w*p.y*q.w - q.x*p.z*q.w + q.y*p.x*q.x - q.x*p.y*q.x - q.w*p.z*q.x + q.x*p.x*q.y + q.y*p.y*q.y + q.z*p.z*q.y + q.w*p.x*q.z - q.z*p.y*q.z + q.y*p.z*q.z)*j+
  80. (- q.y*p.x*q.w + q.x*p.y*q.w + q.w*p.z*q.w + q.z*p.x*q.x + q.w*p.y*q.x - q.x*p.z*q.x - q.w*p.x*q.y + q.z*p.y*q.y - q.y*p.z*q.y + q.x*p.x*q.z + q.y*p.y*q.z + q.z*p.z*q.z)*k
  81. )  
  82.  
  83. // eliminate real part and group terms after p.x, p.y and p.z
  84.  
  85. p' = (
  86. (0 )+
  87. (+ q.w*q.w*p.x + q.x*q.x*p.x - q.y*q.y*p.x - q.z*q.z*p.x - q.z*q.w*p.y + q.x*q.y*p.y + q.x*q.y*p.y - q.z*q.w*p.y + q.y*q.w*p.z + q.x*q.z*p.z + q.y*q.w*p.z + q.x*q.z*p.z)*i+
  88. (+ q.z*q.w*p.x + q.y*q.x*p.x + q.w*q.z*p.x + q.x*q.y*p.x + q.w*q.w*p.y - q.x*q.x*p.y - q.z*q.z*p.y + q.y*q.y*p.y + q.z*q.y*p.z + q.y*q.z*p.z - q.x*q.w*p.z - q.w*q.x*p.z)*j+
  89. (+ q.z*q.x*p.x - q.y*q.w*p.x + q.x*q.z*p.x - q.w*q.y*p.x + q.x*q.w*p.y + q.y*q.z*p.y + q.w*q.x*p.y + q.z*q.y*p.y - q.y*q.y*p.z + q.z*q.z*p.z + q.w*q.w*p.z - q.x*q.x*p.z)*k
  90. )  
  91.  
  92.  
  93. // factor out p.x,p.y and p.z
  94.  
  95. p' = (
  96. (0 )+
  97. (+ (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z)*p.x + (q.x*q.y + q.x*q.y - q.z*q.w - q.z*q.w)*p.y + (q.y*q.w + q.y*q.w + q.x*q.z + q.x*q.z)*p.z)*i+
  98. (+ (q.z*q.w + q.y*q.x + q.w*q.z + q.x*q.y)*p.x + (q.w*q.w - q.x*q.x - q.z*q.z + q.y*q.y)*p.y + (q.z*q.y + q.y*q.z - q.x*q.w - q.w*q.x)*p.z)*j+
  99. (+ (q.z*q.x - q.y*q.w + q.x*q.z - q.w*q.y)*p.x + (q.x*q.w + q.y*q.z + q.w*q.x + q.z*q.y)*p.y - (q.y*q.y + q.z*q.z + q.w*q.w - q.x*q.x)*p.z)*k
  100. )  
  101.  
  102.  
  103. // simplify
  104.  
  105. p' = (
  106. (0 )+
  107. (+ (q.w*q.w+q.x*q.x-q.y*q.y-q.z*q.z)*p.x + (2*q.x*q.y - 2* q.z*q.w )        *p.y + (2*q.y*q.w + 2*q.x*q.z)          *p.z)*i+
  108. (+ (2*q.z*q.w +2*q.x*q.y)           *p.x + (q.w*q.w-q.x*q.x-q.z*q.z+q.y*q.y)*p.y + (2*q.z*q.y - 2*q.x*q.w)          *p.z)*j+
  109. (+ (2*q.z*q.x - 2*q.y*q.w)          *p.x + (2*q.x*q.w + 2*q.y*q.z)          *p.y - (q.y*q.y+q.z*q.z+q.w*q.w-q.x*q.x)*p.z)*k
  110. )
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement