Simbody  3.4
 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 
98 SimTK_ELEMENTWISE_FUNCTION(exp)
99 SimTK_ELEMENTWISE_FUNCTION(log)
100 SimTK_ELEMENTWISE_FUNCTION(sqrt)
101 SimTK_ELEMENTWISE_FUNCTION(sin)
102 SimTK_ELEMENTWISE_FUNCTION(cos)
103 SimTK_ELEMENTWISE_FUNCTION(tan)
104 SimTK_ELEMENTWISE_FUNCTION(asin)
105 SimTK_ELEMENTWISE_FUNCTION(acos)
106 SimTK_ELEMENTWISE_FUNCTION(atan)
107 SimTK_ELEMENTWISE_FUNCTION(sinh)
108 SimTK_ELEMENTWISE_FUNCTION(cosh)
109 SimTK_ELEMENTWISE_FUNCTION(tanh)
110 
111 #undef SimTK_ELEMENTWISE_FUNCTION
112 
113 // The abs() function.
114 
115 template <class ELEM>
116 VectorBase<typename CNT<ELEM>::TAbs> abs(const VectorBase<ELEM>& v) {
117  return v.abs();
118 }
119 template <class ELEM>
120 RowVectorBase<typename CNT<ELEM>::TAbs> abs(const RowVectorBase<ELEM>& v) {
121  return v.abs();
122 }
123 template <class ELEM>
124 MatrixBase<typename CNT<ELEM>::TAbs> abs(const MatrixBase<ELEM>& v) {
125  return v.abs();
126 }
127 template <int N, class ELEM>
128 Vec<N, typename CNT<ELEM>::TAbs> abs(const Vec<N, ELEM>& v) {
129  return v.abs();
130 }
131 template <int N, class ELEM>
132 Row<N, typename CNT<ELEM>::TAbs> abs(const Row<N, ELEM>& v) {
133  return v.abs();
134 }
135 template <int M, int N, class ELEM>
136 Mat<M, N, typename CNT<ELEM>::TAbs> abs(const Mat<M, N, ELEM>& v) {
137  return v.abs();
138 }
139 template <int N, class ELEM>
140 SymMat<N, typename CNT<ELEM>::TAbs> abs(const SymMat<N, ELEM>& v) {
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>
155 RowVectorBase<ELEM> sum(const MatrixBase<ELEM>& v) {
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>
167 Row<N, ELEM> sum(const Mat<M, N, ELEM>& v) {
168  return v.sum();
169 }
170 template <int N, class ELEM>
171 Row<N, ELEM> sum(const SymMat<N, ELEM>& v) {
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();
180  ELEM min = NTraits<ELEM>::getMostPositive();
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();
191  ELEM min = NTraits<ELEM>::getMostPositive();
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>
200 RowVectorBase<ELEM> min(const MatrixBase<ELEM>& v) {
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) {
209  ELEM min = NTraits<ELEM>::getMostPositive();
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) {
219  ELEM min = NTraits<ELEM>::getMostPositive();
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>
228 Row<N, ELEM> min(const Mat<M, N, ELEM>& v) {
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>
235 Row<N, ELEM> min(const SymMat<N, ELEM>& v) {
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();
253  ELEM max = NTraits<ELEM>::getMostNegative();
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();
264  ELEM max = NTraits<ELEM>::getMostNegative();
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>
273 RowVectorBase<ELEM> max(const MatrixBase<ELEM>& v) {
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) {
282  ELEM max = NTraits<ELEM>::getMostNegative();
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) {
292  ELEM max = NTraits<ELEM>::getMostNegative();
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>
301 Row<N, ELEM> max(const Mat<M, N, ELEM>& v) {
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>
308 Row<N, ELEM> max(const SymMat<N, ELEM>& v) {
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>
332 RowVectorBase<ELEM> mean(const MatrixBase<ELEM>& v) {
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>
344 Row<N, ELEM> mean(const Mat<M, N, ELEM>& v) {
345  return sum(v)/M;
346 }
347 template <int N, class ELEM>
348 Row<N, ELEM> mean(const SymMat<N, ELEM>& v) {
349  return sum(v)/N;
350 }
351 
352 // The sort() function.
353 
354 template <class ELEM>
355 VectorBase<ELEM> sort(const VectorBase<ELEM>& v) {
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>
362 RowVectorBase<ELEM> sort(const RowVectorBase<ELEM>& v) {
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>
369 MatrixBase<ELEM> sort(const MatrixBase<ELEM>& v) {
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>
395 Mat<N, N, ELEM> sort(const SymMat<N, ELEM>& v) {
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>
432 RowVectorBase<ELEM> median(const MatrixBase<ELEM>& v) {
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>
453 Row<N, ELEM> median(const Mat<M, N, ELEM>& v) {
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>
460 Row<N, ELEM> median(const SymMat<N, ELEM>& v) {
461  return median(Mat<N, N, ELEM>(v));
462 }
463 
464 } // namespace SimTK
465 
466 #endif // SimTK_SimTKCOMMON_VECTOR_MATH_H_