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
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
|
<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=UTF-8">
<title>Open Dynamics Engine: matrix.h Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>
<!-- Generated by Doxygen 1.5.3 -->
<div class="tabs">
<ul>
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="annotated.html"><span>Data Structures</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
</ul>
</div>
<h1>matrix.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*************************************************************************</span>
<a name="l00002"></a>00002 <span class="comment"> * *</span>
<a name="l00003"></a>00003 <span class="comment"> * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *</span>
<a name="l00004"></a>00004 <span class="comment"> * All rights reserved. Email: russ@q12.org Web: www.q12.org *</span>
<a name="l00005"></a>00005 <span class="comment"> * *</span>
<a name="l00006"></a>00006 <span class="comment"> * This library is free software; you can redistribute it and/or *</span>
<a name="l00007"></a>00007 <span class="comment"> * modify it under the terms of EITHER: *</span>
<a name="l00008"></a>00008 <span class="comment"> * (1) The GNU Lesser General Public License as published by the Free *</span>
<a name="l00009"></a>00009 <span class="comment"> * Software Foundation; either version 2.1 of the License, or (at *</span>
<a name="l00010"></a>00010 <span class="comment"> * your option) any later version. The text of the GNU Lesser *</span>
<a name="l00011"></a>00011 <span class="comment"> * General Public License is included with this library in the *</span>
<a name="l00012"></a>00012 <span class="comment"> * file LICENSE.TXT. *</span>
<a name="l00013"></a>00013 <span class="comment"> * (2) The BSD-style license that is included with this library in *</span>
<a name="l00014"></a>00014 <span class="comment"> * the file LICENSE-BSD.TXT. *</span>
<a name="l00015"></a>00015 <span class="comment"> * *</span>
<a name="l00016"></a>00016 <span class="comment"> * This library is distributed in the hope that it will be useful, *</span>
<a name="l00017"></a>00017 <span class="comment"> * but WITHOUT ANY WARRANTY; without even the implied warranty of *</span>
<a name="l00018"></a>00018 <span class="comment"> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *</span>
<a name="l00019"></a>00019 <span class="comment"> * LICENSE.TXT and LICENSE-BSD.TXT for more details. *</span>
<a name="l00020"></a>00020 <span class="comment"> * *</span>
<a name="l00021"></a>00021 <span class="comment"> *************************************************************************/</span>
<a name="l00022"></a>00022
<a name="l00023"></a>00023 <span class="comment">/* optimized and unoptimized vector and matrix functions */</span>
<a name="l00024"></a>00024
<a name="l00025"></a>00025 <span class="preprocessor">#ifndef _ODE_MATRIX_H_</span>
<a name="l00026"></a>00026 <span class="preprocessor"></span><span class="preprocessor">#define _ODE_MATRIX_H_</span>
<a name="l00027"></a>00027 <span class="preprocessor"></span>
<a name="l00028"></a>00028 <span class="preprocessor">#include <ode/common.h></span>
<a name="l00029"></a>00029
<a name="l00030"></a>00030
<a name="l00031"></a>00031 <span class="preprocessor">#ifdef __cplusplus</span>
<a name="l00032"></a>00032 <span class="preprocessor"></span><span class="keyword">extern</span> <span class="stringliteral">"C"</span> {
<a name="l00033"></a>00033 <span class="preprocessor">#endif</span>
<a name="l00034"></a>00034 <span class="preprocessor"></span>
<a name="l00035"></a>00035
<a name="l00036"></a>00036 <span class="comment">/* set a vector/matrix of size n to all zeros, or to a specific value. */</span>
<a name="l00037"></a>00037
<a name="l00038"></a>00038 ODE_API <span class="keywordtype">void</span> dSetZero (dReal *a, <span class="keywordtype">int</span> n);
<a name="l00039"></a>00039 ODE_API <span class="keywordtype">void</span> dSetValue (dReal *a, <span class="keywordtype">int</span> n, dReal value);
<a name="l00040"></a>00040
<a name="l00041"></a>00041
<a name="l00042"></a>00042 <span class="comment">/* get the dot product of two n*1 vectors. if n <= 0 then</span>
<a name="l00043"></a>00043 <span class="comment"> * zero will be returned (in which case a and b need not be valid).</span>
<a name="l00044"></a>00044 <span class="comment"> */</span>
<a name="l00045"></a>00045
<a name="l00046"></a>00046 ODE_API dReal dDot (<span class="keyword">const</span> dReal *a, <span class="keyword">const</span> dReal *b, <span class="keywordtype">int</span> n);
<a name="l00047"></a>00047
<a name="l00048"></a>00048
<a name="l00049"></a>00049 <span class="comment">/* get the dot products of (a0,b), (a1,b), etc and return them in outsum.</span>
<a name="l00050"></a>00050 <span class="comment"> * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case</span>
<a name="l00051"></a>00051 <span class="comment"> * the input vectors need not be valid). this function is somewhat faster</span>
<a name="l00052"></a>00052 <span class="comment"> * than calling dDot() for all of the combinations separately.</span>
<a name="l00053"></a>00053 <span class="comment"> */</span>
<a name="l00054"></a>00054
<a name="l00055"></a>00055 <span class="comment">/* NOT INCLUDED in the library for now.</span>
<a name="l00056"></a>00056 <span class="comment">void dMultidot2 (const dReal *a0, const dReal *a1,</span>
<a name="l00057"></a>00057 <span class="comment"> const dReal *b, dReal *outsum, int n);</span>
<a name="l00058"></a>00058 <span class="comment">*/</span>
<a name="l00059"></a>00059
<a name="l00060"></a>00060
<a name="l00061"></a>00061 <span class="comment">/* matrix multiplication. all matrices are stored in standard row format.</span>
<a name="l00062"></a>00062 <span class="comment"> * the digit refers to the argument that is transposed:</span>
<a name="l00063"></a>00063 <span class="comment"> * 0: A = B * C (sizes: A:p*r B:p*q C:q*r)</span>
<a name="l00064"></a>00064 <span class="comment"> * 1: A = B' * C (sizes: A:p*r B:q*p C:q*r)</span>
<a name="l00065"></a>00065 <span class="comment"> * 2: A = B * C' (sizes: A:p*r B:p*q C:r*q)</span>
<a name="l00066"></a>00066 <span class="comment"> * case 1,2 are equivalent to saying that the operation is A=B*C but</span>
<a name="l00067"></a>00067 <span class="comment"> * B or C are stored in standard column format.</span>
<a name="l00068"></a>00068 <span class="comment"> */</span>
<a name="l00069"></a>00069
<a name="l00070"></a>00070 ODE_API <span class="keywordtype">void</span> dMultiply0 (dReal *A, <span class="keyword">const</span> dReal *B, <span class="keyword">const</span> dReal *C, <span class="keywordtype">int</span> p,<span class="keywordtype">int</span> q,<span class="keywordtype">int</span> r);
<a name="l00071"></a>00071 ODE_API <span class="keywordtype">void</span> dMultiply1 (dReal *A, <span class="keyword">const</span> dReal *B, <span class="keyword">const</span> dReal *C, <span class="keywordtype">int</span> p,<span class="keywordtype">int</span> q,<span class="keywordtype">int</span> r);
<a name="l00072"></a>00072 ODE_API <span class="keywordtype">void</span> dMultiply2 (dReal *A, <span class="keyword">const</span> dReal *B, <span class="keyword">const</span> dReal *C, <span class="keywordtype">int</span> p,<span class="keywordtype">int</span> q,<span class="keywordtype">int</span> r);
<a name="l00073"></a>00073
<a name="l00074"></a>00074
<a name="l00075"></a>00075 <span class="comment">/* do an in-place cholesky decomposition on the lower triangle of the n*n</span>
<a name="l00076"></a>00076 <span class="comment"> * symmetric matrix A (which is stored by rows). the resulting lower triangle</span>
<a name="l00077"></a>00077 <span class="comment"> * will be such that L*L'=A. return 1 on success and 0 on failure (on failure</span>
<a name="l00078"></a>00078 <span class="comment"> * the matrix is not positive definite).</span>
<a name="l00079"></a>00079 <span class="comment"> */</span>
<a name="l00080"></a>00080
<a name="l00081"></a>00081 ODE_API <span class="keywordtype">int</span> dFactorCholesky (dReal *A, <span class="keywordtype">int</span> n);
<a name="l00082"></a>00082
<a name="l00083"></a>00083
<a name="l00084"></a>00084 <span class="comment">/* solve for x: L*L'*x = b, and put the result back into x.</span>
<a name="l00085"></a>00085 <span class="comment"> * L is size n*n, b is size n*1. only the lower triangle of L is considered.</span>
<a name="l00086"></a>00086 <span class="comment"> */</span>
<a name="l00087"></a>00087
<a name="l00088"></a>00088 ODE_API <span class="keywordtype">void</span> dSolveCholesky (<span class="keyword">const</span> dReal *L, dReal *b, <span class="keywordtype">int</span> n);
<a name="l00089"></a>00089
<a name="l00090"></a>00090
<a name="l00091"></a>00091 <span class="comment">/* compute the inverse of the n*n positive definite matrix A and put it in</span>
<a name="l00092"></a>00092 <span class="comment"> * Ainv. this is not especially fast. this returns 1 on success (A was</span>
<a name="l00093"></a>00093 <span class="comment"> * positive definite) or 0 on failure (not PD).</span>
<a name="l00094"></a>00094 <span class="comment"> */</span>
<a name="l00095"></a>00095
<a name="l00096"></a>00096 ODE_API <span class="keywordtype">int</span> dInvertPDMatrix (<span class="keyword">const</span> dReal *A, dReal *Ainv, <span class="keywordtype">int</span> n);
<a name="l00097"></a>00097
<a name="l00098"></a>00098
<a name="l00099"></a>00099 <span class="comment">/* check whether an n*n matrix A is positive definite, return 1/0 (yes/no).</span>
<a name="l00100"></a>00100 <span class="comment"> * positive definite means that x'*A*x > 0 for any x. this performs a</span>
<a name="l00101"></a>00101 <span class="comment"> * cholesky decomposition of A. if the decomposition fails then the matrix</span>
<a name="l00102"></a>00102 <span class="comment"> * is not positive definite. A is stored by rows. A is not altered.</span>
<a name="l00103"></a>00103 <span class="comment"> */</span>
<a name="l00104"></a>00104
<a name="l00105"></a>00105 ODE_API <span class="keywordtype">int</span> dIsPositiveDefinite (<span class="keyword">const</span> dReal *A, <span class="keywordtype">int</span> n);
<a name="l00106"></a>00106
<a name="l00107"></a>00107
<a name="l00108"></a>00108 <span class="comment">/* factorize a matrix A into L*D*L', where L is lower triangular with ones on</span>
<a name="l00109"></a>00109 <span class="comment"> * the diagonal, and D is diagonal.</span>
<a name="l00110"></a>00110 <span class="comment"> * A is an n*n matrix stored by rows, with a leading dimension of n rounded</span>
<a name="l00111"></a>00111 <span class="comment"> * up to 4. L is written into the strict lower triangle of A (the ones are not</span>
<a name="l00112"></a>00112 <span class="comment"> * written) and the reciprocal of the diagonal elements of D are written into</span>
<a name="l00113"></a>00113 <span class="comment"> * d.</span>
<a name="l00114"></a>00114 <span class="comment"> */</span>
<a name="l00115"></a>00115 ODE_API <span class="keywordtype">void</span> dFactorLDLT (dReal *A, dReal *d, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> nskip);
<a name="l00116"></a>00116
<a name="l00117"></a>00117
<a name="l00118"></a>00118 <span class="comment">/* solve L*x=b, where L is n*n lower triangular with ones on the diagonal,</span>
<a name="l00119"></a>00119 <span class="comment"> * and x,b are n*1. b is overwritten with x.</span>
<a name="l00120"></a>00120 <span class="comment"> * the leading dimension of L is `nskip'.</span>
<a name="l00121"></a>00121 <span class="comment"> */</span>
<a name="l00122"></a>00122 ODE_API <span class="keywordtype">void</span> dSolveL1 (<span class="keyword">const</span> dReal *L, dReal *b, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> nskip);
<a name="l00123"></a>00123
<a name="l00124"></a>00124
<a name="l00125"></a>00125 <span class="comment">/* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal,</span>
<a name="l00126"></a>00126 <span class="comment"> * and x,b are n*1. b is overwritten with x.</span>
<a name="l00127"></a>00127 <span class="comment"> * the leading dimension of L is `nskip'.</span>
<a name="l00128"></a>00128 <span class="comment"> */</span>
<a name="l00129"></a>00129 ODE_API <span class="keywordtype">void</span> dSolveL1T (<span class="keyword">const</span> dReal *L, dReal *b, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> nskip);
<a name="l00130"></a>00130
<a name="l00131"></a>00131
<a name="l00132"></a>00132 <span class="comment">/* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */</span>
<a name="l00133"></a>00133
<a name="l00134"></a>00134 ODE_API <span class="keywordtype">void</span> dVectorScale (dReal *a, <span class="keyword">const</span> dReal *d, <span class="keywordtype">int</span> n);
<a name="l00135"></a>00135
<a name="l00136"></a>00136
<a name="l00137"></a>00137 <span class="comment">/* given `L', a n*n lower triangular matrix with ones on the diagonal,</span>
<a name="l00138"></a>00138 <span class="comment"> * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix</span>
<a name="l00139"></a>00139 <span class="comment"> * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b.</span>
<a name="l00140"></a>00140 <span class="comment"> * the leading dimension of L is `nskip'.</span>
<a name="l00141"></a>00141 <span class="comment"> */</span>
<a name="l00142"></a>00142
<a name="l00143"></a>00143 ODE_API <span class="keywordtype">void</span> dSolveLDLT (<span class="keyword">const</span> dReal *L, <span class="keyword">const</span> dReal *d, dReal *b, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> nskip);
<a name="l00144"></a>00144
<a name="l00145"></a>00145
<a name="l00146"></a>00146 <span class="comment">/* given an L*D*L' factorization of an n*n matrix A, return the updated</span>
<a name="l00147"></a>00147 <span class="comment"> * factorization L2*D2*L2' of A plus the following "top left" matrix:</span>
<a name="l00148"></a>00148 <span class="comment"> *</span>
<a name="l00149"></a>00149 <span class="comment"> * [ b a' ] <-- b is a[0]</span>
<a name="l00150"></a>00150 <span class="comment"> * [ a 0 ] <-- a is a[1..n-1]</span>
<a name="l00151"></a>00151 <span class="comment"> *</span>
<a name="l00152"></a>00152 <span class="comment"> * - L has size n*n, its leading dimension is nskip. L is lower triangular</span>
<a name="l00153"></a>00153 <span class="comment"> * with ones on the diagonal. only the lower triangle of L is referenced.</span>
<a name="l00154"></a>00154 <span class="comment"> * - d has size n. d contains the reciprocal diagonal elements of D.</span>
<a name="l00155"></a>00155 <span class="comment"> * - a has size n.</span>
<a name="l00156"></a>00156 <span class="comment"> * the result is written into L, except that the left column of L and d[0]</span>
<a name="l00157"></a>00157 <span class="comment"> * are not actually modified. see ldltaddTL.m for further comments. </span>
<a name="l00158"></a>00158 <span class="comment"> */</span>
<a name="l00159"></a>00159 ODE_API <span class="keywordtype">void</span> dLDLTAddTL (dReal *L, dReal *d, <span class="keyword">const</span> dReal *a, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> nskip);
<a name="l00160"></a>00160
<a name="l00161"></a>00161
<a name="l00162"></a>00162 <span class="comment">/* given an L*D*L' factorization of a permuted matrix A, produce a new</span>
<a name="l00163"></a>00163 <span class="comment"> * factorization for row and column `r' removed.</span>
<a name="l00164"></a>00164 <span class="comment"> * - A has size n1*n1, its leading dimension in nskip. A is symmetric and</span>
<a name="l00165"></a>00165 <span class="comment"> * positive definite. only the lower triangle of A is referenced.</span>
<a name="l00166"></a>00166 <span class="comment"> * A itself may actually be an array of row pointers.</span>
<a name="l00167"></a>00167 <span class="comment"> * - L has size n2*n2, its leading dimension in nskip. L is lower triangular</span>
<a name="l00168"></a>00168 <span class="comment"> * with ones on the diagonal. only the lower triangle of L is referenced.</span>
<a name="l00169"></a>00169 <span class="comment"> * - d has size n2. d contains the reciprocal diagonal elements of D.</span>
<a name="l00170"></a>00170 <span class="comment"> * - p is a permutation vector. it contains n2 indexes into A. each index</span>
<a name="l00171"></a>00171 <span class="comment"> * must be in the range 0..n1-1.</span>
<a name="l00172"></a>00172 <span class="comment"> * - r is the row/column of L to remove.</span>
<a name="l00173"></a>00173 <span class="comment"> * the new L will be written within the old L, i.e. will have the same leading</span>
<a name="l00174"></a>00174 <span class="comment"> * dimension. the last row and column of L, and the last element of d, are</span>
<a name="l00175"></a>00175 <span class="comment"> * undefined on exit.</span>
<a name="l00176"></a>00176 <span class="comment"> *</span>
<a name="l00177"></a>00177 <span class="comment"> * a fast O(n^2) algorithm is used. see ldltremove.m for further comments.</span>
<a name="l00178"></a>00178 <span class="comment"> */</span>
<a name="l00179"></a>00179 ODE_API <span class="keywordtype">void</span> dLDLTRemove (dReal **A, <span class="keyword">const</span> <span class="keywordtype">int</span> *p, dReal *L, dReal *d,
<a name="l00180"></a>00180 <span class="keywordtype">int</span> n1, <span class="keywordtype">int</span> n2, <span class="keywordtype">int</span> r, <span class="keywordtype">int</span> nskip);
<a name="l00181"></a>00181
<a name="l00182"></a>00182
<a name="l00183"></a>00183 <span class="comment">/* given an n*n matrix A (with leading dimension nskip), remove the r'th row</span>
<a name="l00184"></a>00184 <span class="comment"> * and column by moving elements. the new matrix will have the same leading</span>
<a name="l00185"></a>00185 <span class="comment"> * dimension. the last row and column of A are untouched on exit.</span>
<a name="l00186"></a>00186 <span class="comment"> */</span>
<a name="l00187"></a>00187 ODE_API <span class="keywordtype">void</span> dRemoveRowCol (dReal *A, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> nskip, <span class="keywordtype">int</span> r);
<a name="l00188"></a>00188
<a name="l00189"></a>00189
<a name="l00190"></a>00190 <span class="preprocessor">#ifdef __cplusplus</span>
<a name="l00191"></a>00191 <span class="preprocessor"></span>}
<a name="l00192"></a>00192 <span class="preprocessor">#endif</span>
<a name="l00193"></a>00193 <span class="preprocessor"></span>
<a name="l00194"></a>00194 <span class="preprocessor">#endif</span>
</pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Fri Oct 12 08:36:51 2007 for Open Dynamics Engine by
<a href="http://www.doxygen.org/index.html">
<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.3 </small></address>
</body>
</html>
|
