Guest User

Untitled

a guest
Mar 22nd, 2018
94
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.29 KB | None | 0 0
  1. T = 4 - 4x^2 - 4y^2 - 4z^2
  2. = 4( 1 - x^2 - y^2 - z^2 )
  3. = mat[0] + mat[5] + mat[10] + 1
  4.  
  5. S = 0.5 / sqrt(T)
  6. W = 0.25 / S
  7. X = ( mat[9] - mat[6] ) * S
  8. Y = ( mat[2] - mat[8] ) * S
  9. Z = ( mat[4] - mat[1] ) * S
  10.  
  11. S = sqrt( 1.0 + mr[0] - mr[5] - mr[10] ) * 2;
  12. Qx = 0.5 / S;
  13. Qy = (mr[1] + mr[4] ) / S;
  14. Qz = (mr[2] + mr[8] ) / S;
  15. Qw = (mr[6] + mr[9] ) / S;
  16.  
  17. S = sqrt( 1.0 + mr[5] - mr[0] - mr[10] ) * 2;
  18. Qx = (mr[1] + mr[4] ) / S;
  19. Qy = 0.5 / S;
  20. Qz = (mr[6] + mr[9] ) / S;
  21. Qw = (mr[2] + mr[8] ) / S;
  22.  
  23. S = sqrt( 1.0 + mr[10] - mr[0] - mr[5] ) * 2;
  24. Qx = (mr[2] + mr[8] ) / S;
  25. Qy = (mr[6] + mr[9] ) / S;
  26. Qz = 0.5 / S;
  27. Qw = (mr[1] + mr[4] ) / S;
  28.  
  29. Q = | Qx Qy Qz Qw |
  30.  
  31. Needs["Quaternions`"]
  32. qr[vec_, u_, a_] :=
  33. Module[{qv, qu, r},
  34. qv = ReplacePart[Join[{0}, vec], 0 -> Quaternion];
  35. qu = ReplacePart[Join[{Cos[a/2]}, Sin[a/2] Normalize[u]],
  36. 0 -> Quaternion];
  37. r = qu ** qv ** Conjugate[qu];
  38. N@FullSimplify[ReplacePart[r, 0 -> List][[2 ;; 4]]]]
  39.  
  40. Manipulate[Graphics3D[
  41. {{Red, Line[{{0, 0, 0}, {1, 1, 1}}]},
  42. {Blue,
  43. Arrow[{{0, 0, 0}, qr[{1, 1, 1}, {m, n, p}, an Degree]}]}, {Black,
  44. Arrow[{{0, 0, 0}, {m, n, p}}]}, {Purple, Thickness[0.02],
  45. Line[Table[
  46. qr[{1, 1, 1}, {m, n, p}, j], {j, 0, 2 Pi, 2 Pi/20}]]}}], {{an,
  47. 0}, 0, 360,
  48. AngularGauge[##, GaugeLabels -> {"Degrees", "Value"}] &,
  49. ControlPlacement -> Left}, {m, 0.1, 1}, {n, 0.1, 1}, {p, 0.1, 1}]
  50.  
  51. Needs["Quaternions`"];
  52.  
  53. quaternionToRotation[qq_] /; QuaternionQ[ToQuaternion[qq]] :=
  54. Module[{q = ToQuaternion[qq], aim, r},
  55. aim = AbsIJK[q]; r = Re[q];
  56. If[aim == 0, Return[IdentityMatrix[3], Module]];
  57. First[LinearAlgebra`Private`MatrixPolynomial[
  58. {Prepend[2 aim {r, aim}/(r^2 + aim^2), 1]},
  59. -LeviCivitaTensor[3, List].(Rest[List @@ q]/aim)]]]
  60.  
  61. rotationToQuaternion[m_?OrthogonalMatrixQ] := Module[{d, v, xm},
  62. d = Diagonal[m];
  63. v = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]}};
  64. xm = IdentityMatrix[4] +
  65. DiagonalMatrix[{{1, 1, 1}, {1, -1, -1}, {-1, 1, -1}, {-1, -1, 1}}.d] +
  66. ArrayFlatten[{{0, v}, {Transpose[v], 2 Symmetrize[m - DiagonalMatrix[d]]}}];
  67. Sign[Apply[Quaternion, First[MaximalBy[xm, Norm]]]]]
  68.  
  69. rotationToQuaternion2[m_?OrthogonalMatrixQ] := Module[{d, v, xm},
  70. d = Diagonal[m];
  71. v = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]}};
  72. xm = DiagonalMatrix[{{1, 1, 1}, {1, -1, -1}, {-1, 1, -1}, {-1, -1, 1}}.d] +
  73. ArrayFlatten[{{0, v}, {Transpose[v], 2 Symmetrize[m - DiagonalMatrix[d]]}}];
  74. Sign[Apply[Quaternion, First[Eigenvectors[xm, 1]]]]]
  75.  
  76. BlockRandom[SeedRandom[42]; (* for reproducibility *)
  77. tstq = Apply[Quaternion, Normalize[RandomVariate[NormalDistribution[], 4]]]];
  78.  
  79. Norm[rotationToQuaternion[quaternionToRotation[tstq]] + tstq]
  80. 1.54074*10^-32
  81.  
  82. Norm[rotationToQuaternion2[quaternionToRotation[tstq]] - tstq]
  83. 3.08149*10^-32
  84.  
  85. BlockRandom[SeedRandom[42]; (* for reproducibility *)
  86. tst = Orthogonalize[# Det[#]] &[RandomVariate[NormalDistribution[], {3, 3}]]];
  87.  
  88. Norm[quaternionToRotation[rotationToQuaternion[tst]] - tst]
  89. 5.23541*10^-16
  90.  
  91. Norm[quaternionToRotation[rotationToQuaternion2[tst]] - tst]
  92. 2.37306*10^-16
Add Comment
Please, Sign In to add comment