Open Dynamics Engine
ode/src/matrix.h
1 /*************************************************************************
2  * *
3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4  * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5  * *
6  * This library is free software; you can redistribute it and/or *
7  * modify it under the terms of EITHER: *
8  * (1) The GNU Lesser General Public License as published by the Free *
9  * Software Foundation; either version 2.1 of the License, or (at *
10  * your option) any later version. The text of the GNU Lesser *
11  * General Public License is included with this library in the *
12  * file LICENSE.TXT. *
13  * (2) The BSD-style license that is included with this library in *
14  * the file LICENSE-BSD.TXT. *
15  * *
16  * This library is distributed in the hope that it will be useful, *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19  * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20  * *
21  *************************************************************************/
22 
23 /*
24  * optimized and unoptimized vector and matrix functions
25  * (inlined private versions)
26  */
27 
28 #ifndef _ODE__PRIVATE_MATRIX_H_
29 #define _ODE__PRIVATE_MATRIX_H_
30 
31 
32 #include <ode/matrix.h>
33 
34 
35 #ifdef __cplusplus
36 
37 template <typename element_type>
38 ODE_INLINE
39 void _dSetZero (element_type *a, size_t n)
40 {
41  element_type *acurr = a;
42  element_type *const aend = a + n;
43  while (acurr != aend) {
44  *(acurr++) = 0;
45  }
46 }
47 
48 template <typename element_type>
49 ODE_INLINE
50 void _dSetValue (element_type *a, size_t n, element_type value)
51 {
52  element_type *acurr = a;
53  element_type *const aend = a + n;
54  while (acurr != aend) {
55  *(acurr++) = value;
56  }
57 }
58 
59 
60 #else // #ifndef __cplusplus
61 
62 ODE_PURE_INLINE
63 void _dSetZero (dReal *a, size_t n)
64 {
65  dReal *acurr = a;
66  dReal *const aend = a + n;
67  while (acurr != aend) {
68  *(acurr++) = 0;
69  }
70 }
71 
72 ODE_PURE_INLINE
73 void _dSetValue (dReal *a, size_t n, dReal value)
74 {
75  dReal *acurr = a;
76  dReal *const aend = a + n;
77  while (acurr != aend) {
78  *(acurr++) = value;
79  }
80 }
81 
82 
83 #endif // #ifdef __cplusplus
84 
85 
86 #ifdef __cplusplus
87 extern "C" {
88 #endif
89 
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);
106 
107 ODE_PURE_INLINE size_t _dEstimateFactorCholeskyTmpbufSize(int n)
108 {
109  return dPAD(n) * sizeof(dReal);
110 }
111 
112 ODE_PURE_INLINE size_t _dEstimateSolveCholeskyTmpbufSize(int n)
113 {
114  return dPAD(n) * sizeof(dReal);
115 }
116 
117 ODE_PURE_INLINE size_t _dEstimateInvertPDMatrixTmpbufSize(int n)
118 {
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;
123 }
124 
125 ODE_PURE_INLINE size_t _dEstimateIsPositiveDefiniteTmpbufSize(int n)
126 {
127  return dPAD(n) * n * sizeof(dReal) + _dEstimateFactorCholeskyTmpbufSize(n);
128 }
129 
130 ODE_PURE_INLINE size_t _dEstimateLDLTAddTLTmpbufSize(int nskip)
131 {
132  return nskip * 2 * sizeof(dReal);
133 }
134 
135 ODE_PURE_INLINE size_t _dEstimateLDLTRemoveTmpbufSize(int n2, int nskip)
136 {
137  return n2 * sizeof(dReal) + _dEstimateLDLTAddTLTmpbufSize(nskip);
138 }
139 
140 /* For internal use */
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)
159 
160 
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)
167 
168 
169 #ifdef __cplusplus
170 }
171 #endif
172 
173 
174 #endif // #ifndef _ODE__PRIVATE_MATRIX_H_