misc/builddeps/dp.linux64/include/ode/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
 190 #if defined(__ODE__)
 191
 192 void _dSetZero (dReal *a, size_t n);
 193 void _dSetValue (dReal *a, size_t n, dReal value);
 194 dReal _dDot (const dReal *a, const dReal *b, int n);
 195 void _dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
 196 void _dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
 197 void _dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
 198 int _dFactorCholesky (dReal *A, int n, void *tmpbuf);
 199 void _dSolveCholesky (const dReal *L, dReal *b, int n, void *tmpbuf);
 200 int _dInvertPDMatrix (const dReal *A, dReal *Ainv, int n, void *tmpbuf);
 201 int _dIsPositiveDefinite (const dReal *A, int n, void *tmpbuf);
 202 void _dFactorLDLT (dReal *A, dReal *d, int n, int nskip);
 203 void _dSolveL1 (const dReal *L, dReal *b, int n, int nskip);
 204 void _dSolveL1T (const dReal *L, dReal *b, int n, int nskip);
 205 void _dVectorScale (dReal *a, const dReal *d, int n);
 206 void _dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip);
 207 void _dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip, void *tmpbuf);
 208 void _dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, int n1, int n2, int r, int nskip, void *tmpbuf);
 209 void _dRemoveRowCol (dReal *A, int n, int nskip, int r);
 210
 211 PURE_INLINE size_t _dEstimateFactorCholeskyTmpbufSize(int n)
 212 {
 213   return dPAD(n) * sizeof(dReal);
 214 }
 215
 216 PURE_INLINE size_t _dEstimateSolveCholeskyTmpbufSize(int n)
 217 {
 218   return dPAD(n) * sizeof(dReal);
 219 }
 220
 221 PURE_INLINE size_t _dEstimateInvertPDMatrixTmpbufSize(int n)
 222 {
 223   size_t FactorCholesky_size = _dEstimateFactorCholeskyTmpbufSize(n);
 224   size_t SolveCholesky_size = _dEstimateSolveCholeskyTmpbufSize(n);
 225   size_t MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
 226   return dPAD(n) * (n + 1) * sizeof(dReal) + MaxCholesky_size;
 227 }
 228
 229 PURE_INLINE size_t _dEstimateIsPositiveDefiniteTmpbufSize(int n)
 230 {
 231   return dPAD(n) * n * sizeof(dReal) + _dEstimateFactorCholeskyTmpbufSize(n);
 232 }
 233
 234 PURE_INLINE size_t _dEstimateLDLTAddTLTmpbufSize(int nskip)
 235 {
 236   return nskip * 2 * sizeof(dReal);
 237 }
 238
 239 PURE_INLINE size_t _dEstimateLDLTRemoveTmpbufSize(int n2, int nskip)
 240 {
 241   return n2 * sizeof(dReal) + _dEstimateLDLTAddTLTmpbufSize(nskip);
 242 }
 243
 244 // For internal use
 245 #define dSetZero(a, n) _dSetZero(a, n)
 246 #define dSetValue(a, n, value) _dSetValue(a, n, value)
 247 #define dDot(a, b, n) _dDot(a, b, n)
 248 #define dMultiply0(A, B, C, p, q, r) _dMultiply0(A, B, C, p, q, r)
 249 #define dMultiply1(A, B, C, p, q, r) _dMultiply1(A, B, C, p, q, r)
 250 #define dMultiply2(A, B, C, p, q, r) _dMultiply2(A, B, C, p, q, r)
 251 #define dFactorCholesky(A, n, tmpbuf) _dFactorCholesky(A, n, tmpbuf)
 252 #define dSolveCholesky(L, b, n, tmpbuf) _dSolveCholesky(L, b, n, tmpbuf)
 253 #define dInvertPDMatrix(A, Ainv, n, tmpbuf) _dInvertPDMatrix(A, Ainv, n, tmpbuf)
 254 #define dIsPositiveDefinite(A, n, tmpbuf) _dIsPositiveDefinite(A, n, tmpbuf)
 255 #define dFactorLDLT(A, d, n, nskip) _dFactorLDLT(A, d, n, nskip)
 256 #define dSolveL1(L, b, n, nskip) _dSolveL1(L, b, n, nskip)
 257 #define dSolveL1T(L, b, n, nskip) _dSolveL1T(L, b, n, nskip)
 258 #define dVectorScale(a, d, n) _dVectorScale(a, d, n)
 259 #define dSolveLDLT(L, d, b, n, nskip) _dSolveLDLT(L, d, b, n, nskip)
 260 #define dLDLTAddTL(L, d, a, n, nskip, tmpbuf) _dLDLTAddTL(L, d, a, n, nskip, tmpbuf)
 261 #define dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf) _dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf)
 262 #define dRemoveRowCol(A, n, nskip, r) _dRemoveRowCol(A, n, nskip, r)
 263
 264
 265 #define dEstimateFactorCholeskyTmpbufSize(n) _dEstimateFactorCholeskyTmpbufSize(n)
 266 #define dEstimateSolveCholeskyTmpbufSize(n) _dEstimateSolveCholeskyTmpbufSize(n)
 267 #define dEstimateInvertPDMatrixTmpbufSize(n) _dEstimateInvertPDMatrixTmpbufSize(n)
 268 #define dEstimateIsPositiveDefiniteTmpbufSize(n) _dEstimateIsPositiveDefiniteTmpbufSize(n)
 269 #define dEstimateLDLTAddTLTmpbufSize(nskip) _dEstimateLDLTAddTLTmpbufSize(nskip)
 270 #define dEstimateLDLTRemoveTmpbufSize(n2, nskip) _dEstimateLDLTRemoveTmpbufSize(n2, nskip)
 271
 272
 273 #endif // defined(__ODE__)
 274
 275
 276 #ifdef __cplusplus
 277 }
 278 #endif
 279
 280 #endif