Simbody  3.5
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_
VectorIterator< ELT, VectorBase< ELT > > begin()
Definition: VectorBase.h:458
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:345
const TDiag & getDiag() const
Definition: SymMat.h:818
EStandard sum() const
Definition: Row.h:254
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:608
This is a dataless rehash of the MatrixBase class to specialize it for RowVectors.
Definition: BigMatrix.h:165
TAbs abs() const
Definition: Row.h:240
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
Mat< N, N, ELEM > sort(const SymMat< N, ELEM > &v)
Definition: VectorMath.h:393
VectorView_< ELT > updCol(int j)
Definition: BigMatrix.h:261
int size() const
Definition: VectorBase.h:396
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: MatrixBase.h:688
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&#39;s el...
Definition: Vec.h:364
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
int nrow() const
Return the number of rows m in the logical shape of this matrix.
Definition: MatrixBase.h:137
ELT sum() const
Definition: VectorBase.h:457
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: SymMat.h:858
RowVector_< ELT > sum() const
Alternate name for colSum(); behaves like the Matlab function sum().
Definition: MatrixBase.h:749
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: RowVectorBase.h:298
VectorIterator< ELT, RowVectorBase< ELT > > end()
Definition: RowVectorBase.h:301
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimensions as this Mat but with ea...
Definition: Mat.h:226
int size() const
Definition: RowVectorBase.h:237
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
TAbs abs() const
Definition: VectorBase.h:406
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
const TCol & col(int j) const
Definition: Mat.h:775
TAbs abs() const
Definition: SymMat.h:195
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
ELT sum() const
Definition: RowVectorBase.h:297
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
This is the common base class for Simbody&#39;s Vector_ and Matrix_ classes for handling large...
Definition: BigMatrix.h:163
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: MatrixBase.h:139
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:1205
Definition: negator.h:64
TAbs abs() const
Definition: RowVectorBase.h:247
VectorView_< ELT > col(int j) const
Definition: BigMatrix.h:252