Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- T = 4 - 4x^2 - 4y^2 - 4z^2
- = 4( 1 - x^2 - y^2 - z^2 )
- = mat[0] + mat[5] + mat[10] + 1
- S = 0.5 / sqrt(T)
- W = 0.25 / S
- X = ( mat[9] - mat[6] ) * S
- Y = ( mat[2] - mat[8] ) * S
- Z = ( mat[4] - mat[1] ) * S
- S = sqrt( 1.0 + mr[0] - mr[5] - mr[10] ) * 2;
- Qx = 0.5 / S;
- Qy = (mr[1] + mr[4] ) / S;
- Qz = (mr[2] + mr[8] ) / S;
- Qw = (mr[6] + mr[9] ) / S;
- S = sqrt( 1.0 + mr[5] - mr[0] - mr[10] ) * 2;
- Qx = (mr[1] + mr[4] ) / S;
- Qy = 0.5 / S;
- Qz = (mr[6] + mr[9] ) / S;
- Qw = (mr[2] + mr[8] ) / S;
- S = sqrt( 1.0 + mr[10] - mr[0] - mr[5] ) * 2;
- Qx = (mr[2] + mr[8] ) / S;
- Qy = (mr[6] + mr[9] ) / S;
- Qz = 0.5 / S;
- Qw = (mr[1] + mr[4] ) / S;
- Q = | Qx Qy Qz Qw |
- Needs["Quaternions`"]
- qr[vec_, u_, a_] :=
- Module[{qv, qu, r},
- qv = ReplacePart[Join[{0}, vec], 0 -> Quaternion];
- qu = ReplacePart[Join[{Cos[a/2]}, Sin[a/2] Normalize[u]],
- 0 -> Quaternion];
- r = qu ** qv ** Conjugate[qu];
- N@FullSimplify[ReplacePart[r, 0 -> List][[2 ;; 4]]]]
- Manipulate[Graphics3D[
- {{Red, Line[{{0, 0, 0}, {1, 1, 1}}]},
- {Blue,
- Arrow[{{0, 0, 0}, qr[{1, 1, 1}, {m, n, p}, an Degree]}]}, {Black,
- Arrow[{{0, 0, 0}, {m, n, p}}]}, {Purple, Thickness[0.02],
- Line[Table[
- qr[{1, 1, 1}, {m, n, p}, j], {j, 0, 2 Pi, 2 Pi/20}]]}}], {{an,
- 0}, 0, 360,
- AngularGauge[##, GaugeLabels -> {"Degrees", "Value"}] &,
- ControlPlacement -> Left}, {m, 0.1, 1}, {n, 0.1, 1}, {p, 0.1, 1}]
- Needs["Quaternions`"];
- quaternionToRotation[qq_] /; QuaternionQ[ToQuaternion[qq]] :=
- Module[{q = ToQuaternion[qq], aim, r},
- aim = AbsIJK[q]; r = Re[q];
- If[aim == 0, Return[IdentityMatrix[3], Module]];
- First[LinearAlgebra`Private`MatrixPolynomial[
- {Prepend[2 aim {r, aim}/(r^2 + aim^2), 1]},
- -LeviCivitaTensor[3, List].(Rest[List @@ q]/aim)]]]
- rotationToQuaternion[m_?OrthogonalMatrixQ] := Module[{d, v, xm},
- d = Diagonal[m];
- v = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]}};
- xm = IdentityMatrix[4] +
- DiagonalMatrix[{{1, 1, 1}, {1, -1, -1}, {-1, 1, -1}, {-1, -1, 1}}.d] +
- ArrayFlatten[{{0, v}, {Transpose[v], 2 Symmetrize[m - DiagonalMatrix[d]]}}];
- Sign[Apply[Quaternion, First[MaximalBy[xm, Norm]]]]]
- rotationToQuaternion2[m_?OrthogonalMatrixQ] := Module[{d, v, xm},
- d = Diagonal[m];
- v = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]}};
- xm = DiagonalMatrix[{{1, 1, 1}, {1, -1, -1}, {-1, 1, -1}, {-1, -1, 1}}.d] +
- ArrayFlatten[{{0, v}, {Transpose[v], 2 Symmetrize[m - DiagonalMatrix[d]]}}];
- Sign[Apply[Quaternion, First[Eigenvectors[xm, 1]]]]]
- BlockRandom[SeedRandom[42]; (* for reproducibility *)
- tstq = Apply[Quaternion, Normalize[RandomVariate[NormalDistribution[], 4]]]];
- Norm[rotationToQuaternion[quaternionToRotation[tstq]] + tstq]
- 1.54074*10^-32
- Norm[rotationToQuaternion2[quaternionToRotation[tstq]] - tstq]
- 3.08149*10^-32
- BlockRandom[SeedRandom[42]; (* for reproducibility *)
- tst = Orthogonalize[# Det[#]] &[RandomVariate[NormalDistribution[], {3, 3}]]];
- Norm[quaternionToRotation[rotationToQuaternion[tst]] - tst]
- 5.23541*10^-16
- Norm[quaternionToRotation[rotationToQuaternion2[tst]] - tst]
- 2.37306*10^-16
Add Comment
Please, Sign In to add comment