原理如下：
R = ( R 00 R 01 R 02 R 10 R 11 R 12 R 20 R 21 R 22 ) R = \left(\begin{matrix} R00 & R01 & R02\\ R10 & R11 & R12\\ R20 & R21 & R22\\ \end{matrix}\right) R= R00R10R20R01R11R21R02R12R22
D i a g o n a l R = R 00 + R 11 + R 22 D i a g o n a l R 00 = R 00 − R 11 − R 22 D i a g o n a l R 11 = R 11 − R 00 − R 22 D i a g o n a l R 22 = R 22 − R 00 − R 11 DiagonalR = R00 + R11 +R22\\ DiagonalR00 = R00 - R11 - R22\\ DiagonalR11 = R11-R00-R22\\ DiagonalR22 = R22 - R00 - R11 DiagonalR=R00+R11+R22DiagonalR00=R00−R11−R22DiagonalR11=R11−R00−R22DiagonalR22=R22−R00−R11
分情况如下：
情况一
i
f
(
D
i
a
g
o
n
a
l
R
00
>
=
D
i
a
g
o
n
a
l
R
11
&
&
D
i
a
g
o
n
a
l
R
00
>
=
D
i
a
g
o
n
a
l
R
22
&
&
D
i
a
g
o
n
a
l
R
00
>
=
D
i
a
g
o
n
a
l
R
)
q
u
a
r
t
.
x
=
D
i
a
g
o
n
a
l
R
00
+
1
2
q
u
a
r
t
.
y
=
R
10
+
R
01
4.0
∗
q
u
a
r
t
.
x
q
u
a
r
t
.
z
=
R
20
+
R
01
4.0
∗
q
u
a
r
t
.
x
q
u
a
r
t
.
w
=
R
21
−
R
12
4.0
∗
q
u
a
r
t
.
x
if(DiagonalR00 >= DiagonalR11 \quad\&\& \quad DiagonalR00 >= DiagonalR22 \quad\&\& \quad DiagonalR00 >=DiagonalR)\\\\ \begin{aligned} &quart.x = \frac{\sqrt{DiagonalR00 + 1}}{2}\\ &quart.y = \frac{R10 + R01}{4.0*quart.x}\\ &quart.z = \frac{R20 + R01}{4.0*quart.x}\\ &quart.w = \frac{R21 - R12}{4.0*quart.x}\\ \end{aligned}
if(DiagonalR00>=DiagonalR11&&DiagonalR00>=DiagonalR22&&DiagonalR00>=DiagonalR)quart.x=2DiagonalR00+1quart.y=4.0∗quart.xR10+R01quart.z=4.0∗quart.xR20+R01quart.w=4.0∗quart.xR21−R12
情况二：
i
f
(
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
11
∣
∣
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
22
∣
∣
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
)
i
f
(
D
i
a
g
o
n
a
l
R
11
>
=
D
i
a
g
o
n
a
l
R
00
&
&
D
i
a
g
o
n
a
l
R
11
>
=
D
i
a
g
o
n
a
l
R
22
&
&
D
i
a
g
o
n
a
l
R
11
>
=
D
i
a
g
o
n
a
l
R
)
if(DiagonalR00 < DiagonalR11 \quad|| \quad DiagonalR00 < DiagonalR22 \quad|| \quad DiagonalR00 < DiagonalR)\\ if(DiagonalR11 >= DiagonalR00 \quad\&\& \quad DiagonalR11 >= DiagonalR22 \quad\&\& \quad DiagonalR11 >=DiagonalR)
if(DiagonalR00<DiagonalR11∣∣DiagonalR00<DiagonalR22∣∣DiagonalR00<DiagonalR)if(DiagonalR11>=DiagonalR00&&DiagonalR11>=DiagonalR22&&DiagonalR11>=DiagonalR)
q
u
a
r
t
.
x
=
R
10
+
R
01
4.0
∗
q
u
a
r
t
.
y
q
u
a
r
t
.
y
=
D
i
a
g
o
n
a
l
R
11
+
1
2
q
u
a
r
t
.
z
=
R
21
+
R
12
4.0
∗
q
u
a
r
t
.
y
q
u
a
r
t
.
w
=
R
02
−
R
20
4.0
∗
q
u
a
r
t
.
y
\begin{aligned} &quart.x = \frac{R10 + R01}{4.0*quart.y}\\ &quart.y = \frac{\sqrt{DiagonalR11 + 1}}{2}\\ &quart.z = \frac{R21 + R12}{4.0*quart.y}\\ &quart.w = \frac{R02 - R20}{4.0*quart.y}\\ \end{aligned}
quart.x=4.0∗quart.yR10+R01quart.y=2DiagonalR11+1quart.z=4.0∗quart.yR21+R12quart.w=4.0∗quart.yR02−R20
情况三：
i
f
(
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
11
∣
∣
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
22
∣
∣
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
)
if(DiagonalR00 < DiagonalR11 \quad|| \quad DiagonalR00 < DiagonalR22 \quad|| \quad DiagonalR00 < DiagonalR)
if(DiagonalR00<DiagonalR11∣∣DiagonalR00<DiagonalR22∣∣DiagonalR00<DiagonalR)
i f ( D i a g o n a l R 11 < D i a g o n a l R 00 ∣ ∣ D i a g o n a l R 11 < D i a g o n a l R 22 ∣ ∣ D i a g o n a l R 11 < D i a g o n a l R ) if(DiagonalR11 < DiagonalR00 \quad|| \quad DiagonalR11 < DiagonalR22 \quad|| \quad DiagonalR11 < DiagonalR) if(DiagonalR11<DiagonalR00∣∣DiagonalR11<DiagonalR22∣∣DiagonalR11<DiagonalR)
i f ( D i a g o n a l R 22 > D i a g o n a l R 00 & & D i a g o n a l R 22 > D i a g o n a l R 11 & & D i a g o n a l R 22 > D i a g o n a l R ) if(DiagonalR22 > DiagonalR00 \quad\&\& \quad DiagonalR22 > DiagonalR11 \quad\&\& \quad DiagonalR22 > DiagonalR) if(DiagonalR22>DiagonalR00&&DiagonalR22>DiagonalR11&&DiagonalR22>DiagonalR)
q u a r t . x = R 20 + R 02 4.0 ∗ q u a r t . z q u a r t . y = R 21 + R 12 4.0 ∗ q u a r t . z q u a r t . z = D i a g o n a l R 22 + 1 2 q u a r t . w = R 10 − R 01 4.0 ∗ q u a r t . z \begin{aligned} &quart.x = \frac{R20 + R02}{4.0*quart.z}\\ &quart.y = \frac{R21 + R12}{4.0*quart.z}\\ &quart.z = \frac{\sqrt{DiagonalR22 + 1}}{2}\\ &quart.w = \frac{R10 - R01}{4.0*quart.z}\\ \end{aligned} quart.x=4.0∗quart.zR20+R02quart.y=4.0∗quart.zR21+R12quart.z=2DiagonalR22+1quart.w=4.0∗quart.zR10−R01
情况四：
i
f
(
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
11
∣
∣
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
22
∣
∣
D
i
a
g
o
n
a
l
R
00
<
D
i
a
g
o
n
a
l
R
)
if(DiagonalR00 < DiagonalR11 \quad|| \quad DiagonalR00 < DiagonalR22\quad|| \quad DiagonalR00 < DiagonalR)
if(DiagonalR00<DiagonalR11∣∣DiagonalR00<DiagonalR22∣∣DiagonalR00<DiagonalR)
i f ( D i a g o n a l R 11 < D i a g o n a l R 00 ∣ ∣ D i a g o n a l R 11 < D i a g o n a l R 22 ∣ ∣ D i a g o n a l R 11 < D i a g o n a l R ) if(DiagonalR11 < DiagonalR00 \quad|| \quad DiagonalR11 < DiagonalR22 \quad|| \quad DiagonalR11 < DiagonalR) if(DiagonalR11<DiagonalR00∣∣DiagonalR11<DiagonalR22∣∣DiagonalR11<DiagonalR)
i f ( D i a g o n a l R 22 < = D i a g o n a l R 00 ∣ ∣ D i a g o n a l R 22 < = D i a g o n a l R 11 ∣ ∣ D i a g o n a l R 22 < = D i a g o n a l R ) if(DiagonalR22 <= DiagonalR00 \quad|| \quad DiagonalR22 <= DiagonalR11 \quad|| \quad DiagonalR22 <= DiagonalR) if(DiagonalR22<=DiagonalR00∣∣DiagonalR22<=DiagonalR11∣∣DiagonalR22<=DiagonalR)
q u a r t . x = R 21 − R 12 4.0 ∗ q u a r t . w q u a r t . y = R 02 − R 20 4.0 ∗ q u a r t . w q u a r t . z = R 10 − R 01 4.0 ∗ q u a r t . w q u a r t . w = D i a g o n a l R + 1 2 \begin{aligned} &quart.x = \frac{R21 - R12}{4.0*quart.w}\\ &quart.y = \frac{R02 - R20}{4.0*quart.w}\\ &quart.z = \frac{R10 - R01}{4.0*quart.w}\\ &quart.w = \frac{\sqrt{DiagonalR + 1}}{2}\\ \end{aligned} quart.x=4.0∗quart.wR21−R12quart.y=4.0∗quart.wR02−R20quart.z=4.0∗quart.wR10−R01quart.w=2DiagonalR+1
void RtoQuart(double* R, double* oQ)
{
double R00 = R[0]; double R01 = R[1]; double R02 = R[2];
double R10 = R[3]; double R11 = R[4]; double R12 = R[5];
double R20 = R[6]; double R21 = R[7]; double R22 = R[8];
double DiagonalR = R00 + R11 + R22;
double DiagonalR00 = R00 + R00 - DiagonalR;
double DiagonalR11 = R11 + R11 - DiagonalR;
double DiagonalR22 = R22 + R22 - DiagonalR;
double Quart[4] = { 0.0 };
double Value;
double Factor;
if (DiagonalR00 < DiagonalR11 || DiagonalR00 < DiagonalR22 || DiagonalR00 < DiagonalR)
{
if (DiagonalR11 < DiagonalR00 || DiagonalR11 < DiagonalR22 || DiagonalR11 < DiagonalR)
{
if (DiagonalR22 <= DiagonalR00 || DiagonalR22 <= DiagonalR11 || DiagonalR22 <= DiagonalR)
{
Value = sqrt((DiagonalR + 1.0) * 0.25);
Factor = 1.0 / (Value * 4.0);
Quart[0] = (R21 - R12) * Factor;
Quart[1] = (R02 - R20) * Factor;
Quart[2] = (R10 - R01) * Factor;
Quart[3] = Value;
}
else
{
Value = sqrt((DiagonalR22 + 1.0) * 0.25);
Factor = 1.0 / (Value * 4.0);
Quart[0] = (R20 + R02) * Factor;
Quart[1] = (R21 + R12) * Factor;
Quart[2] = Value;
Quart[3] = (R10 - R01) * Factor;
}
}
else
{
Value = sqrt((DiagonalR11 + 1.0) * 0.25);
Factor = 1.0 / (Value * 4.0);
Quart[0] = (R10 + R01) * Factor;
Quart[1] = Value;
Quart[2] = (R21 + R12) * Factor;
Quart[3] = (R02 - R20)* Factor;
}
}
else
{
Value = sqrt((DiagonalR00 + 1.0) * 0.25);
Factor = 1.0 / (Value * 4.0);
Quart[0] = Value;
Quart[1] = (R10 + R01) * Factor;
Quart[2] = (R20 + R02) * Factor;
Quart[3] = (R21 - R12) * Factor;
}
double QLen = sqrt(Quart[0] * Quart[0] + Quart[1] * Quart[1] + Quart[2] * Quart[2] + Quart[3] * Quart[3]);
Quart[0] = Quart[0] / QLen;
Quart[1] = Quart[1] / QLen;
Quart[2] = Quart[2] / QLen;
Quart[3] = Quart[3] / QLen;
if (Quart[3] < 0)
{
Quart[0] = -Quart[0];
Quart[1] = -Quart[1];
Quart[2] = -Quart[2];
Quart[3] = -Quart[3];
}
oQ[0] = Quart[0];
oQ[1] = Quart[1];
oQ[2] = Quart[2];
oQ[3] = Quart[3];
}
382
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
