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 | } |
---|