20 #ifndef __mia_3d_matrix_hh
21 #define __mia_3d_matrix_hh
97 void print( std::ostream& os)
const;
151 template <
typename T>
156 template <
typename T>
160 template <
typename T>
168 template <
typename T>
176 template <
typename T>
177 template <
typename I>
183 template <
typename T>
189 template <
typename T>
194 template <
typename T>
197 os <<
"<" << this->x <<
", " << this->y <<
", " << this->z <<
" >";
200 template <
typename T>
201 std::ostream& operator << (std::ostream& os, const T3DMatrix<T>& m)
207 template <
typename T>
216 template <
typename T>
224 template <
typename T>
230 template <
typename T>
234 m.
x.y * x.
x + m.
y.y * x.
y + m.
z.y * x.
z,
235 m.
x.z * x.
x + m.
y.z * x.
y + m.
z.z * x.
z);
238 template <
typename T>
242 m.
x.x * x.
x.y + m.
x.y * x.
y.y + m.
x.z * x.
z.y,
243 m.
x.x * x.
x.z + m.
x.y * x.
y.z + m.
x.z * x.
z.z),
245 m.
y.x * x.
x.y + m.
y.y * x.
y.y + m.
y.z * x.
z.y,
246 m.
y.x * x.
x.z + m.
y.y * x.
y.z + m.
y.z * x.
z.z),
248 m.
z.x * x.
x.y + m.
z.y * x.
y.y + m.
z.z * x.
z.y,
249 m.
z.x * x.
x.z + m.
z.y * x.
y.z + m.
z.z * x.
z.z));
253 template <
typename T>
257 this->get_eigenvalues(ev);
258 cvdebug()<<
"Matrix = "<< *
this <<
", Rank: eigenvalues: " << ev <<
"\n";
273 return a > 0.0 ? pow(a,1.0/3.0) : - pow(-a, 1.0/ 3.0);
280 return dot(this->x,
T3DVector<T>(this->y.y * this->z.z - this->z.y * this->y.z,
281 this->y.z * this->z.x - this->y.x * this->z.z,
282 this->y.x * this->z.y - this->y.y * this->z.x));
286 template <
typename T>
291 double t = - get_det();
292 double s = this->x.x * this->y.y + this->z.z * this->y.y + this->x.x * this->z.z -
293 this->x.y * this->y.x - this->x.z * this->z.x - this->y.z * this->z.y;
295 double r = - this->x.x - this->y.y - this->z.z;
299 double p = s - r * r / 3.0;
300 double q = ( 27.0 * t - 9.0 * r * s + 2.0 * r * r * r ) / 27.0;
302 double diskr = q *q / 4.0 + p * p * p / 27.0;
304 cvdebug() <<
"discr =" << diskr <<
"\n";
305 if ( diskr > 1e-6 ) {
307 double sqrt_discr = sqrt(diskr);
308 double u =
cubrt( - q/2.0 + sqrt_discr );
309 double v =
cubrt( - q/2.0 - sqrt_discr );
310 result.
x = u + v - r / 3.0;
311 result.
y = -(u + v) / 2.0 - r / 3.0;
312 result.
z = (u - v)/2.0 * sqrt(3.0f);
317 std::vector<double> res(3);
318 if ( diskr < -1e-6) {
319 double rho = sqrt(-p*p*p/ 27.0);
320 double cphi = - q / ( 2.0 * rho);
321 double phi = acos(cphi)/3.0;
322 double sqrt_p = 2 *
cubrt(rho);
323 res[0] = sqrt_p * cos(phi)- r/3.;
324 res[1] = sqrt_p * cos(phi + M_PI * 2.0 / 3.0) - r/3.;
325 res[2] = sqrt_p * cos(phi + M_PI * 4.0 / 3.0) - r/3.;
328 double u =
cubrt( - q/2.0 );
329 res[0] = 2.0 * u - r / 3.0;
330 res[1] = - u - r / 3.0;
335 std::sort(res.begin(), res.end(), [](
double x,
double y){
return std::fabs(x) >
std::fabs(y);});
337 for_each(res.begin(), res.end(), [](
double& x){
if (
std::fabs(x) < 1e-12) x = 0.0;});
360 bool solve_2x2(T a11, T a12,T b1,T a21, T a22,T b2,T *x1,T *x2)
362 T h1 = a11 * a22 - a12 * a21;
366 *x1 = (b1 * a22 - b2 * a12) / h1;
367 *x2 = (b2 * a11 - b1 * a21) / h1;
377 template <
typename T>
388 float x = std::abs(M.
x.x)+std::abs(M.
y.x)+std::abs(M.
z.x);
389 float y = std::abs(M.
x.y)+std::abs(M.
y.y)+std::abs(M.
z.y);
390 float z = std::abs(M.
x.z)+std::abs(M.
y.z)+std::abs(M.
z.z);
437 for (
int i = 0; i < 3 && !good; i++) {
438 good =
solve_2x2(M[l[i].a][col.
x],M[l[i].
a][col.
y],-M[l[i].
a][col.
z],
439 M[l[i].
b][col.
x],M[l[i].
b][col.
y],-M[l[i].
b][col.
z],
447 if ((M * v).norm2() > 1e-5) {
451 fprintf(stderr,
"WARNING: rank of A-ev*I\n numerical > 2");