Simbody 3.7
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"
29
30#include <cmath> // for std:sin, sqrt, etc.
31#include <algorithm> // for std:sort, nth_element, etc.
32
39namespace 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) \
46template <class ELEM> \
47VectorBase<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} \
54template <class ELEM> \
55RowVectorBase<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} \
62template <class ELEM> \
63MatrixBase<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} \
71template <int N, class ELEM> \
72Vec<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} \
77template <int N, class ELEM> \
78Row<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} \
83template <int M, int N, class ELEM> \
84Mat<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} \
90template <int N, class ELEM> \
91SymMat<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
115template <class ELEM>
117 return v.abs();
118}
119template <class ELEM>
121 return v.abs();
122}
123template <class ELEM>
125 return v.abs();
126}
127template <int N, class ELEM>
129 return v.abs();
130}
131template <int N, class ELEM>
133 return v.abs();
134}
135template <int M, int N, class ELEM>
137 return v.abs();
138}
139template <int N, class ELEM>
141 return v.abs();
142}
143
144// The sum() function.
145
146template <class ELEM>
147ELEM sum(const VectorBase<ELEM>& v) {
148 return v.sum();
149}
150template <class ELEM>
151ELEM sum(const RowVectorBase<ELEM>& v) {
152 return v.sum();
153}
154template <class ELEM>
156 return v.sum();
157}
158template <int N, class ELEM>
159ELEM sum(const Vec<N, ELEM>& v) {
160 return v.sum();
161}
162template <int N, class ELEM>
163ELEM sum(const Row<N, ELEM>& v) {
164 return v.sum();
165}
166template <int M, int N, class ELEM>
168 return v.sum();
169}
170template <int N, class ELEM>
172 return v.sum();
173}
174
175// The min() function.
176
177template <class ELEM>
178ELEM 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}
188template <class ELEM>
189ELEM 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}
199template <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}
207template <int N, class ELEM>
208ELEM 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}
217template <int N, class ELEM>
218ELEM 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}
227template <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}
234template <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
250template <class ELEM>
251ELEM 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}
261template <class ELEM>
262ELEM 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}
272template <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}
280template <int N, class ELEM>
281ELEM 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}
290template <int N, class ELEM>
291ELEM 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}
300template <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}
307template <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
323template <class ELEM>
324ELEM mean(const VectorBase<ELEM>& v) {
325 return sum(v)/v.size();
326}
327template <class ELEM>
328ELEM mean(const RowVectorBase<ELEM>& v) {
329 return sum(v)/v.size();
330}
331template <class ELEM>
333 return sum(v)/v.nrow();
334}
335template <int N, class ELEM>
336ELEM mean(const Vec<N, ELEM>& v) {
337 return sum(v)/N;
338}
339template <int N, class ELEM>
340ELEM mean(const Row<N, ELEM>& v) {
341 return sum(v)/N;
342}
343template <int M, int N, class ELEM>
345 return sum(v)/M;
346}
347template <int N, class ELEM>
349 return sum(v)/N;
350}
351
352// The sort() function.
353
354template <class ELEM>
356 VectorBase<ELEM> temp(v);
357 std::sort(temp.begin(), temp.end());
358 return temp;
359}
360template <class ELEM>
362 RowVectorBase<ELEM> temp(v);
363 std::sort(temp.begin(), temp.end());
364 return temp;
365}
366template <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}
374template <int N, class ELEM>
375Vec<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}
380template <int N, class ELEM>
381Row<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}
386template <int M, int N, class ELEM>
387Mat<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}
392template <int N, class ELEM>
394 return sort(Mat<N, N, ELEM>(v));
395}
396
397// The median() function.
398
399template <class ELEM, class RandomAccessIterator>
400ELEM 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}
419template <class ELEM>
420ELEM median(const VectorBase<ELEM>& v) {
421 VectorBase<ELEM> temp(v);
422 return median<ELEM>(temp.begin(), temp.end());
423}
424template <class ELEM>
426 RowVectorBase<ELEM> temp(v);
427 return median<ELEM>(temp.begin(), temp.end());
428}
429template <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}
440template <int N, class ELEM>
441ELEM median(Vec<N, ELEM> v) { // intentional copy of argument
442 ELEM* pointer = reinterpret_cast<ELEM*>(&v);
443 return median<ELEM>(pointer, pointer+N);
444}
445template <int N, class ELEM>
446ELEM median(Row<N, ELEM> v) { // intentional copy of argument
447 ELEM* pointer = reinterpret_cast<ELEM*>(&v);
448 return median<ELEM>(pointer, pointer+N);
449}
450template <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}
457template <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_
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.
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name.
Definition: Mat.h:1207
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
const TCol & col(int j) const
Definition: Mat.h:777
This is the common base class for Simbody's Vector_ and Matrix_ classes for handling large,...
Definition: MatrixBase.h:68
RowVector_< ELT > sum() const
Alternate name for colSum(); behaves like the Matlab function sum().
Definition: MatrixBase.h:748
VectorView_< ELT > updCol(int j)
Definition: BigMatrix.h:261
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
int nrow() const
Return the number of rows m in the logical shape of this matrix.
Definition: MatrixBase.h:136
int ncol() const
Return the number of columns n in the logical shape of this matrix.
Definition: MatrixBase.h:138
VectorView_< ELT > col(int j) const
Definition: BigMatrix.h:252
Definition: NTraits.h:436
This is a dataless rehash of the MatrixBase class to specialize it for RowVectors.
Definition: RowVectorBase.h:42
VectorIterator< ELT, RowVectorBase< ELT > > begin()
Definition: RowVectorBase.h:298
TAbs abs() const
Definition: RowVectorBase.h:247
ELT sum() const
Definition: RowVectorBase.h:297
int size() const
Definition: RowVectorBase.h:237
VectorIterator< ELT, RowVectorBase< ELT > > end()
Definition: RowVectorBase.h:301
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: Row.h:132
EStandard sum() const
Definition: Row.h:254
TAbs abs() const
Definition: Row.h:240
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SymMat.h:87
const TDiag & getDiag() const
Definition: SymMat.h:818
TAbs abs() const
Definition: SymMat.h:195
TRow sum() const
This is an alternate name for colSum(); behaves like the Matlab function of the same name.
Definition: SymMat.h:858
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
This is a fixed-length column vector designed for no-overhead inline computation.
Definition: Vec.h:184
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:366
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 Vectors.
Definition: VectorBase.h:42
VectorIterator< ELT, VectorBase< ELT > > begin()
Definition: VectorBase.h:458
int size() const
Definition: VectorBase.h:396
ELT sum() const
Definition: VectorBase.h:457
TAbs abs() const
Definition: VectorBase.h:406
VectorIterator< ELT, VectorBase< ELT > > end()
Definition: VectorBase.h:461
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
VectorBase< ELEM > sort(const VectorBase< ELEM > &v)
Definition: VectorMath.h:355
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
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
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
ELEM mean(const VectorBase< ELEM > &v)
Definition: VectorMath.h:324
Mat< N, N, ELEM > sort(const SymMat< N, ELEM > &v)
Definition: VectorMath.h:393
ELEM median(RandomAccessIterator start, RandomAccessIterator end)
Definition: VectorMath.h:400