1 /************************************************************************* 2 * D bindings for ODE * 3 * * 4 * C header port by Daniel "q66" Kolesa <quaker66@gmail.com> * 5 * * 6 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * 7 * All rights reserved. Email: russ@q12.org Web: www.q12.org * 8 * * 9 * This library is free software; you can redistribute it and/or * 10 * modify it under the terms of EITHER: * 11 * (1) The GNU Lesser General Public License as published by the Free * 12 * Software Foundation; either version 2.1 of the License, or (at * 13 * your option) any later version. The text of the GNU Lesser * 14 * General Public License is included with this library in the * 15 * file LICENSE.TXT. * 16 * (2) The BSD-style license that is included with this library in * 17 * the file LICENSE-BSD.TXT. * 18 * * 19 * This library is distributed in the hope that it will be useful, * 20 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 21 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * 22 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * 23 * * 24 *************************************************************************/ 25 26 /* optimized and unoptimized vector and matrix functions */ 27 28 module deimos.ode.matrix; 29 30 private import deimos.ode.common; 31 32 extern (C): 33 nothrow: 34 35 /* set a vector/matrix of size n to all zeros, or to a specific value. */ 36 37 void dSetZero(dReal* a, int n); 38 void dSetValue(dReal* a, int n, dReal value); 39 40 41 /* get the dot product of two n*1 vectors. if n <= 0 then 42 * zero will be returned (in which case a and b need not be valid). 43 */ 44 45 dReal dDot(in dReal* a, in dReal* b, int n); 46 47 48 /* get the dot products of (a0,b), (a1,b), etc and return them in outsum. 49 * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case 50 * the input vectors need not be valid). this function is somewhat faster 51 * than calling dDot() for all of the combinations separately. 52 */ 53 54 /* NOT INCLUDED in the library for now. 55 void dMultidot2 (const dReal* a0, const dReal* a1, 56 const dReal* b, dReal* outsum, int n); 57 */ 58 59 60 /* matrix multiplication. all matrices are stored in standard row format. 61 * the digit refers to the argument that is transposed: 62 * 0: A = B * C (sizes: A:p*r B:p*q C:q*r) 63 * 1: A = B' * C (sizes: A:p*r B:q*p C:q*r) 64 * 2: A = B * C' (sizes: A:p*r B:p*q C:r*q) 65 * case 1,2 are equivalent to saying that the operation is A=B*C but 66 * B or C are stored in standard column format. 67 */ 68 69 void dMultiply0(dReal* A, in dReal* B, in dReal* C, int p, int q, int r); 70 void dMultiply1(dReal* A, in dReal* B, in dReal* C, int p, int q, int r); 71 void dMultiply2(dReal* A, in dReal* B, in dReal* C, int p, int q, int r); 72 73 74 /* do an in-place cholesky decomposition on the lower triangle of the n*n 75 * symmetric matrix A (which is stored by rows). the resulting lower triangle 76 * will be such that L*L'=A. return 1 on success and 0 on failure (on failure 77 * the matrix is not positive definite). 78 */ 79 80 int dFactorCholesky(dReal* A, int n); 81 82 83 /* solve for x: L*L'*x = b, and put the result back into x. 84 * L is size n*n, b is size n*1. only the lower triangle of L is considered. 85 */ 86 87 void dSolveCholesky(in dReal* L, dReal* b, int n); 88 89 90 /* compute the inverse of the n*n positive definite matrix A and put it in 91 * Ainv. this is not especially fast. this returns 1 on success (A was 92 * positive definite) or 0 on failure (not PD). 93 */ 94 95 int dInvertPDMatrix(in dReal* A, dReal* Ainv, int n); 96 97 98 /* check whether an n*n matrix A is positive definite, return 1/0 (yes/no). 99 * positive definite means that x'*A*x > 0 for any x. this performs a 100 * cholesky decomposition of A. if the decomposition fails then the matrix 101 * is not positive definite. A is stored by rows. A is not altered. 102 */ 103 104 int dIsPositiveDefinite(in dReal* A, int n); 105 106 107 /* factorize a matrix A into L*D*L', where L is lower triangular with ones on 108 * the diagonal, and D is diagonal. 109 * A is an n*n matrix stored by rows, with a leading dimension of n rounded 110 * up to 4. L is written into the strict lower triangle of A (the ones are not 111 * written) and the reciprocal of the diagonal elements of D are written into 112 * d. 113 */ 114 void dFactorLDLT(dReal* A, dReal* d, int n, int nskip); 115 116 117 /* solve L*x=b, where L is n*n lower triangular with ones on the diagonal, 118 * and x,b are n*1. b is overwritten with x. 119 * the leading dimension of L is `nskip'. 120 */ 121 void dSolveL1(in dReal* L, dReal* b, int n, int nskip); 122 123 124 /* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal, 125 * and x,b are n*1. b is overwritten with x. 126 * the leading dimension of L is `nskip'. 127 */ 128 void dSolveL1T(in dReal* L, dReal* b, int n, int nskip); 129 130 131 /* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */ 132 133 void dVectorScale(dReal* a, in dReal* d, int n); 134 135 136 /* given `L', a n*n lower triangular matrix with ones on the diagonal, 137 * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix 138 * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b. 139 * the leading dimension of L is `nskip'. 140 */ 141 142 void dSolveLDLT(in dReal* L, in dReal* d, dReal* b, int n, int nskip); 143 144 145 /* given an L*D*L' factorization of an n*n matrix A, return the updated 146 * factorization L2*D2*L2' of A plus the following "top left" matrix: 147 * 148 * [ b a' ] <-- b is a[0] 149 * [ a 0 ] <-- a is a[1..n-1] 150 * 151 * - L has size n*n, its leading dimension is nskip. L is lower triangular 152 * with ones on the diagonal. only the lower triangle of L is referenced. 153 * - d has size n. d contains the reciprocal diagonal elements of D. 154 * - a has size n. 155 * the result is written into L, except that the left column of L and d[0] 156 * are not actually modified. see ldltaddTL.m for further comments. 157 */ 158 void dLDLTAddTL(dReal* L, dReal* d, in dReal* a, int n, int nskip); 159 160 161 /* given an L*D*L' factorization of a permuted matrix A, produce a new 162 * factorization for row and column `r' removed. 163 * - A has size n1*n1, its leading dimension in nskip. A is symmetric and 164 * positive definite. only the lower triangle of A is referenced. 165 * A itself may actually be an array of row pointers. 166 * - L has size n2*n2, its leading dimension in nskip. L is lower triangular 167 * with ones on the diagonal. only the lower triangle of L is referenced. 168 * - d has size n2. d contains the reciprocal diagonal elements of D. 169 * - p is a permutation vector. it contains n2 indexes into A. each index 170 * must be in the range 0..n1-1. 171 * - r is the row/column of L to remove. 172 * the new L will be written within the old L, i.e. will have the same leading 173 * dimension. the last row and column of L, and the last element of d, are 174 * undefined on exit. 175 * 176 * a fast O(n^2) algorithm is used. see ldltremove.m for further comments. 177 */ 178 void dLDLTRemove( 179 dReal** A, in int *p, dReal* L, dReal* d, int n1, int n2, int r, int nskip 180 ); 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 void dRemoveRowCol(dReal* A, int n, int nskip, int r);