Simbody  3.6
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  VectorBase<ELEM> temp(v);
357  std::sort(temp.begin(), temp.end());
358  return temp;
359 }
360 template <class ELEM>
362  RowVectorBase<ELEM> temp(v);
363  std::sort(temp.begin(), temp.end());
364  return temp;
365 }
366 template <class ELEM>
368  const int cols = v.ncol();
369  MatrixBase<ELEM> temp(v);
370  for (int i = 0; i < cols; ++i)
371  temp.updCol(i) = sort(temp.col(i));
372  return temp;
373 }
374 template <int N, class ELEM>
375 Vec<N, ELEM> sort(Vec<N, ELEM> v) { // intentional copy of argument
376  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
377  std::sort(pointer, pointer+N);
378  return v;
379 }
380 template <int N, class ELEM>
381 Row<N, ELEM> sort(Row<N, ELEM> v) { // intentional copy of argument
382  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
383  std::sort(pointer, pointer+N);
384  return v;
385 }
386 template <int M, int N, class ELEM>
387 Mat<M, N, ELEM> sort(Mat<M, N, ELEM> v) { // intentional copy of argument
388  for (int i = 0; i < N; ++i)
389  v.col(i) = sort(v.col(i));
390  return v;
391 }
392 template <int N, class ELEM>
394  return sort(Mat<N, N, ELEM>(v));
395 }
396 
397 // The median() function.
398 
399 template <class ELEM, class RandomAccessIterator>
400 ELEM median(RandomAccessIterator start, RandomAccessIterator end) {
401  const ptrdiff_t size = (ptrdiff_t)(end-start);
402  RandomAccessIterator mid = start+(size-1)/2;
403  std::nth_element(start, mid, end);
404  if (size%2 == 0 && mid+1 < end) {
405  // An even number of element. The median is the mean of the two middle elements.
406  // nth_element has given us the first of them and partially sorted the list.
407  // We need to scan through the rest of the list and find the next element in
408  // sorted order.
409 
410  RandomAccessIterator min = mid+1;
411  for (RandomAccessIterator iter = mid+1; iter < end; iter++) {
412  if (*iter < *min)
413  min = iter;
414  }
415  return (*mid+*min)/2;
416  }
417  return *mid;
418 }
419 template <class ELEM>
420 ELEM median(const VectorBase<ELEM>& v) {
421  VectorBase<ELEM> temp(v);
422  return median<ELEM>(temp.begin(), temp.end());
423 }
424 template <class ELEM>
425 ELEM median(const RowVectorBase<ELEM>& v) {
426  RowVectorBase<ELEM> temp(v);
427  return median<ELEM>(temp.begin(), temp.end());
428 }
429 template <class ELEM>
431  int cols = v.ncol(), rows = v.nrow();
432  RowVectorBase<ELEM> temp(cols);
433  VectorBase<ELEM> column(rows);
434  for (int i = 0; i < cols; ++i) {
435  column = v.col(i);
436  temp[i] = median<ELEM>(column);
437  }
438  return temp;
439 }
440 template <int N, class ELEM>
441 ELEM median(Vec<N, ELEM> v) { // intentional copy of argument
442  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
443  return median<ELEM>(pointer, pointer+N);
444 }
445 template <int N, class ELEM>
446 ELEM median(Row<N, ELEM> v) { // intentional copy of argument
447  ELEM* pointer = reinterpret_cast<ELEM*>(&v);
448  return median<ELEM>(pointer, pointer+N);
449 }
450 template <int M, int N, class ELEM>
452  Row<N, ELEM> temp;
453  for (int i = 0; i < N; ++i)
454  temp[i] = median(v(i));
455  return temp;
456 }
457 template <int N, class ELEM>
459  return median(Mat<N, N, ELEM>(v));
460 }
461 
462 } // namespace SimTK
463 
464 #endif // SimTK_SimTKCOMMON_VECTOR_MATH_H_
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimensions as this Mat but with ea...
Definition: Mat.h:228
VectorIterator< ELT, VectorBase< ELT > > begin()
Definition: VectorBase.h:458
TAbs abs() const
Definition: Row.h:240
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: SymMat.h:858
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:621
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:347
This is a dataless rehash of the MatrixBase class to specialize it for RowVectors.
Definition: BigMatrix.h:165
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: Mat.h:1207
ELEM median(RandomAccessIterator start, RandomAccessIterator end)
Definition: VectorMath.h:400
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
ELT sum() const
Definition: VectorBase.h:457
Mat< N, N, ELEM > sort(const SymMat< N, ELEM > &v)
Definition: VectorMath.h:393
VectorView_< ELT > updCol(int j)
Definition: BigMatrix.h:261
const TCol & col(int j) const
Definition: Mat.h:777
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
EStandard sum() const
Sum just adds up all the elements into a single return element that is the same type as this Vec&#39;s el...
Definition: Vec.h:366
VectorView_< ELT > col(int j) const
Definition: BigMatrix.h:252
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
const TDiag & getDiag() const
Definition: SymMat.h:818
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
ELEM mean(const VectorBase< ELEM > &v)
Definition: VectorMath.h:324
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:618
TAbs abs() const
Definition: VectorBase.h:406
VectorBase< ELEM > sort(const VectorBase< ELEM > &v)
Definition: VectorMath.h:355
ELT sum() const
Definition: RowVectorBase.h:297
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
int ncol() const
Return the number of columns n in the logical shape of this matrix.
Definition: MatrixBase.h:138
VectorIterator< ELT, RowVectorBase< ELT > > begin()
Definition: RowVectorBase.h:298
VectorIterator< ELT, RowVectorBase< ELT > > end()
Definition: RowVectorBase.h:301
TAbs abs() const
Definition: RowVectorBase.h:247
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
EStandard sum() const
Definition: Row.h:254
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:619
int size() const
Definition: RowVectorBase.h:237
This is a dataless rehash of the MatrixBase class to specialize it for Vectors.
Definition: BigMatrix.h:164
VectorIterator< ELT, VectorBase< ELT > > end()
Definition: VectorBase.h:461
int size() const
Definition: VectorBase.h:396
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
This is the common base class for Simbody&#39;s Vector_ and Matrix_ classes for handling large...
Definition: BigMatrix.h:163
void abs(TAbs &mabs) const
abs() is elementwise absolute value; that is, the return value has the same dimension as this Matrix ...
Definition: MatrixBase.h:687
This is the header which should be included in user programs that would like to make use of all the S...
Includes internal headers providing declarations for the basic SimTK Core classes.
RowVector_< ELT > sum() const
Alternate name for colSum(); behaves like the Matlab function sum().
Definition: MatrixBase.h:748
Definition: negator.h:64
int nrow() const
Return the number of rows m in the logical shape of this matrix.
Definition: MatrixBase.h:136
TAbs abs() const
Definition: SymMat.h:195