summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/mat.cpp
diff options
context:
space:
mode:
authorsanine <sanine.not@pm.me>2022-10-01 20:59:36 -0500
committersanine <sanine.not@pm.me>2022-10-01 20:59:36 -0500
commitc5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch)
tree277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/ode/src/mat.cpp
parent1cf9cc3408af7008451f9133fb95af66a9697d15 (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.cpp231
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;
+}