Simbody 3.7
Loading...
Searching...
No Matches
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>
123template <class ELEM>
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>
139template <int N, class ELEM>
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>
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...
#define SimTK_ELEMENTWISE_FUNCTION(func)
Definition VectorMath.h:45
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
ELEM sum(const VectorBase< ELEM > &v)
Definition VectorMath.h:147
ELEM mean(const VectorBase< ELEM > &v)
Definition VectorMath.h:324
ELEM median(RandomAccessIterator start, RandomAccessIterator end)
Definition VectorMath.h:400