Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules 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_
VectorView_< ELT > col(int j) const
Definition: BigMatrix.h:252
RowVector_< ELT > sum() const
Alternate name for colSum(); behaves like the Matlab function sum().
Definition: MatrixBase.h:774
const TDiag & getDiag() const
Definition: SymMat.h:818
TAbs abs() const
Definition: VectorBase.h:406
ELEM median(RandomAccessIterator start, RandomAccessIterator end)
Definition: VectorMath.h:402
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
TAbs abs() const
Definition: SymMat.h:195
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
VectorIterator< ELT, RowVectorBase< ELT > > end()
Definition: RowVectorBase.h:301
TAbs abs() const
Definition: Row.h:240
int size() const
Definition: VectorBase.h:396
Mat< N, N, ELEM > sort(const SymMat< N, ELEM > &v)
Definition: VectorMath.h:395
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
This is a dataless rehash of the MatrixBase class to specialize it for Vectors.
Definition: BigMatrix.h:164
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: Mat.h:1205
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
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:364
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:580
EStandard sum() const
Definition: Row.h:254
VectorIterator< ELT, VectorBase< ELT > > end()
Definition: VectorBase.h:461
This is the common base class for Simbody's Vector_ and Matrix_ classes for handling large...
Definition: BigMatrix.h:163
VectorView_< ELT > updCol(int j)
Definition: BigMatrix.h:261
ELEM mean(const VectorBase< ELEM > &v)
Definition: VectorMath.h:324
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
VectorIterator< ELT, VectorBase< ELT > > begin()
Definition: VectorBase.h:458
VectorBase< ELEM > sort(const VectorBase< ELEM > &v)
Definition: VectorMath.h:355
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name...
Definition: SymMat.h:858
VectorIterator< ELT, RowVectorBase< ELT > > begin()
Definition: RowVectorBase.h:298
int nrow() const
Return the number of rows m in the logical shape of this matrix.
Definition: MatrixBase.h:137
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:578
int ncol() const
Return the number of columns n in the logical shape of this matrix.
Definition: MatrixBase.h:139
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:713
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
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
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:579
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:577
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
This is a dataless rehash of the MatrixBase class to specialize it for RowVectors.
Definition: BigMatrix.h:165
TAbs abs() const
Definition: RowVectorBase.h:247
ELT sum() const
Definition: RowVectorBase.h:297
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.
int size() const
Definition: RowVectorBase.h:237
Definition: negator.h:64
ELT sum() const
Definition: VectorBase.h:457
const TCol & col(int j) const
Definition: Mat.h:775