ODE  0.13.1
 All Data Structures Functions Variables Typedefs Enumerations Groups
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 /* optimized and unoptimized vector and matrix functions */
24 
25 #ifndef _ODE_MATRIX_H_
26 #define _ODE_MATRIX_H_
27 
28 #include <ode/common.h>
29 
30 
31 #ifdef __cplusplus
32 extern "C" {
33 #endif
34 
35 
36 /* set a vector/matrix of size n to all zeros, or to a specific value. */
37 
38 ODE_API void dSetZero (dReal *a, int n);
39 ODE_API void dSetValue (dReal *a, int n, dReal value);
40 
41 
42 /* get the dot product of two n*1 vectors. if n <= 0 then
43  * zero will be returned (in which case a and b need not be valid).
44  */
45 
46 ODE_API dReal dDot (const dReal *a, const dReal *b, int n);
47 
48 
49 /* get the dot products of (a0,b), (a1,b), etc and return them in outsum.
50  * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case
51  * the input vectors need not be valid). this function is somewhat faster
52  * than calling dDot() for all of the combinations separately.
53  */
54 
55 /* NOT INCLUDED in the library for now.
56 void dMultidot2 (const dReal *a0, const dReal *a1,
57  const dReal *b, dReal *outsum, int n);
58 */
59 
60 
61 /* matrix multiplication. all matrices are stored in standard row format.
62  * the digit refers to the argument that is transposed:
63  * 0: A = B * C (sizes: A:p*r B:p*q C:q*r)
64  * 1: A = B' * C (sizes: A:p*r B:q*p C:q*r)
65  * 2: A = B * C' (sizes: A:p*r B:p*q C:r*q)
66  * case 1,2 are equivalent to saying that the operation is A=B*C but
67  * B or C are stored in standard column format.
68  */
69 
70 ODE_API void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
71 ODE_API void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
72 ODE_API void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
73 
74 
75 /* do an in-place cholesky decomposition on the lower triangle of the n*n
76  * symmetric matrix A (which is stored by rows). the resulting lower triangle
77  * will be such that L*L'=A. return 1 on success and 0 on failure (on failure
78  * the matrix is not positive definite).
79  */
80 
81 ODE_API int dFactorCholesky (dReal *A, int n);
82 
83 
84 /* solve for x: L*L'*x = b, and put the result back into x.
85  * L is size n*n, b is size n*1. only the lower triangle of L is considered.
86  */
87 
88 ODE_API void dSolveCholesky (const dReal *L, dReal *b, int n);
89 
90 
91 /* compute the inverse of the n*n positive definite matrix A and put it in
92  * Ainv. this is not especially fast. this returns 1 on success (A was
93  * positive definite) or 0 on failure (not PD).
94  */
95 
96 ODE_API int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n);
97 
98 
99 /* check whether an n*n matrix A is positive definite, return 1/0 (yes/no).
100  * positive definite means that x'*A*x > 0 for any x. this performs a
101  * cholesky decomposition of A. if the decomposition fails then the matrix
102  * is not positive definite. A is stored by rows. A is not altered.
103  */
104 
105 ODE_API int dIsPositiveDefinite (const dReal *A, int n);
106 
107 
108 /* factorize a matrix A into L*D*L', where L is lower triangular with ones on
109  * the diagonal, and D is diagonal.
110  * A is an n*n matrix stored by rows, with a leading dimension of n rounded
111  * up to 4. L is written into the strict lower triangle of A (the ones are not
112  * written) and the reciprocal of the diagonal elements of D are written into
113  * d.
114  */
115 ODE_API void dFactorLDLT (dReal *A, dReal *d, int n, int nskip);
116 
117 
118 /* solve L*x=b, where L is n*n lower triangular with ones on the diagonal,
119  * and x,b are n*1. b is overwritten with x.
120  * the leading dimension of L is `nskip'.
121  */
122 ODE_API void dSolveL1 (const dReal *L, dReal *b, int n, int nskip);
123 
124 
125 /* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal,
126  * and x,b are n*1. b is overwritten with x.
127  * the leading dimension of L is `nskip'.
128  */
129 ODE_API void dSolveL1T (const dReal *L, dReal *b, int n, int nskip);
130 
131 
132 /* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */
133 
134 ODE_API void dVectorScale (dReal *a, const dReal *d, int n);
135 
136 
137 /* given `L', a n*n lower triangular matrix with ones on the diagonal,
138  * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix
139  * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b.
140  * the leading dimension of L is `nskip'.
141  */
142 
143 ODE_API void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip);
144 
145 
146 /* given an L*D*L' factorization of an n*n matrix A, return the updated
147  * factorization L2*D2*L2' of A plus the following "top left" matrix:
148  *
149  * [ b a' ] <-- b is a[0]
150  * [ a 0 ] <-- a is a[1..n-1]
151  *
152  * - L has size n*n, its leading dimension is nskip. L is lower triangular
153  * with ones on the diagonal. only the lower triangle of L is referenced.
154  * - d has size n. d contains the reciprocal diagonal elements of D.
155  * - a has size n.
156  * the result is written into L, except that the left column of L and d[0]
157  * are not actually modified. see ldltaddTL.m for further comments.
158  */
159 ODE_API void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip);
160 
161 
162 /* given an L*D*L' factorization of a permuted matrix A, produce a new
163  * factorization for row and column `r' removed.
164  * - A has size n1*n1, its leading dimension in nskip. A is symmetric and
165  * positive definite. only the lower triangle of A is referenced.
166  * A itself may actually be an array of row pointers.
167  * - L has size n2*n2, its leading dimension in nskip. L is lower triangular
168  * with ones on the diagonal. only the lower triangle of L is referenced.
169  * - d has size n2. d contains the reciprocal diagonal elements of D.
170  * - p is a permutation vector. it contains n2 indexes into A. each index
171  * must be in the range 0..n1-1.
172  * - r is the row/column of L to remove.
173  * the new L will be written within the old L, i.e. will have the same leading
174  * dimension. the last row and column of L, and the last element of d, are
175  * undefined on exit.
176  *
177  * a fast O(n^2) algorithm is used. see ldltremove.m for further comments.
178  */
179 ODE_API void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
180  int n1, int n2, int r, int nskip);
181 
182 
183 /* given an n*n matrix A (with leading dimension nskip), remove the r'th row
184  * and column by moving elements. the new matrix will have the same leading
185  * dimension. the last row and column of A are untouched on exit.
186  */
187 ODE_API void dRemoveRowCol (dReal *A, int n, int nskip, int r);
188 
189 #ifdef __cplusplus
190 }
191 #endif
192 
193 
194 #endif