add ode

1 files changed, 160 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/matrix.h b/libs/ode-0.16.1/ode/src/matrix.h
new file mode 100644
index 0000000..b723722
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/matrix.h
@@ -0,0 +1,160 @@
+/*************************************************************************

+ *                                                                       *

+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *

+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

+ *                                                                       *

+ * This library is free software; you can redistribute it and/or         *

+ * modify it under the terms of EITHER:                                  *

+ *   (1) The GNU Lesser General Public License as published by the Free  *

+ *       Software Foundation; either version 2.1 of the License, or (at  *

+ *       your option) any later version. The text of the GNU Lesser      *

+ *       General Public License is included with this library in the     *

+ *       file LICENSE.TXT.                                               *

+ *   (2) The BSD-style license that is included with this library in     *

+ *       the file LICENSE-BSD.TXT.                                       *

+ *                                                                       *

+ * This library is distributed in the hope that it will be useful,       *

+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *

+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *

+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

+ *                                                                       *

+ *************************************************************************/

+

+/* 

+ * optimized and unoptimized vector and matrix functions 

+ * (inlined private versions)

+ */

+

+#ifndef _ODE__PRIVATE_MATRIX_H_

+#define _ODE__PRIVATE_MATRIX_H_

+

+

+#include <ode/matrix.h>

+

+

+#ifdef __cplusplus

+

+template <unsigned a_stride, typename element_type>

+ODE_INLINE

+void dxtSetZero (element_type *a, sizeint n)

+{

+    element_type *const aend = a + n * a_stride;

+    for (element_type *acurr = a; acurr != aend; acurr += a_stride) {

+        *acurr = (element_type)0;

+    }

+}

+

+template <typename element_type>

+ODE_INLINE

+void dxSetZero (element_type *a, sizeint n)

+{

+    dxtSetZero<1>(a, n);

+}

+

+template <typename element_type>

+ODE_INLINE

+void dxSetValue (element_type *a, sizeint n, element_type value)

+{

+    element_type *const aend = a + n;

+    for (element_type *acurr = a; acurr != aend; ++acurr) {

+        *acurr = value;

+    }

+}

+

+

+#else // #ifndef __cplusplus

+

+ODE_PURE_INLINE

+void dxSetZero (dReal *a, sizeint n)

+{

+    dReal *const aend = a + n;

+    dReal *acurr;

+    for (acurr = a; acurr != aend; ++acurr) {

+        *acurr = 0;

+    }

+}

+

+ODE_PURE_INLINE

+void dxSetValue (dReal *a, sizeint n, dReal value)

+{

+    dReal *const aend = a + n;

+    dReal *acurr;

+    for (acurr = a; acurr != aend; ++acurr) {

+        *acurr = value;

+    }

+}

+

+

+#endif // #ifdef __cplusplus

+

+

+dReal dxDot (const dReal *a, const dReal *b, unsigned n);

+void dxMultiply0 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);

+void dxMultiply1 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);

+void dxMultiply2 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);

+int dxFactorCholesky (dReal *A, unsigned n, void *tmpbuf);

+void dxSolveCholesky (const dReal *L, dReal *b, unsigned n, void *tmpbuf);

+int dxInvertPDMatrix (const dReal *A, dReal *Ainv, unsigned n, void *tmpbuf);

+int dxIsPositiveDefinite (const dReal *A, unsigned n, void *tmpbuf);

+void dxLDLTAddTL (dReal *L, dReal *d, const dReal *a, unsigned n, unsigned nskip, void *tmpbuf);

+void dxLDLTRemove (dReal **A, const unsigned *p, dReal *L, dReal *d, unsigned n1, unsigned n2, unsigned r, unsigned nskip, void *tmpbuf);

+void dxRemoveRowCol (dReal *A, unsigned n, unsigned nskip, unsigned r);

+

+ODE_PURE_INLINE sizeint dxEstimateFactorCholeskyTmpbufSize(unsigned n)

+{

+    return dPAD(n) * sizeof(dReal);

+}

+

+ODE_PURE_INLINE sizeint dxEstimateSolveCholeskyTmpbufSize(unsigned n)

+{

+    return dPAD(n) * sizeof(dReal);

+}

+

+ODE_PURE_INLINE sizeint dxEstimateInvertPDMatrixTmpbufSize(unsigned n)

+{

+    sizeint FactorCholesky_size = dxEstimateFactorCholeskyTmpbufSize(n);

+    sizeint SolveCholesky_size = dxEstimateSolveCholeskyTmpbufSize(n);

+    sizeint MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;

+    return (sizeint)dPAD(n) * (n + 1) * sizeof(dReal) + MaxCholesky_size;

+}

+

+ODE_PURE_INLINE sizeint dxEstimateIsPositiveDefiniteTmpbufSize(unsigned n)

+{

+    return (sizeint)dPAD(n) * n * sizeof(dReal) + dxEstimateFactorCholeskyTmpbufSize(n);

+}

+

+ODE_PURE_INLINE sizeint dxEstimateLDLTAddTLTmpbufSize(unsigned nskip)

+{

+    return nskip * (2 * sizeof(dReal));

+}

+

+ODE_PURE_INLINE sizeint dxEstimateLDLTRemoveTmpbufSize(unsigned n2, unsigned nskip)

+{

+    return n2 * sizeof(dReal) + dxEstimateLDLTAddTLTmpbufSize(nskip);

+}

+

+/* For internal use */

+#define dSetZero(a, n) dxSetZero(a, n)

+#define dSetValue(a, n, value) dxSetValue(a, n, value)

+#define dDot(a, b, n) dxDot(a, b, n)

+#define dMultiply0(A, B, C, p, q, r) dxMultiply0(A, B, C, p, q, r)

+#define dMultiply1(A, B, C, p, q, r) dxMultiply1(A, B, C, p, q, r)

+#define dMultiply2(A, B, C, p, q, r) dxMultiply2(A, B, C, p, q, r)

+#define dFactorCholesky(A, n, tmpbuf) dxFactorCholesky(A, n, tmpbuf)

+#define dSolveCholesky(L, b, n, tmpbuf) dxSolveCholesky(L, b, n, tmpbuf)

+#define dInvertPDMatrix(A, Ainv, n, tmpbuf) dxInvertPDMatrix(A, Ainv, n, tmpbuf)

+#define dIsPositiveDefinite(A, n, tmpbuf) dxIsPositiveDefinite(A, n, tmpbuf)

+#define dLDLTAddTL(L, d, a, n, nskip, tmpbuf) dxLDLTAddTL(L, d, a, n, nskip, tmpbuf)

+#define dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf) dxLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf)

+#define dRemoveRowCol(A, n, nskip, r) dxRemoveRowCol(A, n, nskip, r)

+

+

+#define dEstimateFactorCholeskyTmpbufSize(n) dxEstimateFactorCholeskyTmpbufSize(n)

+#define dEstimateSolveCholeskyTmpbufSize(n) dxEstimateSolveCholeskyTmpbufSize(n)

+#define dEstimateInvertPDMatrixTmpbufSize(n) dxEstimateInvertPDMatrixTmpbufSize(n)

+#define dEstimateIsPositiveDefiniteTmpbufSize(n) dxEstimateIsPositiveDefiniteTmpbufSize(n)

+#define dEstimateLDLTAddTLTmpbufSize(nskip) dxEstimateLDLTAddTLTmpbufSize(nskip)

+#define dEstimateLDLTRemoveTmpbufSize(n2, nskip) dxEstimateLDLTRemoveTmpbufSize(n2, nskip)

+

+

+#endif // #ifndef _ODE__PRIVATE_MATRIX_H_