|
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 n = rows; |
|
42 m = cols; |
|
43 data = (dReal*) dAlloc (n*m*sizeof(dReal)); |
|
44 dSetZero (data,n*m); |
|
45 } |
|
46 |
|
47 |
|
48 dMatrix::dMatrix (const dMatrix &a) |
|
49 { |
|
50 n = a.n; |
|
51 m = a.m; |
|
52 data = (dReal*) dAlloc (n*m*sizeof(dReal)); |
|
53 memcpy (data,a.data,n*m*sizeof(dReal)); |
|
54 } |
|
55 |
|
56 |
|
57 dMatrix::dMatrix (int rows, int cols, |
|
58 dReal *_data, int rowskip, int colskip) |
|
59 { |
|
60 n = rows; |
|
61 m = cols; |
|
62 data = (dReal*) dAlloc (n*m*sizeof(dReal)); |
|
63 for (int i=0; i<n; i++) { |
|
64 for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip]; |
|
65 } |
|
66 } |
|
67 |
|
68 |
|
69 dMatrix::~dMatrix() |
|
70 { |
|
71 if (data) dFree (data,n*m*sizeof(dReal)); |
|
72 } |
|
73 |
|
74 |
|
75 dReal & dMatrix::operator () (int i, int j) |
|
76 { |
|
77 return data [i*m+j]; |
|
78 } |
|
79 |
|
80 |
|
81 void dMatrix::operator= (const dMatrix &a) |
|
82 { |
|
83 if (data) dFree (data,n*m*sizeof(dReal)); |
|
84 n = a.n; |
|
85 m = a.m; |
|
86 if (n > 0 && m > 0) { |
|
87 data = (dReal*) dAlloc (n*m*sizeof(dReal)); |
|
88 memcpy (data,a.data,n*m*sizeof(dReal)); |
|
89 } |
|
90 else data = 0; |
|
91 } |
|
92 |
|
93 |
|
94 void dMatrix::operator= (dReal a) |
|
95 { |
|
96 for (int i=0; i<n*m; i++) data[i] = a; |
|
97 } |
|
98 |
|
99 |
|
100 dMatrix dMatrix::transpose() |
|
101 { |
|
102 dMatrix r (m,n); |
|
103 for (int i=0; i<n; i++) { |
|
104 for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j]; |
|
105 } |
|
106 return r; |
|
107 } |
|
108 |
|
109 |
|
110 dMatrix dMatrix::select (int np, int *p, int nq, int *q) |
|
111 { |
|
112 dMatrix r (np,nq); |
|
113 for (int i=0; i<np; i++) { |
|
114 for (int j=0; j<nq; j++) { |
|
115 if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m) |
|
116 r.data[i*nq+j] = data[p[i]*m+q[j]]; |
|
117 } |
|
118 } |
|
119 return r; |
|
120 } |
|
121 |
|
122 |
|
123 dMatrix dMatrix::operator + (const dMatrix &a) |
|
124 { |
|
125 dMatrix r (n,m); |
|
126 for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i]; |
|
127 return r; |
|
128 } |
|
129 |
|
130 |
|
131 dMatrix dMatrix::operator - (const dMatrix &a) |
|
132 { |
|
133 dMatrix r (n,m); |
|
134 for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i]; |
|
135 return r; |
|
136 } |
|
137 |
|
138 |
|
139 dMatrix dMatrix::operator - () |
|
140 { |
|
141 dMatrix r (n,m); |
|
142 for (int i=0; i<n*m; i++) r.data[i] = -data[i]; |
|
143 return r; |
|
144 } |
|
145 |
|
146 |
|
147 dMatrix dMatrix::operator * (const dMatrix &a) |
|
148 { |
|
149 dMatrix r (n,a.m); |
|
150 for (int i=0; i<n; i++) { |
|
151 for (int j=0; j<a.m; j++) { |
|
152 dReal sum = 0; |
|
153 for (int k=0; k<m; k++) sum += dMUL(data[i*m+k],a.data[k*a.m+j]); |
|
154 r.data [i*a.m+j] = sum; |
|
155 } |
|
156 } |
|
157 return r; |
|
158 } |
|
159 |
|
160 |
|
161 void dMatrix::operator += (const dMatrix &a) |
|
162 { |
|
163 for (int i=0; i<n*m; i++) data[i] += a.data[i]; |
|
164 } |
|
165 |
|
166 |
|
167 void dMatrix::operator -= (const dMatrix &a) |
|
168 { |
|
169 for (int i=0; i<n*m; i++) data[i] -= a.data[i]; |
|
170 } |
|
171 |
|
172 |
|
173 void dMatrix::clearUpperTriangle() |
|
174 { |
|
175 for (int i=0; i<n; i++) { |
|
176 for (int j=i+1; j<m; j++) data[i*m+j] = 0; |
|
177 } |
|
178 } |
|
179 |
|
180 |
|
181 void dMatrix::clearLowerTriangle() |
|
182 { |
|
183 for (int i=0; i<n; i++) { |
|
184 for (int j=0; j<i; j++) data[i*m+j] = 0; |
|
185 } |
|
186 } |
|
187 |
|
188 |
|
189 void dMatrix::makeRandom (dReal range) |
|
190 { |
|
191 for (int i=0; i<n; i++) { |
|
192 for (int j=0; j<m; j++) |
|
193 data[i*m+j] = dMUL((dMUL(dRandReal(),REAL(2.0))-REAL(1.0)),range); |
|
194 } |
|
195 } |
|
196 |
|
197 |
|
198 void dMatrix::print (char *fmt, FILE *f) |
|
199 { |
|
200 for (int i=0; i<n; i++) { |
|
201 for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]); |
|
202 fprintf (f,"\n"); |
|
203 } |
|
204 } |
|
205 |
|
206 |
|
207 dReal dMatrix::maxDifference (const dMatrix &a) |
|
208 { |
|
209 dReal max = 0; |
|
210 for (int i=0; i<n; i++) { |
|
211 for (int j=0; j<m; j++) { |
|
212 dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]); |
|
213 if (diff > max) max = diff; |
|
214 } |
|
215 } |
|
216 return max; |
|
217 } |