28 #ifndef _ODE__PRIVATE_MATRIX_H_
29 #define _ODE__PRIVATE_MATRIX_H_
32 #include <ode/matrix.h>
37 template <
typename element_type>
39 void _dSetZero (element_type *a,
size_t n)
41 element_type *acurr = a;
42 element_type *
const aend = a + n;
43 while (acurr != aend) {
48 template <
typename element_type>
50 void _dSetValue (element_type *a,
size_t n, element_type value)
52 element_type *acurr = a;
53 element_type *
const aend = a + n;
54 while (acurr != aend) {
60 #else // #ifndef __cplusplus
63 void _dSetZero (dReal *a,
size_t n)
66 dReal *
const aend = a + n;
67 while (acurr != aend) {
73 void _dSetValue (dReal *a,
size_t n, dReal value)
76 dReal *
const aend = a + n;
77 while (acurr != aend) {
83 #endif // #ifdef __cplusplus
90 dReal _dDot (
const dReal *a,
const dReal *b,
int n);
91 void _dMultiply0 (dReal *A,
const dReal *B,
const dReal *C,
int p,
int q,
int r);
92 void _dMultiply1 (dReal *A,
const dReal *B,
const dReal *C,
int p,
int q,
int r);
93 void _dMultiply2 (dReal *A,
const dReal *B,
const dReal *C,
int p,
int q,
int r);
94 int _dFactorCholesky (dReal *A,
int n,
void *tmpbuf);
95 void _dSolveCholesky (
const dReal *L, dReal *b,
int n,
void *tmpbuf);
96 int _dInvertPDMatrix (
const dReal *A, dReal *Ainv,
int n,
void *tmpbuf);
97 int _dIsPositiveDefinite (
const dReal *A,
int n,
void *tmpbuf);
98 void _dFactorLDLT (dReal *A, dReal *d,
int n,
int nskip);
99 void _dSolveL1 (
const dReal *L, dReal *b,
int n,
int nskip);
100 void _dSolveL1T (
const dReal *L, dReal *b,
int n,
int nskip);
101 void _dVectorScale (dReal *a,
const dReal *d,
int n);
102 void _dSolveLDLT (
const dReal *L,
const dReal *d, dReal *b,
int n,
int nskip);
103 void _dLDLTAddTL (dReal *L, dReal *d,
const dReal *a,
int n,
int nskip,
void *tmpbuf);
104 void _dLDLTRemove (dReal **A,
const int *p, dReal *L, dReal *d,
int n1,
int n2,
int r,
int nskip,
void *tmpbuf);
105 void _dRemoveRowCol (dReal *A,
int n,
int nskip,
int r);
107 ODE_PURE_INLINE
size_t _dEstimateFactorCholeskyTmpbufSize(
int n)
109 return dPAD(n) *
sizeof(dReal);
112 ODE_PURE_INLINE
size_t _dEstimateSolveCholeskyTmpbufSize(
int n)
114 return dPAD(n) *
sizeof(dReal);
117 ODE_PURE_INLINE
size_t _dEstimateInvertPDMatrixTmpbufSize(
int n)
119 size_t FactorCholesky_size = _dEstimateFactorCholeskyTmpbufSize(n);
120 size_t SolveCholesky_size = _dEstimateSolveCholeskyTmpbufSize(n);
121 size_t MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
122 return dPAD(n) * (n + 1) *
sizeof(dReal) + MaxCholesky_size;
125 ODE_PURE_INLINE
size_t _dEstimateIsPositiveDefiniteTmpbufSize(
int n)
127 return dPAD(n) * n *
sizeof(dReal) + _dEstimateFactorCholeskyTmpbufSize(n);
130 ODE_PURE_INLINE
size_t _dEstimateLDLTAddTLTmpbufSize(
int nskip)
132 return nskip * 2 *
sizeof(dReal);
135 ODE_PURE_INLINE
size_t _dEstimateLDLTRemoveTmpbufSize(
int n2,
int nskip)
137 return n2 *
sizeof(dReal) + _dEstimateLDLTAddTLTmpbufSize(nskip);
141 #define dSetZero(a, n) _dSetZero(a, n)
142 #define dSetValue(a, n, value) _dSetValue(a, n, value)
143 #define dDot(a, b, n) _dDot(a, b, n)
144 #define dMultiply0(A, B, C, p, q, r) _dMultiply0(A, B, C, p, q, r)
145 #define dMultiply1(A, B, C, p, q, r) _dMultiply1(A, B, C, p, q, r)
146 #define dMultiply2(A, B, C, p, q, r) _dMultiply2(A, B, C, p, q, r)
147 #define dFactorCholesky(A, n, tmpbuf) _dFactorCholesky(A, n, tmpbuf)
148 #define dSolveCholesky(L, b, n, tmpbuf) _dSolveCholesky(L, b, n, tmpbuf)
149 #define dInvertPDMatrix(A, Ainv, n, tmpbuf) _dInvertPDMatrix(A, Ainv, n, tmpbuf)
150 #define dIsPositiveDefinite(A, n, tmpbuf) _dIsPositiveDefinite(A, n, tmpbuf)
151 #define dFactorLDLT(A, d, n, nskip) _dFactorLDLT(A, d, n, nskip)
152 #define dSolveL1(L, b, n, nskip) _dSolveL1(L, b, n, nskip)
153 #define dSolveL1T(L, b, n, nskip) _dSolveL1T(L, b, n, nskip)
154 #define dVectorScale(a, d, n) _dVectorScale(a, d, n)
155 #define dSolveLDLT(L, d, b, n, nskip) _dSolveLDLT(L, d, b, n, nskip)
156 #define dLDLTAddTL(L, d, a, n, nskip, tmpbuf) _dLDLTAddTL(L, d, a, n, nskip, tmpbuf)
157 #define dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf) _dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf)
158 #define dRemoveRowCol(A, n, nskip, r) _dRemoveRowCol(A, n, nskip, r)
161 #define dEstimateFactorCholeskyTmpbufSize(n) _dEstimateFactorCholeskyTmpbufSize(n)
162 #define dEstimateSolveCholeskyTmpbufSize(n) _dEstimateSolveCholeskyTmpbufSize(n)
163 #define dEstimateInvertPDMatrixTmpbufSize(n) _dEstimateInvertPDMatrixTmpbufSize(n)
164 #define dEstimateIsPositiveDefiniteTmpbufSize(n) _dEstimateIsPositiveDefiniteTmpbufSize(n)
165 #define dEstimateLDLTAddTLTmpbufSize(nskip) _dEstimateLDLTAddTLTmpbufSize(nskip)
166 #define dEstimateLDLTRemoveTmpbufSize(n2, nskip) _dEstimateLDLTRemoveTmpbufSize(n2, nskip)
174 #endif // #ifndef _ODE__PRIVATE_MATRIX_H_