ode/src/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 /*
  24  * optimized and unoptimized vector and matrix functions
  25  * (inlined private versions)
  26  */
  27
  28 #ifndef _ODE__PRIVATE_MATRIX_H_
  29 #define _ODE__PRIVATE_MATRIX_H_
  30
  31
  32 #include <ode/matrix.h>
  33
  34
  35 #ifdef __cplusplus
  36
  37 template <unsigned a_stride, typename element_type>
  38 ODE_INLINE
  39 void dxtSetZero (element_type *a, sizeint n)
  40 {
  41     element_type *const aend = a + n * a_stride;
  42     for (element_type *acurr = a; acurr != aend; acurr += a_stride) {
  43         *acurr = (element_type)0;
  44     }
  45 }
  46
  47 template <typename element_type>
  48 ODE_INLINE
  49 void dxSetZero (element_type *a, sizeint n)
  50 {
  51     dxtSetZero<1>(a, n);
  52 }
  53
  54 template <typename element_type>
  55 ODE_INLINE
  56 void dxSetValue (element_type *a, sizeint n, element_type value)
  57 {
  58     element_type *const aend = a + n;
  59     for (element_type *acurr = a; acurr != aend; ++acurr) {
  60         *acurr = value;
  61     }
  62 }
  63
  64
  65 #else // #ifndef __cplusplus
  66
  67 ODE_PURE_INLINE
  68 void dxSetZero (dReal *a, sizeint n)
  69 {
  70     dReal *const aend = a + n;
  71     dReal *acurr;
  72     for (acurr = a; acurr != aend; ++acurr) {
  73         *acurr = 0;
  74     }
  75 }
  76
  77 ODE_PURE_INLINE
  78 void dxSetValue (dReal *a, sizeint n, dReal value)
  79 {
  80     dReal *const aend = a + n;
  81     dReal *acurr;
  82     for (acurr = a; acurr != aend; ++acurr) {
  83         *acurr = value;
  84     }
  85 }
  86
  87
  88 #endif // #ifdef __cplusplus
  89
  90
  91 ODE_PURE_INLINE
  92 dReal dxCalculateModuloMaximum(const dReal *valueStorage, sizeint valueCount)
  93 {
  94     dIASSERT(valueCount != 0);
  95
  96     dReal moduleMaximum = dFabs(*valueStorage);
  97
  98     const dReal *const storageEnd = valueStorage + valueCount;
  99     for (const dReal *currentValue = valueStorage + 1; currentValue != storageEnd; ++currentValue) {
 100         moduleMaximum = dMax(moduleMaximum, dFabs(*currentValue));
 101     }
 102
 103     return moduleMaximum;
 104 }
 105
 106
 107 dReal dxDot (const dReal *a, const dReal *b, unsigned n);
 108 void dxMultiply0 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
 109 void dxMultiply1 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
 110 void dxMultiply2 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
 111 int dxFactorCholesky (dReal *A, unsigned n, void *tmpbuf);
 112 void dxSolveCholesky (const dReal *L, dReal *b, unsigned n, void *tmpbuf);
 113 int dxInvertPDMatrix (const dReal *A, dReal *Ainv, unsigned n, void *tmpbuf);
 114 int dxIsPositiveDefinite (const dReal *A, unsigned n, void *tmpbuf);
 115 void dxLDLTAddTL (dReal *L, dReal *d, const dReal *a, unsigned n, unsigned nskip, void *tmpbuf);
 116 void dxLDLTRemove (dReal **A, const unsigned *p, dReal *L, dReal *d, unsigned n1, unsigned n2, unsigned r, unsigned nskip, void *tmpbuf);
 117 void dxRemoveRowCol (dReal *A, unsigned n, unsigned nskip, unsigned r);
 118
 119 ODE_PURE_INLINE sizeint dxEstimateFactorCholeskyTmpbufSize(unsigned n)
 120 {
 121     return dPAD(n) * sizeof(dReal);
 122 }
 123
 124 ODE_PURE_INLINE sizeint dxEstimateSolveCholeskyTmpbufSize(unsigned n)
 125 {
 126     return dPAD(n) * sizeof(dReal);
 127 }
 128
 129 ODE_PURE_INLINE sizeint dxEstimateInvertPDMatrixTmpbufSize(unsigned n)
 130 {
 131     sizeint FactorCholesky_size = dxEstimateFactorCholeskyTmpbufSize(n);
 132     sizeint SolveCholesky_size = dxEstimateSolveCholeskyTmpbufSize(n);
 133     sizeint MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
 134     return (sizeint)dPAD(n) * (n + 1) * sizeof(dReal) + MaxCholesky_size;
 135 }
 136
 137 ODE_PURE_INLINE sizeint dxEstimateIsPositiveDefiniteTmpbufSize(unsigned n)
 138 {
 139     return (sizeint)dPAD(n) * n * sizeof(dReal) + dxEstimateFactorCholeskyTmpbufSize(n);
 140 }
 141
 142 ODE_PURE_INLINE sizeint dxEstimateLDLTAddTLTmpbufSize(unsigned nskip)
 143 {
 144     return nskip * (2 * sizeof(dReal));
 145 }
 146
 147 ODE_PURE_INLINE sizeint dxEstimateLDLTRemoveTmpbufSize(unsigned n2, unsigned nskip)
 148 {
 149     return n2 * sizeof(dReal) + dxEstimateLDLTAddTLTmpbufSize(nskip);
 150 }
 151
 152 /* For internal use */
 153 #define dSetZero(a, n) dxSetZero(a, n)
 154 #define dSetValue(a, n, value) dxSetValue(a, n, value)
 155 #define dDot(a, b, n) dxDot(a, b, n)
 156 #define dMultiply0(A, B, C, p, q, r) dxMultiply0(A, B, C, p, q, r)
 157 #define dMultiply1(A, B, C, p, q, r) dxMultiply1(A, B, C, p, q, r)
 158 #define dMultiply2(A, B, C, p, q, r) dxMultiply2(A, B, C, p, q, r)
 159 #define dFactorCholesky(A, n, tmpbuf) dxFactorCholesky(A, n, tmpbuf)
 160 #define dSolveCholesky(L, b, n, tmpbuf) dxSolveCholesky(L, b, n, tmpbuf)
 161 #define dInvertPDMatrix(A, Ainv, n, tmpbuf) dxInvertPDMatrix(A, Ainv, n, tmpbuf)
 162 #define dIsPositiveDefinite(A, n, tmpbuf) dxIsPositiveDefinite(A, n, tmpbuf)
 163 #define dLDLTAddTL(L, d, a, n, nskip, tmpbuf) dxLDLTAddTL(L, d, a, n, nskip, tmpbuf)
 164 #define dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf) dxLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf)
 165 #define dRemoveRowCol(A, n, nskip, r) dxRemoveRowCol(A, n, nskip, r)
 166
 167
 168 #define dEstimateFactorCholeskyTmpbufSize(n) dxEstimateFactorCholeskyTmpbufSize(n)
 169 #define dEstimateSolveCholeskyTmpbufSize(n) dxEstimateSolveCholeskyTmpbufSize(n)
 170 #define dEstimateInvertPDMatrixTmpbufSize(n) dxEstimateInvertPDMatrixTmpbufSize(n)
 171 #define dEstimateIsPositiveDefiniteTmpbufSize(n) dxEstimateIsPositiveDefiniteTmpbufSize(n)
 172 #define dEstimateLDLTAddTLTmpbufSize(nskip) dxEstimateLDLTAddTLTmpbufSize(nskip)
 173 #define dEstimateLDLTRemoveTmpbufSize(n2, nskip) dxEstimateLDLTRemoveTmpbufSize(n2, nskip)
 174
 175
 176 #endif // #ifndef _ODE__PRIVATE_MATRIX_H_