Simbody  3.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
VectorMath.h
Go to the documentation of this file.
1 #ifndef SimTK_SimTKCOMMON_VECTOR_MATH_H_
2 #define SimTK_SimTKCOMMON_VECTOR_MATH_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2008-12 Stanford University and the Authors. *
13  * Authors: Peter Eastman *
14  * Contributors: Michael Sherman *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKcommon/basics.h"
28 #include "SimTKcommon/Simmatrix.h"
29 
30 #include <cmath> // for std:sin, sqrt, etc.
31 #include <algorithm> // for std:sort, nth_element, etc.
32 
39 namespace SimTK {
40 
41 // We can use a single definition for a number of functions that simply call a
42 // function on each element, returning a value of the same type.
43 // Note that some of these intentionally copy their argument for use as a temp.
44 
45 #define SimTK_ELEMENTWISE_FUNCTION(func) \
46 template <class ELEM> \
47 VectorBase<ELEM> func(const VectorBase<ELEM>& v) { \
48  const int size = v.size(); \
49  Vector_<ELEM> temp(size); \
50  for (int i = 0; i < size; ++i) \
51  temp[i] = std::func(v[i]); \
52  return temp; \
53 } \
54 template <class ELEM> \
55 RowVectorBase<ELEM> func(const RowVectorBase<ELEM>& v){\
56  const int size = v.size(); \
57  RowVector_<ELEM> temp(size); \
58  for (int i = 0; i < size; ++i) \
59  temp[i] = std::func(v[i]); \
60  return temp; \
61 } \
62 template <class ELEM> \
63 MatrixBase<ELEM> func(const MatrixBase<ELEM>& v) { \
64  const int rows = v.nrow(), cols = v.ncol(); \
65  Matrix_<ELEM> temp(rows, cols); \
66  for (int i = 0; i < rows; ++i) \
67  for (int j = 0; j < cols; ++j) \
68  temp(i, j) = std::func(v(i, j)); \
69  return temp; \
70 } \
71 template <int N, class ELEM> \
72 Vec<N, ELEM> func(Vec<N, ELEM> v) { \
73  for (int i = 0; i < N; ++i) \
74  v[i] = std::func(v[i]); \
75  return v; \
76 } \
77 template <int N, class ELEM> \
78 Row<N, ELEM> func(Row<N, ELEM> v) { \
79  for (int i = 0; i < N; ++i) \
80  v[i] = std::func(v[i]); \
81  return v; \
82 } \
83 template <int M, int N, class ELEM> \
84 Mat<M, N, ELEM> func(Mat<M, N, ELEM> v) { \
85  for (int i = 0; i < M; ++i) \
86  for (int j = 0; j < N; ++j) \
87  v(i, j) = std::func(v(i, j)); \
88  return v; \
89 } \
90 template <int N, class ELEM> \
91 SymMat<N, ELEM> func(SymMat<N, ELEM> v) { \
92  for (int i = 0; i < N; ++i) \
93  for (int j = 0; j <= i; ++j) \
94  v(i, j) = std::func(v(i, j)); \
95  return v; \
96 } \
97 
110 
111 #undef SimTK_ELEMENTWISE_FUNCTION
112 
113 // The abs() function.
114 
115 template <class ELEM>
117  return v.abs();
118 }
119 template <class ELEM>
121  return v.abs();
122 }
123 template <class ELEM>
125  return v.abs();
126 }
127 template <int N, class ELEM>
129  return v.abs();
130 }
131 template <int N, class ELEM>
133  return v.abs();
134 }
135 template <int M, int N, class ELEM>
137  return v.abs();
138 }
139 template <int N, class ELEM>
141  return v.abs();
142 }
143 
144 // The sum() function.
145 
146 template <class ELEM>
147 ELEM sum(const VectorBase<ELEM>& v) {
148  return v.sum();
149 }
150 template <class ELEM>
151 ELEM sum(const RowVectorBase<ELEM>& v) {
152  return v.sum();
153 }
154 template <class ELEM>
156  return v.sum();
157 }
158 template <int N, class ELEM>
159 ELEM sum(const Vec<N, ELEM>& v) {
160  return v.sum();
161 }
162 template <int N, class ELEM>
163 ELEM sum(const Row<N, ELEM>& v) {
164  return v.sum();
165 }
166 template <int M, int N, class ELEM>
168  return v.sum();
169 }
170 template <int N, class ELEM>
172  return v.sum();
173 }
174 
175 // The min() function.
176 
177 template <class ELEM>
178 ELEM min(const VectorBase<ELEM>& v) {
179  const int size = v.size();
181  for (int i = 0; i < size; ++i) {
182  ELEM val = v[i];
183  if (val < min)
184  min = val;
185  }
186  return min;
187 }
188 template <class ELEM>
189 ELEM min(const RowVectorBase<ELEM>& v) {
190  const int size = v.size();
192  for (int i = 0; i < size; ++i) {
193  ELEM val = v[i];
194  if (val < min)
195  min = val;
196  }
197  return min;
198 }
199 template <class ELEM>
201  int cols = v.ncol();
202  RowVectorBase<ELEM> temp(cols);
203  for (int i = 0; i < cols; ++i)
204  temp[i] = min(v(i));
205  return temp;
206 }
207 template <int N, class ELEM>
208 ELEM min(const Vec<N, ELEM>& v) {
210  for (int i = 0; i < N; ++i) {
211  ELEM val = v[i];
212  if (val < min)
213  min = val;
214  }
215  return min;
216 }
217 template <int N, class ELEM>
218 ELEM min(const Row<N, ELEM>& v) {
220  for (int i = 0; i < N; ++i) {
221  ELEM val = v[i];
222  if (val < min)
223  min = val;
224  }
225  return min;
226 }
227 template <int M, int N, class ELEM>
229  Row<N, ELEM> temp;
230  for (int i = 0; i < N; ++i)
231  temp[i] = min(v(i));
232  return temp;
233 }
234 template <int N, class ELEM>
236  Row<N, ELEM> temp(~v.getDiag());
237  for (int i = 1; i < N; ++i)
238  for (int j = 0; j < i; ++j) {
239  ELEM value = v.getEltLower(i, j);
240  if (value < temp[i])
241  temp[i] = value;
242  if (value < temp[j])
243  temp[j] = value;
244  }
245  return temp;
246 }
247 
248 // The max() function.
249 
250 template <class ELEM>
251 ELEM max(const VectorBase<ELEM>& v) {
252  const int size = v.size();
254  for (int i = 0; i < size; ++i) {
255  ELEM val = v[i];
256  if (val > max)
257  max = val;
258  }
259  return max;
260 }
261 template <class ELEM>
262 ELEM max(const RowVectorBase<ELEM>& v) {
263  const int size = v.size();
265  for (int i = 0; i < size; ++i) {
266  ELEM val = v[i];
267  if (val > max)
268  max = val;
269  }
270  return max;
271 }
272 template <class ELEM>
274  int cols = v.ncol();
275  RowVectorBase<ELEM> temp(cols);
276  for (int i = 0; i < cols; ++i)
277  temp[i] = max(v(i));
278  return temp;
279 }
280 template <int N, class ELEM>
281 ELEM max(const Vec<N, ELEM>& v) {
283  for (int i = 0; i < N; ++i) {
284  ELEM val = v[i];
285  if (val > max)
286  max = val;
287  }
288  return max;
289 }
290 template <int N, class ELEM>
291 ELEM max(const Row<N, ELEM>& v) {
293  for (int i = 0; i < N; ++i) {
294  ELEM val = v[i];
295  if (val > max)
296  max = val;
297  }
298  return max;
299 }
300 template <int M, int N, class ELEM>
302  Row<N, ELEM> temp;
303  for (int i = 0; i < N; ++i)
304  temp[i] = max(v(i));
305  return temp;
306 }
307 template <int N, class ELEM>
309  Row<N, ELEM> temp(~v.getDiag());
310  for (int i = 1; i < N; ++i)
311  for (int j = 0; j < i; ++j) {
312  ELEM value = v.getEltLower(i, j);
313  if (value > temp[i])
314  temp[i] = value;
315  if (value > temp[j])
316  temp[j] = value;
317  }
318  return temp;
319 }
320 
321 // The mean() function.
322 
323 template <class ELEM>
324 ELEM mean(const VectorBase<ELEM>& v) {
325  return sum(v)/v.size();
326 }
327 template <class ELEM>
328 ELEM mean(const RowVectorBase<ELEM>& v) {
329  return sum(v)/v.size();
330 }
331 template <class ELEM>
333  return sum(v)/v.nrow();
334 }
335 template <int N, class ELEM>
336 ELEM mean(const Vec<N, ELEM>& v) {
337  return sum(v)/N;
338 }
339 template <int N, class ELEM>
340 ELEM mean(const Row<N, ELEM>& v) {
341  return sum(v)/N;
342 }
343 template <int M, int N, class ELEM>
345  return sum(v)/M;
346 }
347 template <int N, class ELEM>
349  return sum(v)/N;
350 }
351 
352 // The sort() function.
353 
354 template <class ELEM>
356  const int size = v.size();
357  VectorBase<ELEM> temp(v);
358  std::sort(temp.begin(), temp.end());
359  return temp;
360 }
361 template <class ELEM>
363  const int size = v.size();
364  RowVectorBase<ELEM> temp(v);
365  std::sort(temp.begin(), temp.end());
366  return temp;
367 }
368 template <class ELEM>
370  const int rows = v.nrow(), cols = v.ncol();
371  MatrixBase<ELEM> temp(v);
372  for (int i = 0; i < cols; ++i)
373  temp.updCol(i) = sort(temp.col(i));
374  return temp;
375 }
376 template <int N, class ELEM>
377 Vec<N, ELEM> sort(Vec<N, ELEM> v) { // intentional copy of argument
378  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
379  std::sort(pointer, pointer+N);
380  return v;
381 }
382 template <int N, class ELEM>
383 Row<N, ELEM> sort(Row<N, ELEM> v) { // intentional copy of argument
384  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
385  std::sort(pointer, pointer+N);
386  return v;
387 }
388 template <int M, int N, class ELEM>
389 Mat<M, N, ELEM> sort(Mat<M, N, ELEM> v) { // intentional copy of argument
390  for (int i = 0; i < N; ++i)
391  v.col(i) = sort(v.col(i));
392  return v;
393 }
394 template <int N, class ELEM>
396  return sort(Mat<N, N, ELEM>(v));
397 }
398 
399 // The median() function.
400 
401 template <class ELEM, class RandomAccessIterator>
402 ELEM median(RandomAccessIterator start, RandomAccessIterator end) {
403  const ptrdiff_t size = (ptrdiff_t)(end-start);
404  RandomAccessIterator mid = start+(size-1)/2;
405  std::nth_element(start, mid, end);
406  if (size%2 == 0 && mid+1 < end) {
407  // An even number of element. The median is the mean of the two middle elements.
408  // nth_element has given us the first of them and partially sorted the list.
409  // We need to scan through the rest of the list and find the next element in
410  // sorted order.
411 
412  RandomAccessIterator min = mid+1;
413  for (RandomAccessIterator iter = mid+1; iter < end; iter++) {
414  if (*iter < *min)
415  min = iter;
416  }
417  return (*mid+*min)/2;
418  }
419  return *mid;
420 }
421 template <class ELEM>
422 ELEM median(const VectorBase<ELEM>& v) {
423  VectorBase<ELEM> temp(v);
424  return median<ELEM>(temp.begin(), temp.end());
425 }
426 template <class ELEM>
427 ELEM median(const RowVectorBase<ELEM>& v) {
428  RowVectorBase<ELEM> temp(v);
429  return median<ELEM>(temp.begin(), temp.end());
430 }
431 template <class ELEM>
433  int cols = v.ncol(), rows = v.nrow();
434  RowVectorBase<ELEM> temp(cols);
435  VectorBase<ELEM> column(rows);
436  for (int i = 0; i < cols; ++i) {
437  column = v.col(i);
438  temp[i] = median<ELEM>(column);
439  }
440  return temp;
441 }
442 template <int N, class ELEM>
443 ELEM median(Vec<N, ELEM> v) { // intentional copy of argument
444  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
445  return median<ELEM>(pointer, pointer+N);
446 }
447 template <int N, class ELEM>
448 ELEM median(Row<N, ELEM> v) { // intentional copy of argument
449  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
450  return median<ELEM>(pointer, pointer+N);
451 }
452 template <int M, int N, class ELEM>
454  Row<N, ELEM> temp;
455  for (int i = 0; i < N; ++i)
456  temp[i] = median(v(i));
457  return temp;
458 }
459 template <int N, class ELEM>
461  return median(Mat<N, N, ELEM>(v));
462 }
463 
464 } // namespace SimTK
465 
466 #endif // SimTK_SimTKCOMMON_VECTOR_MATH_H_
VectorIterator< ELT, VectorBase< ELT > > begin()
Definition: BigMatrix.h:1473
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:292
const TDiag & getDiag() const
Definition: SymMat.h:802
EStandard sum() const
Definition: Row.h:240
const E & getEltLower(int i, int j) const
Definition: SymMat.h:822
RS is total spacing between rows in memory (default 1)
Definition: SymMat.h:71
This is a dataless rehash of the MatrixBase class to specialize it for RowVectors.
Definition: BigMatrix.h:177
TAbs abs() const
Definition: Row.h:226
ELEM median(RandomAccessIterator start, RandomAccessIterator end)
Definition: VectorMath.h:402
Mat< N, N, ELEM > sort(const SymMat< N, ELEM > &v)
Definition: VectorMath.h:395
VectorView_< ELT > updCol(int j)
Definition: BigMatrix.h:2341
int size() const
Definition: BigMatrix.h:1411
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
SimTK_ELEMENTWISE_FUNCTION(exp) SimTK_ELEMENTWISE_FUNCTION(log) SimTK_ELEMENTWISE_FUNCTION(sqrt) SimTK_ELEMENTWISE_FUNCTION(sin) SimTK_ELEMENTWISE_FUNCTION(cos) SimTK_ELEMENTWISE_FUNCTION(tan) SimTK_ELEMENTWISE_FUNCTION(asin) SimTK_ELEMENTWISE_FUNCTION(acos) SimTK_ELEMENTWISE_FUNCTION(atan) SimTK_ELEMENTWISE_FUNCTION(sinh) SimTK_ELEMENTWISE_FUNCTION(cosh) SimTK_ELEMENTWISE_FUNCTION(tanh) template< class ELEM > VectorBase< typename CNT< ELEM >
Definition: VectorMath.h:98
void abs(TAbs &mabs) const
abs() is elementwise absolute value; that is, the return value has the same dimension as this Matrix ...
Definition: BigMatrix.h:864
ELEM mean(const VectorBase< ELEM > &v)
Definition: VectorMath.h:324
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
EStandard sum() const
Sum just adds up all the elements into a single return element that is the same type as this Vec's el...
Definition: Vec.h:311
This is a fixed length column vector designed for no-overhead inline computation. ...
Definition: Vec.h:131
int nrow() const
Return the number of rows m in the logical shape of this matrix.
Definition: BigMatrix.h:288
ELT sum() const
Definition: BigMatrix.h:1472
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: SymMat.h:842
RowVector_< ELT > sum() const
Alternate name for colSum(); behaves like the Matlab function sum().
Definition: BigMatrix.h:925
VectorBase< ELEM > sort(const VectorBase< ELEM > &v)
Definition: VectorMath.h:355
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
VectorIterator< ELT, RowVectorBase< ELT > > begin()
Definition: BigMatrix.h:1751
VectorIterator< ELT, RowVectorBase< ELT > > end()
Definition: BigMatrix.h:1754
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimensions as this Mat but with ea...
Definition: Mat.h:180
int size() const
Definition: BigMatrix.h:1690
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
TAbs abs() const
Definition: BigMatrix.h:1421
Generic Row.
Definition: Row.h:118
const TCol & col(int j) const
Definition: Mat.h:729
TAbs abs() const
Definition: SymMat.h:179
This is a dataless rehash of the MatrixBase class to specialize it for Vectors.
Definition: BigMatrix.h:176
VectorIterator< ELT, VectorBase< ELT > > end()
Definition: BigMatrix.h:1476
ELT sum() const
Definition: BigMatrix.h:1750
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:51
Variable-size 2d matrix of Composite Numerical Type (ELT) elements.
Definition: BigMatrix.h:175
This is the header which should be included in user programs that would like to make use of all the S...
int ncol() const
Return the number of columns n in the logical shape of this matrix.
Definition: BigMatrix.h:290
Includes internal headers providing declarations for the basic SimTK Core classes.
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: Mat.h:1159
Definition: negator.h:64
TAbs abs() const
Definition: BigMatrix.h:1700
VectorView_< ELT > col(int j) const
Definition: BigMatrix.h:2332