ode/src/fastldltsolve_impl.h

   1
   2
   3 /*************************************************************************
   4  *                                                                       *
   5  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   6  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   7  *                                                                       *
   8  * This library is free software; you can redistribute it and/or         *
   9  * modify it under the terms of EITHER:                                  *
  10  *   (1) The GNU Lesser General Public License as published by the Free  *
  11  *       Software Foundation; either version 2.1 of the License, or (at  *
  12  *       your option) any later version. The text of the GNU Lesser      *
  13  *       General Public License is included with this library in the     *
  14  *       file LICENSE.TXT.                                               *
  15  *   (2) The BSD-style license that is included with this library in     *
  16  *       the file LICENSE-BSD.TXT.                                       *
  17  *                                                                       *
  18  * This library is distributed in the hope that it will be useful,       *
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  21  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  22  *                                                                       *
  23  *************************************************************************/
  24
  25 #ifndef _ODE_MATRIX_IMPL_H_
  26 #define _ODE_MATRIX_IMPL_H_
  27
  28
  29 #include "fastlsolve_impl.h"
  30 #include "fastltsolve_impl.h"
  31 #include "fastvecscale_impl.h"
  32
  33
  34 template<unsigned int d_stride, unsigned int b_stride>
  35 void solveEquationSystemWithLDLT(const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip)
  36 {
  37     dAASSERT(L != NULL);
  38     dAASSERT(d != NULL);
  39     dAASSERT(b != NULL);
  40     dAASSERT(rowCount > 0);
  41     dAASSERT(rowSkip >= rowCount);
  42
  43     solveL1Straight<b_stride>(L, b, rowCount, rowSkip);
  44     scaleLargeVector<b_stride, d_stride>(b, d, rowCount);
  45     solveL1Transposed<b_stride>(L, b, rowCount, rowSkip);
  46 }
  47
  48
  49 #endif