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 195