1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
|
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
* optimized and unoptimized vector and matrix functions
* (inlined private versions)
*/
#ifndef _ODE__PRIVATE_MATRIX_H_
#define _ODE__PRIVATE_MATRIX_H_
#include <ode/matrix.h>
#ifdef __cplusplus
template <unsigned a_stride, typename element_type>
ODE_INLINE
void dxtSetZero (element_type *a, sizeint n)
{
element_type *const aend = a + n * a_stride;
for (element_type *acurr = a; acurr != aend; acurr += a_stride) {
*acurr = (element_type)0;
}
}
template <typename element_type>
ODE_INLINE
void dxSetZero (element_type *a, sizeint n)
{
dxtSetZero<1>(a, n);
}
template <typename element_type>
ODE_INLINE
void dxSetValue (element_type *a, sizeint n, element_type value)
{
element_type *const aend = a + n;
for (element_type *acurr = a; acurr != aend; ++acurr) {
*acurr = value;
}
}
#else // #ifndef __cplusplus
ODE_PURE_INLINE
void dxSetZero (dReal *a, sizeint n)
{
dReal *const aend = a + n;
dReal *acurr;
for (acurr = a; acurr != aend; ++acurr) {
*acurr = 0;
}
}
ODE_PURE_INLINE
void dxSetValue (dReal *a, sizeint n, dReal value)
{
dReal *const aend = a + n;
dReal *acurr;
for (acurr = a; acurr != aend; ++acurr) {
*acurr = value;
}
}
#endif // #ifdef __cplusplus
dReal dxDot (const dReal *a, const dReal *b, unsigned n);
void dxMultiply0 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
void dxMultiply1 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
void dxMultiply2 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
int dxFactorCholesky (dReal *A, unsigned n, void *tmpbuf);
void dxSolveCholesky (const dReal *L, dReal *b, unsigned n, void *tmpbuf);
int dxInvertPDMatrix (const dReal *A, dReal *Ainv, unsigned n, void *tmpbuf);
int dxIsPositiveDefinite (const dReal *A, unsigned n, void *tmpbuf);
void dxLDLTAddTL (dReal *L, dReal *d, const dReal *a, unsigned n, unsigned nskip, void *tmpbuf);
void dxLDLTRemove (dReal **A, const unsigned *p, dReal *L, dReal *d, unsigned n1, unsigned n2, unsigned r, unsigned nskip, void *tmpbuf);
void dxRemoveRowCol (dReal *A, unsigned n, unsigned nskip, unsigned r);
ODE_PURE_INLINE sizeint dxEstimateFactorCholeskyTmpbufSize(unsigned n)
{
return dPAD(n) * sizeof(dReal);
}
ODE_PURE_INLINE sizeint dxEstimateSolveCholeskyTmpbufSize(unsigned n)
{
return dPAD(n) * sizeof(dReal);
}
ODE_PURE_INLINE sizeint dxEstimateInvertPDMatrixTmpbufSize(unsigned n)
{
sizeint FactorCholesky_size = dxEstimateFactorCholeskyTmpbufSize(n);
sizeint SolveCholesky_size = dxEstimateSolveCholeskyTmpbufSize(n);
sizeint MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
return (sizeint)dPAD(n) * (n + 1) * sizeof(dReal) + MaxCholesky_size;
}
ODE_PURE_INLINE sizeint dxEstimateIsPositiveDefiniteTmpbufSize(unsigned n)
{
return (sizeint)dPAD(n) * n * sizeof(dReal) + dxEstimateFactorCholeskyTmpbufSize(n);
}
ODE_PURE_INLINE sizeint dxEstimateLDLTAddTLTmpbufSize(unsigned nskip)
{
return nskip * (2 * sizeof(dReal));
}
ODE_PURE_INLINE sizeint dxEstimateLDLTRemoveTmpbufSize(unsigned n2, unsigned nskip)
{
return n2 * sizeof(dReal) + dxEstimateLDLTAddTLTmpbufSize(nskip);
}
/* For internal use */
#define dSetZero(a, n) dxSetZero(a, n)
#define dSetValue(a, n, value) dxSetValue(a, n, value)
#define dDot(a, b, n) dxDot(a, b, n)
#define dMultiply0(A, B, C, p, q, r) dxMultiply0(A, B, C, p, q, r)
#define dMultiply1(A, B, C, p, q, r) dxMultiply1(A, B, C, p, q, r)
#define dMultiply2(A, B, C, p, q, r) dxMultiply2(A, B, C, p, q, r)
#define dFactorCholesky(A, n, tmpbuf) dxFactorCholesky(A, n, tmpbuf)
#define dSolveCholesky(L, b, n, tmpbuf) dxSolveCholesky(L, b, n, tmpbuf)
#define dInvertPDMatrix(A, Ainv, n, tmpbuf) dxInvertPDMatrix(A, Ainv, n, tmpbuf)
#define dIsPositiveDefinite(A, n, tmpbuf) dxIsPositiveDefinite(A, n, tmpbuf)
#define dLDLTAddTL(L, d, a, n, nskip, tmpbuf) dxLDLTAddTL(L, d, a, n, nskip, tmpbuf)
#define dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf) dxLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf)
#define dRemoveRowCol(A, n, nskip, r) dxRemoveRowCol(A, n, nskip, r)
#define dEstimateFactorCholeskyTmpbufSize(n) dxEstimateFactorCholeskyTmpbufSize(n)
#define dEstimateSolveCholeskyTmpbufSize(n) dxEstimateSolveCholeskyTmpbufSize(n)
#define dEstimateInvertPDMatrixTmpbufSize(n) dxEstimateInvertPDMatrixTmpbufSize(n)
#define dEstimateIsPositiveDefiniteTmpbufSize(n) dxEstimateIsPositiveDefiniteTmpbufSize(n)
#define dEstimateLDLTAddTLTmpbufSize(nskip) dxEstimateLDLTAddTLTmpbufSize(nskip)
#define dEstimateLDLTRemoveTmpbufSize(n2, nskip) dxEstimateLDLTRemoveTmpbufSize(n2, nskip)
#endif // #ifndef _ODE__PRIVATE_MATRIX_H_
|
