126 |
126 |
_z = axis.z() * sht;
|
127 |
127 |
}
|
128 |
128 |
|
129 |
|
void fromMatrix(Matrix<3> m)
|
|
129 |
void fromMatrix(const Matrix<3>& m)
|
130 |
130 |
{
|
131 |
|
float tr = m(0, 0) + m(1, 1) + m(2, 2);
|
|
131 |
double tr = m.trace();
|
132 |
132 |
|
133 |
|
float S = 0.0;
|
|
133 |
double S;
|
134 |
134 |
if (tr > 0)
|
135 |
135 |
{
|
136 |
136 |
S = sqrt(tr+1.0) * 2;
|
... | ... | |
139 |
139 |
_y = (m(0, 2) - m(2, 0)) / S;
|
140 |
140 |
_z = (m(1, 0) - m(0, 1)) / S;
|
141 |
141 |
}
|
142 |
|
else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))
|
|
142 |
else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
|
143 |
143 |
{
|
144 |
144 |
S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
|
145 |
145 |
_w = (m(2, 1) - m(1, 2)) / S;
|
... | ... | |
147 |
147 |
_y = (m(0, 1) + m(1, 0)) / S;
|
148 |
148 |
_z = (m(0, 2) + m(2, 0)) / S;
|
149 |
149 |
}
|
150 |
|
else if (m(1, 1) < m(2, 2))
|
|
150 |
else if (m(1, 1) > m(2, 2))
|
151 |
151 |
{
|
152 |
152 |
S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
|
153 |
153 |
_w = (m(0, 2) - m(2, 0)) / S;
|