2 #error "CholeskyDecomp is now in ROOT"
3 #ifndef ROOT_Math_CholeskyDecomp
4 #define ROOT_Math_CholeskyDecomp
25 namespace CholeskyDecompHelpers {
28 template<
class F,
unsigned N,
class M>
struct _inverter;
29 template<
class F,
unsigned N,
class V>
struct _solver;
81 {
return fArr[((i * (i + 1)) / 2) + j]; }
84 {
return fArr[((i * (i + 1)) / 2) + j]; }
98 fOk = _decomposer<F, N, M>()(
fL, m);
115 fOk = _decomposer<F, N, PackedArrayAdapter<G> >()(
124 operator bool()
const {
return fOk; }
135 template<
class V>
bool Solve(V& rhs)
const
138 if (
fOk) _solver<F,N,V>()(rhs,
fL);
return fOk;
151 if (
fOk) _inverter<F,N,M>()(m,
fL);
return fOk;
170 _inverter<F,N,PackedArrayAdapter<G> >()(adapted,
fL);
177 namespace CholeskyDecompHelpers {
179 template<
class F,
unsigned N,
class M>
struct _decomposer
198 for (
unsigned i = 0;
i <
N; base1 += ++
i) {
202 for (
unsigned j = 0;
j <
i; base2 += ++
j) {
204 for (
unsigned k =
j;
k--; )
205 tmp -= base1[
k] * base2[
k];
206 base1[
j] = tmp *= base2[
j];
208 tmpdiag += tmp *
tmp;
211 tmpdiag =
src(i, i) - tmpdiag;
213 if (tmpdiag <=
F(0))
return false;
221 template<
class F,
unsigned N,
class M>
struct _inverter
227 F l[
N * (
N + 1) / 2];
231 for (
unsigned i = 1;
i <
N; base1 += ++
i) {
232 for (
unsigned j = 0;
j <
i; ++
j) {
234 const F *
base2 = &
l[(i * (i - 1)) / 2];
235 for (
unsigned k = i;
k-- >
j; base2 -=
k)
236 tmp -= base1[
k] * base2[j];
237 base1[
j] = tmp * base1[
i];
242 for (
unsigned i = N;
i--; ) {
243 for (
unsigned j =
i + 1;
j--; ) {
245 base1 = &
l[(N * (N - 1)) / 2];
246 for (
unsigned k = N;
k-- >
i; base1 -=
k)
247 tmp += base1[i] * base1[
j];
255 template<
class F,
unsigned N,
class V>
struct _solver
261 for (
unsigned k = 0;
k <
N; ++
k) {
262 const unsigned base = (
k * (
k + 1)) / 2;
264 for (
unsigned i =
k;
i--; )
265 sum += rhs[
i] * l[base +
i];
267 rhs[
k] = (rhs[
k] - sum) * l[base +
k];
270 for (
unsigned k = N;
k--; ) {
272 for (
unsigned i = N; --
i >
k; )
273 sum += rhs[
i] * l[(
i * (
i + 1)) / 2 +
k];
275 rhs[
k] = (rhs[
k] - sum) * l[(k * (k + 1)) / 2 + k];
286 if (
src(0,0) <=
F(0))
return false;
288 dst[1] =
src(1,0) * dst[0];
289 dst[2] =
src(1,1) - dst[1] * dst[1];
290 if (dst[2] <=
F(0))
return false;
292 dst[3] =
src(2,0) * dst[0];
293 dst[4] = (
src(2,1) - dst[1] * dst[3]) * dst[2];
294 dst[5] =
src(2,2) - (dst[3] * dst[3] + dst[4] * dst[4]);
295 if (dst[5] <=
F(0))
return false;
297 dst[6] =
src(3,0) * dst[0];
298 dst[7] = (
src(3,1) - dst[1] * dst[6]) * dst[2];
299 dst[8] = (
src(3,2) - dst[3] * dst[6] - dst[4] * dst[7]) * dst[5];
300 dst[9] =
src(3,3) - (dst[6] * dst[6] + dst[7] * dst[7] + dst[8] * dst[8]);
301 if (dst[9] <=
F(0))
return false;
303 dst[10] =
src(4,0) * dst[0];
304 dst[11] = (
src(4,1) - dst[1] * dst[10]) * dst[2];
305 dst[12] = (
src(4,2) - dst[3] * dst[10] - dst[4] * dst[11]) * dst[5];
306 dst[13] = (
src(4,3) - dst[6] * dst[10] - dst[7] * dst[11] - dst[8] * dst[12]) * dst[9];
307 dst[14] =
src(4,4) - (dst[10]*dst[10]+dst[11]*dst[11]+dst[12]*dst[12]+dst[13]*dst[13]);
308 if (dst[14] <=
F(0))
return false;
310 dst[15] =
src(5,0) * dst[0];
311 dst[16] = (
src(5,1) - dst[1] * dst[15]) * dst[2];
312 dst[17] = (
src(5,2) - dst[3] * dst[15] - dst[4] * dst[16]) * dst[5];
313 dst[18] = (
src(5,3) - dst[6] * dst[15] - dst[7] * dst[16] - dst[8] * dst[17]) * dst[9];
314 dst[19] = (
src(5,4) - dst[10] * dst[15] - dst[11] * dst[16] - dst[12] * dst[17] - dst[13] * dst[18]) * dst[14];
315 dst[20] =
src(5,5) - (dst[15]*dst[15]+dst[16]*dst[16]+dst[17]*dst[17]+dst[18]*dst[18]+dst[19]*dst[19]);
316 if (dst[20] <=
F(0))
return false;
327 if (
src(0,0) <=
F(0))
return false;
329 dst[1] =
src(1,0) * dst[0];
330 dst[2] =
src(1,1) - dst[1] * dst[1];
331 if (dst[2] <=
F(0))
return false;
333 dst[3] =
src(2,0) * dst[0];
334 dst[4] = (
src(2,1) - dst[1] * dst[3]) * dst[2];
335 dst[5] =
src(2,2) - (dst[3] * dst[3] + dst[4] * dst[4]);
336 if (dst[5] <=
F(0))
return false;
338 dst[6] =
src(3,0) * dst[0];
339 dst[7] = (
src(3,1) - dst[1] * dst[6]) * dst[2];
340 dst[8] = (
src(3,2) - dst[3] * dst[6] - dst[4] * dst[7]) * dst[5];
341 dst[9] =
src(3,3) - (dst[6] * dst[6] + dst[7] * dst[7] + dst[8] * dst[8]);
342 if (dst[9] <=
F(0))
return false;
344 dst[10] =
src(4,0) * dst[0];
345 dst[11] = (
src(4,1) - dst[1] * dst[10]) * dst[2];
346 dst[12] = (
src(4,2) - dst[3] * dst[10] - dst[4] * dst[11]) * dst[5];
347 dst[13] = (
src(4,3) - dst[6] * dst[10] - dst[7] * dst[11] - dst[8] * dst[12]) * dst[9];
348 dst[14] =
src(4,4) - (dst[10]*dst[10]+dst[11]*dst[11]+dst[12]*dst[12]+dst[13]*dst[13]);
349 if (dst[14] <=
F(0))
return false;
360 if (
src(0,0) <=
F(0))
return false;
362 dst[1] =
src(1,0) * dst[0];
363 dst[2] =
src(1,1) - dst[1] * dst[1];
364 if (dst[2] <=
F(0))
return false;
366 dst[3] =
src(2,0) * dst[0];
367 dst[4] = (
src(2,1) - dst[1] * dst[3]) * dst[2];
368 dst[5] =
src(2,2) - (dst[3] * dst[3] + dst[4] * dst[4]);
369 if (dst[5] <=
F(0))
return false;
371 dst[6] =
src(3,0) * dst[0];
372 dst[7] = (
src(3,1) - dst[1] * dst[6]) * dst[2];
373 dst[8] = (
src(3,2) - dst[3] * dst[6] - dst[4] * dst[7]) * dst[5];
374 dst[9] =
src(3,3) - (dst[6] * dst[6] + dst[7] * dst[7] + dst[8] * dst[8]);
375 if (dst[9] <=
F(0))
return false;
386 if (
src(0,0) <=
F(0))
return false;
388 dst[1] =
src(1,0) * dst[0];
389 dst[2] =
src(1,1) - dst[1] * dst[1];
390 if (dst[2] <=
F(0))
return false;
392 dst[3] =
src(2,0) * dst[0];
393 dst[4] = (
src(2,1) - dst[1] * dst[3]) * dst[2];
394 dst[5] =
src(2,2) - (dst[3] * dst[3] + dst[4] * dst[4]);
395 if (dst[5] <=
F(0))
return false;
406 if (
src(0,0) <=
F(0))
return false;
408 dst[1] =
src(1,0) * dst[0];
409 dst[2] =
src(1,1) - dst[1] * dst[1];
410 if (dst[2] <=
F(0))
return false;
421 if (
src(0,0) <=
F(0))
return false;
440 const F li21 = -src[1] * src[0] * src[2];
441 const F li32 = -src[4] * src[2] * src[5];
442 const F li31 = (src[1] * src[4] * src[2] - src[3]) * src[0] * src[5];
443 const F li43 = -src[8] * src[9] * src[5];
444 const F li42 = (src[4] * src[8] * src[5] - src[7]) * src[2] * src[9];
445 const F li41 = (-src[1] * src[4] * src[8] * src[2] * src[5] +
446 src[1] * src[7] * src[2] + src[3] * src[8] * src[5] - src[6]) * src[0] * src[9];
447 const F li54 = -src[13] * src[14] * src[9];
448 const F li53 = (src[13] * src[8] * src[9] - src[12]) * src[5] * src[14];
449 const F li52 = (-src[4] * src[8] * src[13] * src[5] * src[9] +
450 src[4] * src[12] * src[5] + src[7] * src[13] * src[9] - src[11]) * src[2] * src[14];
451 const F li51 = (src[1]*src[4]*src[8]*src[13]*src[2]*src[5]*src[9] -
452 src[13]*src[8]*src[3]*src[9]*src[5] - src[12]*src[4]*src[1]*src[2]*src[5] - src[13]*src[7]*src[1]*src[9]*src[2] +
453 src[11]*src[1]*src[2] + src[12]*src[3]*src[5] + src[13]*src[6]*src[9] -src[10]) * src[0] * src[14];
454 const F li65 = -src[19] * src[20] * src[14];
455 const F li64 = (src[19] * src[13] * src[14] - src[18]) * src[9] * src[20];
456 const F li63 = (-src[8] * src[13] * src[19] * src[9] * src[14] +
457 src[8] * src[18] * src[9] + src[12] * src[19] * src[14] - src[17]) * src[5] * src[20];
458 const F li62 = (src[4]*src[8]*src[13]*src[19]*src[5]*src[9]*src[14] -
459 src[18]*src[8]*src[4]*src[9]*src[5] - src[19]*src[12]*src[4]*src[14]*src[5] -src[19]*src[13]*src[7]*src[14]*src[9] +
460 src[17]*src[4]*src[5] + src[18]*src[7]*src[9] + src[19]*src[11]*src[14] - src[16]) * src[2] * src[20];
461 const F li61 = (-src[19]*src[13]*src[8]*src[4]*src[1]*src[2]*src[5]*src[9]*src[14] +
462 src[18]*src[8]*src[4]*src[1]*src[2]*src[5]*src[9] + src[19]*src[12]*src[4]*src[1]*src[2]*src[5]*src[14] +
463 src[19]*src[13]*src[7]*src[1]*src[2]*src[9]*src[14] + src[19]*src[13]*src[8]*src[3]*src[5]*src[9]*src[14] -
464 src[17]*src[4]*src[1]*src[2]*src[5] - src[18]*src[7]*src[1]*src[2]*src[9] - src[19]*src[11]*src[1]*src[2]*src[14] -
465 src[18]*src[8]*src[3]*src[5]*src[9] - src[19]*src[12]*src[3]*src[5]*src[14] - src[19]*src[13]*src[6]*src[9]*src[14] +
466 src[16]*src[1]*src[2] + src[17]*src[3]*src[5] + src[18]*src[6]*src[9] + src[19]*src[10]*src[14] - src[15]) *
469 dst(0,0) = li61*li61 + li51*li51 + li41*li41 + li31*li31 + li21*li21 + src[0]*src[0];
470 dst(1,0) = li61*li62 + li51*li52 + li41*li42 + li31*li32 + li21*src[2];
471 dst(1,1) = li62*li62 + li52*li52 + li42*li42 + li32*li32 + src[2]*src[2];
472 dst(2,0) = li61*li63 + li51*li53 + li41*li43 + li31*src[5];
473 dst(2,1) = li62*li63 + li52*li53 + li42*li43 + li32*src[5];
474 dst(2,2) = li63*li63 + li53*li53 + li43*li43 + src[5]*src[5];
475 dst(3,0) = li61*li64 + li51*li54 + li41*src[9];
476 dst(3,1) = li62*li64 + li52*li54 + li42*src[9];
477 dst(3,2) = li63*li64 + li53*li54 + li43*src[9];
478 dst(3,3) = li64*li64 + li54*li54 + src[9]*src[9];
479 dst(4,0) = li61*li65 + li51*src[14];
480 dst(4,1) = li62*li65 + li52*src[14];
481 dst(4,2) = li63*li65 + li53*src[14];
482 dst(4,3) = li64*li65 + li54*src[14];
483 dst(4,4) = li65*li65 + src[14]*src[14];
484 dst(5,0) = li61*src[20];
485 dst(5,1) = li62*src[20];
486 dst(5,2) = li63*src[20];
487 dst(5,3) = li64*src[20];
488 dst(5,4) = li65*src[20];
489 dst(5,5) = src[20]*src[20];
498 const F li21 = -src[1] * src[0] * src[2];
499 const F li32 = -src[4] * src[2] * src[5];
500 const F li31 = (src[1] * src[4] * src[2] - src[3]) * src[0] * src[5];
501 const F li43 = -src[8] * src[9] * src[5];
502 const F li42 = (src[4] * src[8] * src[5] - src[7]) * src[2] * src[9];
503 const F li41 = (-src[1] * src[4] * src[8] * src[2] * src[5] +
504 src[1] * src[7] * src[2] + src[3] * src[8] * src[5] - src[6]) * src[0] * src[9];
505 const F li54 = -src[13] * src[14] * src[9];
506 const F li53 = (src[13] * src[8] * src[9] - src[12]) * src[5] * src[14];
507 const F li52 = (-src[4] * src[8] * src[13] * src[5] * src[9] +
508 src[4] * src[12] * src[5] + src[7] * src[13] * src[9] - src[11]) * src[2] * src[14];
509 const F li51 = (src[1]*src[4]*src[8]*src[13]*src[2]*src[5]*src[9] -
510 src[13]*src[8]*src[3]*src[9]*src[5] - src[12]*src[4]*src[1]*src[2]*src[5] - src[13]*src[7]*src[1]*src[9]*src[2] +
511 src[11]*src[1]*src[2] + src[12]*src[3]*src[5] + src[13]*src[6]*src[9] -src[10]) * src[0] * src[14];
513 dst(0,0) = li51*li51 + li41*li41 + li31*li31 + li21*li21 + src[0]*src[0];
514 dst(1,0) = li51*li52 + li41*li42 + li31*li32 + li21*src[2];
515 dst(1,1) = li52*li52 + li42*li42 + li32*li32 + src[2]*src[2];
516 dst(2,0) = li51*li53 + li41*li43 + li31*src[5];
517 dst(2,1) = li52*li53 + li42*li43 + li32*src[5];
518 dst(2,2) = li53*li53 + li43*li43 + src[5]*src[5];
519 dst(3,0) = li51*li54 + li41*src[9];
520 dst(3,1) = li52*li54 + li42*src[9];
521 dst(3,2) = li53*li54 + li43*src[9];
522 dst(3,3) = li54*li54 + src[9]*src[9];
523 dst(4,0) = li51*src[14];
524 dst(4,1) = li52*src[14];
525 dst(4,2) = li53*src[14];
526 dst(4,3) = li54*src[14];
527 dst(4,4) = src[14]*src[14];
536 const F li21 = -src[1] * src[0] * src[2];
537 const F li32 = -src[4] * src[2] * src[5];
538 const F li31 = (src[1] * src[4] * src[2] - src[3]) * src[0] * src[5];
539 const F li43 = -src[8] * src[9] * src[5];
540 const F li42 = (src[4] * src[8] * src[5] - src[7]) * src[2] * src[9];
541 const F li41 = (-src[1] * src[4] * src[8] * src[2] * src[5] +
542 src[1] * src[7] * src[2] + src[3] * src[8] * src[5] - src[6]) * src[0] * src[9];
544 dst(0,0) = li41*li41 + li31*li31 + li21*li21 + src[0]*src[0];
545 dst(1,0) = li41*li42 + li31*li32 + li21*src[2];
546 dst(1,1) = li42*li42 + li32*li32 + src[2]*src[2];
547 dst(2,0) = li41*li43 + li31*src[5];
548 dst(2,1) = li42*li43 + li32*src[5];
549 dst(2,2) = li43*li43 + src[5]*src[5];
550 dst(3,0) = li41*src[9];
551 dst(3,1) = li42*src[9];
552 dst(3,2) = li43*src[9];
553 dst(3,3) = src[9]*src[9];
562 const F li21 = -src[1] * src[0] * src[2];
563 const F li32 = -src[4] * src[2] * src[5];
564 const F li31 = (src[1] * src[4] * src[2] - src[3]) * src[0] * src[5];
566 dst(0,0) = li31*li31 + li21*li21 + src[0]*src[0];
567 dst(1,0) = li31*li32 + li21*src[2];
568 dst(1,1) = li32*li32 + src[2]*src[2];
569 dst(2,0) = li31*src[5];
570 dst(2,1) = li32*src[5];
571 dst(2,2) = src[5]*src[5];
580 const F li21 = -src[1] * src[0] * src[2];
582 dst(0,0) = li21*li21 + src[0]*src[0];
583 dst(1,0) = li21*src[2];
584 dst(1,1) = src[2]*src[2];
593 dst(0,0) = src[0]*src[0];
611 const F y0 = rhs[0] * l[0];
612 const F y1 = (rhs[1]-l[1]*y0)*l[2];
613 const F y2 = (rhs[2]-(l[3]*y0+l[4]*y1))*l[5];
614 const F y3 = (rhs[3]-(l[6]*y0+l[7]*y1+l[8]*y2))*l[9];
615 const F y4 = (rhs[4]-(l[10]*y0+l[11]*y1+l[12]*y2+l[13]*y3))*l[14];
616 const F y5 = (rhs[5]-(l[15]*y0+l[16]*y1+l[17]*y2+l[18]*y3+l[19]*y4))*l[20];
619 rhs[4] = (y4-l[19]*rhs[5])*l[14];
620 rhs[3] = (y3-(l[18]*rhs[5]+l[13]*rhs[4]))*l[9];
621 rhs[2] = (y2-(l[17]*rhs[5]+l[12]*rhs[4]+l[8]*rhs[3]))*l[5];
622 rhs[1] = (y1-(l[16]*rhs[5]+l[11]*rhs[4]+l[7]*rhs[3]+l[4]*rhs[2]))*l[2];
623 rhs[0] = (y0-(l[15]*rhs[5]+l[10]*rhs[4]+l[6]*rhs[3]+l[3]*rhs[2]+l[1]*rhs[1]))*l[0];
633 const F y0 = rhs[0] * l[0];
634 const F y1 = (rhs[1]-l[1]*y0)*l[2];
635 const F y2 = (rhs[2]-(l[3]*y0+l[4]*y1))*l[5];
636 const F y3 = (rhs[3]-(l[6]*y0+l[7]*y1+l[8]*y2))*l[9];
637 const F y4 = (rhs[4]-(l[10]*y0+l[11]*y1+l[12]*y2+l[13]*y3))*l[14];
640 rhs[3] = (y3-(l[13]*rhs[4]))*l[9];
641 rhs[2] = (y2-(l[12]*rhs[4]+l[8]*rhs[3]))*l[5];
642 rhs[1] = (y1-(l[11]*rhs[4]+l[7]*rhs[3]+l[4]*rhs[2]))*l[2];
643 rhs[0] = (y0-(l[10]*rhs[4]+l[6]*rhs[3]+l[3]*rhs[2]+l[1]*rhs[1]))*l[0];
653 const F y0 = rhs[0] * l[0];
654 const F y1 = (rhs[1]-l[1]*y0)*l[2];
655 const F y2 = (rhs[2]-(l[3]*y0+l[4]*y1))*l[5];
656 const F y3 = (rhs[3]-(l[6]*y0+l[7]*y1+l[8]*y2))*l[9];
659 rhs[2] = (y2-(l[8]*rhs[3]))*l[5];
660 rhs[1] = (y1-(l[7]*rhs[3]+l[4]*rhs[2]))*l[2];
661 rhs[0] = (y0-(l[6]*rhs[3]+l[3]*rhs[2]+l[1]*rhs[1]))*l[0];
671 const F y0 = rhs[0] * l[0];
672 const F y1 = (rhs[1]-l[1]*y0)*l[2];
673 const F y2 = (rhs[2]-(l[3]*y0+l[4]*y1))*l[5];
676 rhs[1] = (y1-(l[4]*rhs[2]))*l[2];
677 rhs[0] = (y0-(l[3]*rhs[2]+l[1]*rhs[1]))*l[0];
687 const F y0 = rhs[0] * l[0];
688 const F y1 = (rhs[1]-l[1]*y0)*l[2];
691 rhs[0] = (y0-(l[1]*rhs[1]))*l[0];
701 rhs[0] *= l[0] * l[0];
718 #endif // ROOT_Math_CHOLESKYDECOMP
F fL[N *(N+1)/2]
lower triangular matrix L
bool Invert(M &m) const
place the inverse into m
PackedArrayAdapter(G *arr)
constructor
bool operator()(F *dst, const M &src) const
method to do the decomposition
const G operator()(unsigned i, unsigned j) const
read access to elements (make sure that j <= i)
CholeskyDecomp(G *m)
perform a Cholesky decomposition
void operator()(M &dst, const F *src) const
method to do the inversion
bool fOk
flag indicating a successful decomposition
class to compute the Cholesky decomposition of a matrix
bool operator()(F *dst, const M &src) const
method to do the decomposition
G * fArr
pointer to first array element
G & operator()(unsigned i, unsigned j)
write access to elements (make sure that j <= i)
void operator()(M &dst, const F *src) const
method to do the inversion
void operator()(M &dst, const F *src) const
method to do the inversion
adapter for packed arrays (to SMatrix indexing conventions)
void operator()(V &rhs, const F *l) const
method to solve the linear system
void operator()(M &dst, const F *src) const
method to do the inversion
bool operator()(F *dst, const M &src) const
method to do the decomposition
struct to obtain the inverse from a Cholesky decomposition
struct to do a Cholesky decomposition
bool operator()(F *dst, const M &src) const
method to do the decomposition
void operator()(V &rhs, const F *l) const
method to solve the linear system
bool Solve(V &rhs) const
solves a linear system for the given right hand side
bool Invert(G *m) const
place the inverse into m
void operator()(M &dst, const F *src) const
method to do the inversion
bool operator()(F *dst, const M &src) const
method to do the decomposition
void operator()(V &rhs, const F *l) const
method to solve the linear system
CholeskyDecomp(const M &m)
perform a Cholesky decomposition
bool operator()(F *dst, const M &src) const
method to do the decomposition
void operator()(V &rhs, const F *l) const
method to solve the linear system
void operator()(V &rhs, const F *l) const
method to solve the linear system
std::vector< std::vector< double > > tmp
void operator()(V &rhs, const F *l) const
method to solve the linear system
void operator()(V &rhs, const F *l) const
method to solve the linear system
void operator()(M &dst, const F *src) const
method to do the inversion
static uInt32 F(BLOWFISH_CTX *ctx, uInt32 x)
bool operator()(F *dst, const M &src) const
method to do the decomposition
bool ok() const
returns true if decomposition was successful
void operator()(M &dst, const F *src) const
method to do the inversion
struct to solve a linear system using its Cholesky decomposition