diff options
author | dan miller | 2007-10-19 05:20:07 +0000 |
---|---|---|
committer | dan miller | 2007-10-19 05:20:07 +0000 |
commit | fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd (patch) | |
tree | 51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/ode/src/mat.cpp | |
parent | resubmitting ode (diff) | |
download | opensim-SC-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.zip opensim-SC-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.tar.gz opensim-SC-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.tar.bz2 opensim-SC-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.tar.xz |
dont ask
Diffstat (limited to 'libraries/ode-0.9/ode/src/mat.cpp')
-rw-r--r-- | libraries/ode-0.9/ode/src/mat.cpp | 230 |
1 files changed, 0 insertions, 230 deletions
diff --git a/libraries/ode-0.9/ode/src/mat.cpp b/libraries/ode-0.9/ode/src/mat.cpp deleted file mode 100644 index 6e635dc..0000000 --- a/libraries/ode-0.9/ode/src/mat.cpp +++ /dev/null | |||
@@ -1,230 +0,0 @@ | |||
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 | #include <ode/config.h> | ||
24 | #include <ode/misc.h> | ||
25 | #include <ode/matrix.h> | ||
26 | #include <ode/error.h> | ||
27 | #include <ode/memory.h> | ||
28 | #include "mat.h" | ||
29 | |||
30 | |||
31 | dMatrix::dMatrix() | ||
32 | { | ||
33 | n = 0; | ||
34 | m = 0; | ||
35 | data = 0; | ||
36 | } | ||
37 | |||
38 | |||
39 | dMatrix::dMatrix (int rows, int cols) | ||
40 | { | ||
41 | if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); | ||
42 | n = rows; | ||
43 | m = cols; | ||
44 | data = (dReal*) dAlloc (n*m*sizeof(dReal)); | ||
45 | dSetZero (data,n*m); | ||
46 | } | ||
47 | |||
48 | |||
49 | dMatrix::dMatrix (const dMatrix &a) | ||
50 | { | ||
51 | n = a.n; | ||
52 | m = a.m; | ||
53 | data = (dReal*) dAlloc (n*m*sizeof(dReal)); | ||
54 | memcpy (data,a.data,n*m*sizeof(dReal)); | ||
55 | } | ||
56 | |||
57 | |||
58 | dMatrix::dMatrix (int rows, int cols, | ||
59 | dReal *_data, int rowskip, int colskip) | ||
60 | { | ||
61 | if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); | ||
62 | n = rows; | ||
63 | m = cols; | ||
64 | data = (dReal*) dAlloc (n*m*sizeof(dReal)); | ||
65 | for (int i=0; i<n; i++) { | ||
66 | for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip]; | ||
67 | } | ||
68 | } | ||
69 | |||
70 | |||
71 | dMatrix::~dMatrix() | ||
72 | { | ||
73 | if (data) dFree (data,n*m*sizeof(dReal)); | ||
74 | } | ||
75 | |||
76 | |||
77 | dReal & dMatrix::operator () (int i, int j) | ||
78 | { | ||
79 | if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)"); | ||
80 | return data [i*m+j]; | ||
81 | } | ||
82 | |||
83 | |||
84 | void dMatrix::operator= (const dMatrix &a) | ||
85 | { | ||
86 | if (data) dFree (data,n*m*sizeof(dReal)); | ||
87 | n = a.n; | ||
88 | m = a.m; | ||
89 | if (n > 0 && m > 0) { | ||
90 | data = (dReal*) dAlloc (n*m*sizeof(dReal)); | ||
91 | memcpy (data,a.data,n*m*sizeof(dReal)); | ||
92 | } | ||
93 | else data = 0; | ||
94 | } | ||
95 | |||
96 | |||
97 | void dMatrix::operator= (dReal a) | ||
98 | { | ||
99 | for (int i=0; i<n*m; i++) data[i] = a; | ||
100 | } | ||
101 | |||
102 | |||
103 | dMatrix dMatrix::transpose() | ||
104 | { | ||
105 | dMatrix r (m,n); | ||
106 | for (int i=0; i<n; i++) { | ||
107 | for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j]; | ||
108 | } | ||
109 | return r; | ||
110 | } | ||
111 | |||
112 | |||
113 | dMatrix dMatrix::select (int np, int *p, int nq, int *q) | ||
114 | { | ||
115 | if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes"); | ||
116 | dMatrix r (np,nq); | ||
117 | for (int i=0; i<np; i++) { | ||
118 | for (int j=0; j<nq; j++) { | ||
119 | if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m) | ||
120 | dDebug (0,"Matrix select, bad index arrays"); | ||
121 | r.data[i*nq+j] = data[p[i]*m+q[j]]; | ||
122 | } | ||
123 | } | ||
124 | return r; | ||
125 | } | ||
126 | |||
127 | |||
128 | dMatrix dMatrix::operator + (const dMatrix &a) | ||
129 | { | ||
130 | if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes"); | ||
131 | dMatrix r (n,m); | ||
132 | for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i]; | ||
133 | return r; | ||
134 | } | ||
135 | |||
136 | |||
137 | dMatrix dMatrix::operator - (const dMatrix &a) | ||
138 | { | ||
139 | if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes"); | ||
140 | dMatrix r (n,m); | ||
141 | for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i]; | ||
142 | return r; | ||
143 | } | ||
144 | |||
145 | |||
146 | dMatrix dMatrix::operator - () | ||
147 | { | ||
148 | dMatrix r (n,m); | ||
149 | for (int i=0; i<n*m; i++) r.data[i] = -data[i]; | ||
150 | return r; | ||
151 | } | ||
152 | |||
153 | |||
154 | dMatrix dMatrix::operator * (const dMatrix &a) | ||
155 | { | ||
156 | if (m != a.n) dDebug (0,"matrix *, mismatched sizes"); | ||
157 | dMatrix r (n,a.m); | ||
158 | for (int i=0; i<n; i++) { | ||
159 | for (int j=0; j<a.m; j++) { | ||
160 | dReal sum = 0; | ||
161 | for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j]; | ||
162 | r.data [i*a.m+j] = sum; | ||
163 | } | ||
164 | } | ||
165 | return r; | ||
166 | } | ||
167 | |||
168 | |||
169 | void dMatrix::operator += (const dMatrix &a) | ||
170 | { | ||
171 | if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes"); | ||
172 | for (int i=0; i<n*m; i++) data[i] += a.data[i]; | ||
173 | } | ||
174 | |||
175 | |||
176 | void dMatrix::operator -= (const dMatrix &a) | ||
177 | { | ||
178 | if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes"); | ||
179 | for (int i=0; i<n*m; i++) data[i] -= a.data[i]; | ||
180 | } | ||
181 | |||
182 | |||
183 | void dMatrix::clearUpperTriangle() | ||
184 | { | ||
185 | if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices"); | ||
186 | for (int i=0; i<n; i++) { | ||
187 | for (int j=i+1; j<m; j++) data[i*m+j] = 0; | ||
188 | } | ||
189 | } | ||
190 | |||
191 | |||
192 | void dMatrix::clearLowerTriangle() | ||
193 | { | ||
194 | if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices"); | ||
195 | for (int i=0; i<n; i++) { | ||
196 | for (int j=0; j<i; j++) data[i*m+j] = 0; | ||
197 | } | ||
198 | } | ||
199 | |||
200 | |||
201 | void dMatrix::makeRandom (dReal range) | ||
202 | { | ||
203 | for (int i=0; i<n; i++) { | ||
204 | for (int j=0; j<m; j++) | ||
205 | data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; | ||
206 | } | ||
207 | } | ||
208 | |||
209 | |||
210 | void dMatrix::print (char *fmt, FILE *f) | ||
211 | { | ||
212 | for (int i=0; i<n; i++) { | ||
213 | for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]); | ||
214 | fprintf (f,"\n"); | ||
215 | } | ||
216 | } | ||
217 | |||
218 | |||
219 | dReal dMatrix::maxDifference (const dMatrix &a) | ||
220 | { | ||
221 | if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes"); | ||
222 | dReal max = 0; | ||
223 | for (int i=0; i<n; i++) { | ||
224 | for (int j=0; j<m; j++) { | ||
225 | dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]); | ||
226 | if (diff > max) max = diff; | ||
227 | } | ||
228 | } | ||
229 | return max; | ||
230 | } | ||