diff options
author | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
---|---|---|
committer | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
commit | c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch) | |
tree | 277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/ode/src/mat.cpp | |
parent | 1cf9cc3408af7008451f9133fb95af66a9697d15 (diff) |
add ode
Diffstat (limited to 'libs/ode-0.16.1/ode/src/mat.cpp')
-rw-r--r-- | libs/ode-0.16.1/ode/src/mat.cpp | 231 |
1 files changed, 231 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/mat.cpp b/libs/ode-0.16.1/ode/src/mat.cpp new file mode 100644 index 0000000..67f5673 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/mat.cpp @@ -0,0 +1,231 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/misc.h> +#include <ode/error.h> +#include <ode/memory.h> +#include "config.h" +#include "matrix.h" +#include "mat.h" + + +dMatrix::dMatrix() +{ + n = 0; + m = 0; + data = 0; +} + + +dMatrix::dMatrix (int rows, int cols) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + dSetZero (data,n*m); +} + + +dMatrix::dMatrix (const dMatrix &a) +{ + n = a.n; + m = a.m; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); +} + + +dMatrix::dMatrix (int rows, int cols, + dReal *_data, int rowskip, int colskip) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip]; + } +} + + +dMatrix::~dMatrix() +{ + if (data) dFree (data,n*m*sizeof(dReal)); +} + + +dReal & dMatrix::operator () (int i, int j) +{ + if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)"); + return data [i*m+j]; +} + + +void dMatrix::operator= (const dMatrix &a) +{ + if (data) dFree (data,n*m*sizeof(dReal)); + n = a.n; + m = a.m; + if (n > 0 && m > 0) { + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); + } + else data = 0; +} + + +void dMatrix::operator= (dReal a) +{ + for (int i=0; i<n*m; i++) data[i] = a; +} + + +dMatrix dMatrix::transpose() +{ + dMatrix r (m,n); + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j]; + } + return r; +} + + +dMatrix dMatrix::select (int np, int *p, int nq, int *q) +{ + if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes"); + dMatrix r (np,nq); + for (int i=0; i<np; i++) { + for (int j=0; j<nq; j++) { + if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m) + dDebug (0,"Matrix select, bad index arrays"); + r.data[i*nq+j] = data[p[i]*m+q[j]]; + } + } + return r; +} + + +dMatrix dMatrix::operator + (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes"); + dMatrix r (n,m); + for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i]; + return r; +} + + +dMatrix dMatrix::operator - (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes"); + dMatrix r (n,m); + for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i]; + return r; +} + + +dMatrix dMatrix::operator - () +{ + dMatrix r (n,m); + for (int i=0; i<n*m; i++) r.data[i] = -data[i]; + return r; +} + + +dMatrix dMatrix::operator * (const dMatrix &a) +{ + if (m != a.n) dDebug (0,"matrix *, mismatched sizes"); + dMatrix r (n,a.m); + for (int i=0; i<n; i++) { + for (int j=0; j<a.m; j++) { + dReal sum = 0; + for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j]; + r.data [i*a.m+j] = sum; + } + } + return r; +} + + +void dMatrix::operator += (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes"); + for (int i=0; i<n*m; i++) data[i] += a.data[i]; +} + + +void dMatrix::operator -= (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes"); + for (int i=0; i<n*m; i++) data[i] -= a.data[i]; +} + + +void dMatrix::clearUpperTriangle() +{ + if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices"); + for (int i=0; i<n; i++) { + for (int j=i+1; j<m; j++) data[i*m+j] = 0; + } +} + + +void dMatrix::clearLowerTriangle() +{ + if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices"); + for (int i=0; i<n; i++) { + for (int j=0; j<i; j++) data[i*m+j] = 0; + } +} + + +void dMatrix::makeRandom (dReal range) +{ + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) + data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; + } +} + + +void dMatrix::print (const char *fmt, FILE *f) +{ + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]); + fprintf (f,"\n"); + } +} + + +dReal dMatrix::maxDifference (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes"); + dReal max = 0; + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) { + dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]); + if (diff > max) max = diff; + } + } + return max; +} |