Simbody 3.7
SmallMatrixMixed.h
Go to the documentation of this file.
1#ifndef SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
2#define SimTK_SIMMATRIX_SMALLMATRIX_MIXED_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) 2005-12 Stanford University and the Authors. *
13 * Authors: Michael Sherman *
14 * Contributors: *
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
33namespace SimTK {
34
35 // COMPARISON
36
37// m==s
38template <int M, class EL, int CSL, int RSL, class ER, int RSR> inline
40 for (int i=0; i<M; ++i) {
41 if (l(i,i) != r.getDiag()[i]) return false;
42 for (int j=0; j<i; ++j)
43 if (l(i,j) != r.getEltLower(i,j)) return false;
44 for (int j=i+1; j<M; ++j)
45 if (l(i,j) != r.getEltUpper(i,j)) return false;
46 }
47
48 return true;
49}
50// m!=s
51template <int M, class EL, int CSL, int RSL, class ER, int RSR> inline
53 return !(l==r);
54}
55
56// s==m
57template <int M, class EL, int RSL, class ER, int CSR, int RSR> inline
59 return r==l;
60}
61// s!=m
62template <int M, class EL, int RSL, class ER, int CSR, int RSR> inline
64 return !(r==l);
65}
66
67 // DOT PRODUCT
68
69// Dot product and corresponding infix operator*(). Note that
70// the '*' operator is just a matrix multiply so is strictly
71// row*column to produce a scalar (1x1) result.
72//
73// In keeping with Matlab precedent, however, the explicit dot()
74// function is defined on two column vectors
75// v and w such that dot(v,w)= ~v * w. That means we use the
76// Hermitian transpose of the elements of v, and the elements of
77// w unchanged. If v and/or w are rows, we first convert them
78// to vectors via *positional* transpose. So no matter what shape
79// these have on the way in it is always the Hermitian transpose
80// (or complex conjugate for scalars) of the first item times
81// the unchanged elements of the second item.
82
83
84template <int M, class E1, int S1, class E2, int S2> inline
85typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
86dot(const Vec<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
87 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul sum(dot(reinterpret_cast<const Vec<M-1,E1,S1>&>(r), reinterpret_cast<const Vec<M-1,E2,S2>&>(v)) + CNT<E1>::transpose(r[M-1])*v[M-1]);
88 return sum;
89}
90template <class E1, int S1, class E2, int S2> inline
91typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
92dot(const Vec<1,E1,S1>& r, const Vec<1,E2,S2>& v) {
93 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul sum(CNT<E1>::transpose(r[0])*v[0]);
94 return sum;
95}
96
97// dot product (row * conforming vec)
98template <int N, class E1, int S1, class E2, int S2> inline
99typename CNT<E1>::template Result<E2>::Mul
101 typename CNT<E1>::template Result<E2>::Mul sum(reinterpret_cast<const Row<N-1,E1,S1>&>(r)*reinterpret_cast<const Vec<N-1,E2,S2>&>(v) + r[N-1]*v[N-1]);
102 return sum;
103}
104template <class E1, int S1, class E2, int S2> inline
105typename CNT<E1>::template Result<E2>::Mul
107 typename CNT<E1>::template Result<E2>::Mul sum(r[0]*v[0]);
108 return sum;
109}
110
111// Alternate acceptable forms for dot().
112template <int N, class E1, int S1, class E2, int S2> inline
113typename CNT<E1>::template Result<E2>::Mul
114dot(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
115 return dot(r.positionalTranspose(),v);
116}
117template <int M, class E1, int S1, class E2, int S2> inline
118typename CNT<E1>::template Result<E2>::Mul
119dot(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
120 return dot(v,r.positionalTranspose());
121}
122template <int N, class E1, int S1, class E2, int S2> inline
123typename CNT<E1>::template Result<E2>::Mul
124dot(const Row<N,E1,S1>& r, const Row<N,E2,S2>& s) {
126}
127
128 // OUTER PRODUCT
129
130// Outer product and corresponding infix operator*(). Note that
131// the '*' operator is just a matrix multiply so is strictly
132// column_mX1 * row_1Xm to produce an mXm matrix result.
133//
134// Although Matlab doesn't provide outer(), to be consistent
135// we'll treat it as discussed for dot() above. That is, outer
136// is defined on two column vectors
137// v and w such that outer(v,w)= v * ~w. That means we use the
138// elements of v unchanged but use the Hermitian transpose of
139// the elements of w. If v and/or w are rows, we first convert them
140// to vectors via *positional* transpose. So no matter what shape
141// these have on the way in it is always the unchanged elements of
142// the first item times the Hermitian transpose (or complex
143// conjugate for scalars) of the elements of the second item.
144
145template <int M, class E1, int S1, class E2, int S2> inline
146Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul>
147outer(const Vec<M,E1,S1>& v, const Vec<M,E2,S2>& w) {
148 Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul> m;
149 for (int i=0; i<M; ++i)
150 m[i] = v[i] * ~w;
151 return m;
152}
153
154// This is the general conforming case of Vec*Row (outer product)
155template <int M, class E1, int S1, class E2, int S2> inline
156typename Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::Mul
158 return Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::MulOp::perform(v,r);
159}
160
161
162// Alternate acceptable forms for outer().
163template <int M, class E1, int S1, class E2, int S2> inline
164Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
165outer(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
166 return outer(v,r.positionalTranspose());
167}
168template <int M, class E1, int S1, class E2, int S2> inline
169Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
170outer(const Row<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
171 return outer(r.positionalTranspose(),v);
172}
173template <int M, class E1, int S1, class E2, int S2> inline
174Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
175outer(const Row<M,E1,S1>& r, const Row<M,E2,S2>& s) {
177}
178
179 // MAT*VEC, ROW*MAT (conforming)
180
181// vec = mat * vec (conforming)
182template <int M, int N, class ME, int CS, int RS, class E, int S> inline
183typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul
185 typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul result;
186 for (int i=0; i<M; ++i)
187 result[i] = m[i]*v;
188 return result;
189}
190
191// row = row * mat (conforming)
192template <int M, class E, int S, int N, class ME, int CS, int RS> inline
193typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul
195 typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul result;
196 for (int i=0; i<N; ++i)
197 result[i] = r*m(i);
198 return result;
199}
200
201 // SYMMAT*VEC, ROW*SYMMAT (conforming)
202
203// vec = sym * vec (conforming)
204template <int N, class ME, int RS, class E, int S> inline
205typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul
207 typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul result;
208 for (int i=0; i<N; ++i) {
209 result[i] = m.getDiag()[i]*v[i];
210 for (int j=0; j<i; ++j)
211 result[i] += m.getEltLower(i,j)*v[j];
212 for (int j=i+1; j<N; ++j)
213 result[i] += m.getEltUpper(i,j)*v[j];
214 }
215 return result;
216}
217
218// Unroll loops for small cases.
219
220// 1 flop.
221template <class ME, int RS, class E, int S> inline
222typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul
224 typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul result;
225 result[0] = m.getDiag()[0]*v[0];
226 return result;
227}
228
229// 6 flops.
230template <class ME, int RS, class E, int S> inline
231typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul
233 typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul result;
234 result[0] = m.getDiag()[0] *v[0] + m.getEltUpper(0,1)*v[1];
235 result[1] = m.getEltLower(1,0)*v[0] + m.getDiag()[1] *v[1];
236 return result;
237}
238
239// 15 flops.
240template <class ME, int RS, class E, int S> inline
241typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul
243 typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul result;
244 result[0] = m.getDiag()[0] *v[0] + m.getEltUpper(0,1)*v[1] + m.getEltUpper(0,2)*v[2];
245 result[1] = m.getEltLower(1,0)*v[0] + m.getDiag()[1] *v[1] + m.getEltUpper(1,2)*v[2];
246 result[2] = m.getEltLower(2,0)*v[0] + m.getEltLower(2,1)*v[1] + m.getDiag()[2] *v[2];
247 return result;
248}
249
250// row = row * sym (conforming)
251template <int M, class E, int S, class ME, int RS> inline
252typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul
254 typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul result;
255 for (int j=0; j<M; ++j) {
256 result[j] = r[j]*m.getDiag()[j];
257 for (int i=0; i<j; ++i)
258 result[j] += r[i]*m.getEltUpper(i,j);
259 for (int i=j+1; i<M; ++i)
260 result[j] += r[i]*m.getEltLower(i,j);
261 }
262 return result;
263}
264
265// Unroll loops for small cases.
266
267// 1 flop.
268template <class E, int S, class ME, int RS> inline
269typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul
271 typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul result;
272 result[0] = r[0]*m.getDiag()[0];
273 return result;
274}
275
276// 6 flops.
277template <class E, int S, class ME, int RS> inline
278typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul
280 typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul result;
281 result[0] = r[0]*m.getDiag()[0] + r[1]*m.getEltLower(1,0);
282 result[1] = r[0]*m.getEltUpper(0,1) + r[1]*m.getDiag()[1];
283 return result;
284}
285
286// 15 flops.
287template <class E, int S, class ME, int RS> inline
288typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul
290 typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul result;
291 result[0] = r[0]*m.getDiag()[0] + r[1]*m.getEltLower(1,0) + r[2]*m.getEltLower(2,0);
292 result[1] = r[0]*m.getEltUpper(0,1) + r[1]*m.getDiag()[1] + r[2]*m.getEltLower(2,1);
293 result[2] = r[0]*m.getEltUpper(0,2) + r[1]*m.getEltUpper(1,2) + r[2]*m.getDiag()[2];
294 return result;
295}
296
297 // NONCONFORMING MULTIPLY
298
299 // Nonconforming: Vec on left: v*r v*m v*sym v*v
300 // Result has the shape of the "most composite" (deepest) argument.
301
302// elementwise multiply (vec * nonconforming row)
303template <int M, class E1, int S1, int N, class E2, int S2> inline
304typename Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulNon
306 return Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulOpNonConforming::perform(v,r);
307}
308// elementwise multiply (vec * nonconforming mat)
309template <int M, class E1, int S1, int MM, int NN, class E2, int CS2, int RS2> inline
310typename Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >::MulNon
312 return Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >
313 ::MulOpNonConforming::perform(v,m);
314}
315// elementwise multiply (vec * nonconforming symmat)
316template <int M, class E1, int S1, int MM, class E2, int RS2> inline
317typename Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >::MulNon
319 return Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >
320 ::MulOpNonConforming::perform(v,m);
321}
322// elementwise multiply (vec * nonconforming vec)
323template <int M, class E1, int S1, int MM, class E2, int S2> inline
324typename Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >::MulNon
325operator*(const Vec<M,E1,S1>& v1, const Vec<MM,E2,S2>& v2) {
326 return Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >
327 ::MulOpNonConforming::perform(v1,v2);
328}
329
330 // Nonconforming: Row on left -- r*v r*r r*m r*sym
331
332
333// (row or mat) = row * mat (nonconforming)
334template <int M, class E, int S, int MM, int NN, class ME, int CS, int RS> inline
335typename Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >::MulNon
337 return Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >
338 ::MulOpNonConforming::perform(r,m);
339}
340// (row or vec) = row * vec (nonconforming)
341template <int N, class E1, int S1, int M, class E2, int S2> inline
342typename Row<N,E1,S1>::template Result<Vec<M,E2,S2> >::MulNon
344 return Row<N,E1,S1>::template Result<Vec<M,E2,S2> >
345 ::MulOpNonConforming::perform(r,v);
346}
347// (row1 or row2) = row1 * row2(nonconforming)
348template <int N1, class E1, int S1, int N2, class E2, int S2> inline
349typename Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >::MulNon
350operator*(const Row<N1,E1,S1>& r1, const Row<N2,E2,S2>& r2) {
351 return Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >
352 ::MulOpNonConforming::perform(r1,r2);
353}
354
355 // Nonconforming: Mat on left -- m*v m*r m*sym
356
357// (mat or vec) = mat * vec (nonconforming)
358template <int M, int N, class ME, int CS, int RS, int MM, class E, int S> inline
359typename Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >::MulNon
361 return Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >
362 ::MulOpNonConforming::perform(m,v);
363}
364// (mat or row) = mat * row (nonconforming)
365template <int M, int N, class ME, int CS, int RS, int NN, class E, int S> inline
366typename Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >::MulNon
368 return Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >
369 ::MulOpNonConforming::perform(m,r);
370}
371
372// (mat or sym) = mat * sym (nonconforming)
373template <int M, int N, class ME, int CS, int RS, int Dim, class E, int S> inline
374typename Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >::MulNon
376 return Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >
377 ::MulOpNonConforming::perform(m,sy);
378}
379
380 // CROSS PRODUCT
381
382// Cross product and corresponding operator%, but only for 2- and 3-Vecs
383// and Rows. It is OK to mix same-length Vecs & Rows; cross product is
384// defined elementwise and never transposes the individual elements.
385//
386// We also define crossMat(v) below which produces a 2x2 or 3x3
387// matrix M such that M*w = v % w for w the same length vector (or row) as v.
388// TODO: the 3d crossMat is skew symmetric but currently there is no
389// SkewMat class defined so we have to return a full 3x3.
390
391// For 3d cross product, we'll follow Matlab and make the return type a
392// Row if either or both arguments are Rows, although I'm not sure that's
393// a good idea! Note that the definition of crossMat means crossMat(r)*v
394// will yield a column, while r%v is a Row.
395
396// We define v % m where v is a 3-vector and m is a 3xN matrix.
397// This returns a matrix c of the same dimensions as m where
398// column c(i) = v % m(i), that is, each column of c is the cross
399// product of v and the corresponding column of m. This definition means that
400// v % m == crossMat(v)*m
401// which seems like a good idea. (But note that v%m takes 9*N flops while
402// crossMat(v)*m takes 15*N flops.) If we have a row vector r instead,
403// we define r % m == v % m so again r%m==crossMat(r)*m. We also
404// define the cross product operator with an Mx3 matrix on the left,
405// defined so that c[i] = m[i]%v, that is, the rows of c are the
406// cross products of the corresonding rows of m and vector v. Again,
407// we allow v to be a row without any change to the results or return type.
408// This definition means m % v = m * crossMat(v), but again it is faster.
409
410// v = v % v
411template <class E1, int S1, class E2, int S2> inline
412Vec<3,typename CNT<E1>::template Result<E2>::Mul>
413cross(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {
414 return Vec<3,typename CNT<E1>::template Result<E2>::Mul>
415 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
416}
417template <class E1, int S1, class E2, int S2> inline
418Vec<3,typename CNT<E1>::template Result<E2>::Mul>
419operator%(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
420
421// r = v % r
422template <class E1, int S1, class E2, int S2> inline
423Row<3,typename CNT<E1>::template Result<E2>::Mul>
424cross(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {
425 return Row<3,typename CNT<E1>::template Result<E2>::Mul>
426 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
427}
428template <class E1, int S1, class E2, int S2> inline
429Row<3,typename CNT<E1>::template Result<E2>::Mul>
430operator%(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
431
432// r = r % v
433template <class E1, int S1, class E2, int S2> inline
434Row<3,typename CNT<E1>::template Result<E2>::Mul>
435cross(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {
436 return Row<3,typename CNT<E1>::template Result<E2>::Mul>
437 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
438}
439template <class E1, int S1, class E2, int S2> inline
440Row<3,typename CNT<E1>::template Result<E2>::Mul>
441operator%(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
442
443// r = r % r
444template <class E1, int S1, class E2, int S2> inline
445Row<3,typename CNT<E1>::template Result<E2>::Mul>
446cross(const Row<3,E1,S1>& a, const Row<3,E2,S2>& b) {
447 return Row<3,typename CNT<E1>::template Result<E2>::Mul>
448 (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
449}
450template <class E1, int S1, class E2, int S2> inline
451Row<3,typename CNT<E1>::template Result<E2>::Mul>
452operator%(const Row<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
453
454
455 // Cross a vector with a matrix. The meaning is given by substituting
456 // the vector's cross product matrix and performing a matrix multiply.
457 // We implement v % m(3,N) for a full matrix m, and v % s(3,3) for
458 // a 3x3 symmetric matrix (producing a 3x3 full result). Variants are
459 // provided with the vector on the right and for when the vector is
460 // supplied as a row (which doesn't change the result). See above
461 // for more details.
462
463// m = v % m
464// Cost is 9*N flops.
465template <class E1, int S1, int N, class E2, int CS, int RS> inline
466Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> // packed
468 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> result;
469 for (int j=0; j < N; ++j)
470 result(j) = v % m(j);
471 return result;
472}
473template <class E1, int S1, int N, class E2, int CS, int RS> inline
474Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
475operator%(const Vec<3,E1,S1>& v, const Mat<3,N,E2,CS,RS>& m) {return cross(v,m);}
476
477// Same as above except we have a Row of N Vec<3>s instead of the matrix.
478// Cost is 9*N flops.
479template <class E1, int S1, int N, class E2, int S2, int S3> inline
480Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
481cross(const Vec<3,E1,S1>& v, const Row<N,Vec<3,E2,S2>,S3>& m) {
482 Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > result;
483 for (int j=0; j < N; ++j)
484 result(j) = v % m(j);
485 return result;
486}
487// Specialize for N==3 to avoid ambiguity
488template <class E1, int S1, class E2, int S2, int S3> inline
489Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
490cross(const Vec<3,E1,S1>& v, const Row<3,Vec<3,E2,S2>,S3>& m) {
491 Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > result;
492 for (int j=0; j < 3; ++j)
493 result(j) = v % m(j);
494 return result;
495}
496template <class E1, int S1, int N, class E2, int S2, int S3> inline
497Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
498operator%(const Vec<3,E1,S1>& v, const Row<N,Vec<3,E2,S2>,S3>& m)
499{ return cross(v,m); }
500template <class E1, int S1, class E2, int S2, int S3> inline
501Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
502operator%(const Vec<3,E1,S1>& v, const Row<3,Vec<3,E2,S2>,S3>& m)
503{ return cross(v,m); }
504
505// m = v % s
506// By writing this out elementwise for the symmetric case we can do this
507// in 24 flops, a small savings over doing three cross products of 9 flops each.
508template<class EV, int SV, class EM, int RS> inline
509Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
510cross(const Vec<3,EV,SV>& v, const SymMat<3,EM,RS>& s) {
511 const EV& x=v[0]; const EV& y=v[1]; const EV& z=v[2];
512 const EM& a=s(0,0);
513 const EM& b=s(1,0); const EM& d=s(1,1);
514 const EM& c=s(2,0); const EM& e=s(2,1); const EM& f=s(2,2);
515
516 typedef typename CNT<EV>::template Result<EM>::Mul EResult;
517 const EResult xe=x*e, yc=y*c, zb=z*b;
518 return Mat<3,3,EResult>
519 ( yc-zb, y*e-z*d, y*f-z*e,
520 z*a-x*c, zb-xe, z*c-x*f,
521 x*b-y*a, x*d-y*b, xe-yc );
522}
523template <class EV, int SV, class EM, int RS> inline
524Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
525operator%(const Vec<3,EV,SV>& v, const SymMat<3,EM,RS>& s) {return cross(v,s);}
526
527// m = r % m
528template <class E1, int S1, int N, class E2, int CS, int RS> inline
529Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> // packed
531 return cross(r.positionalTranspose(), m);
532}
533template <class E1, int S1, int N, class E2, int CS, int RS> inline
534Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
535operator%(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {return cross(r,m);}
536
537// m = r % s
538template<class EV, int SV, class EM, int RS> inline
539Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
540cross(const Row<3,EV,SV>& r, const SymMat<3,EM,RS>& s) {
541 return cross(r.positionalTranspose(), s);
542}
543template<class EV, int SV, class EM, int RS> inline
544Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
545operator%(const Row<3,EV,SV>& r, const SymMat<3,EM,RS>& s) {return cross(r,s);}
546
547// m = m % v
548template <int M, class EM, int CS, int RS, class EV, int S> inline
549Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> // packed
551 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> result;
552 for (int i=0; i < M; ++i)
553 result[i] = m[i] % v;
554 return result;
555}
556template <int M, class EM, int CS, int RS, class EV, int S> inline
557Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> // packed
558operator%(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {return cross(m,v);}
559
560// m = s % v
561// By writing this out elementwise for the symmetric case we can do this
562// in 24 flops, a small savings over doing three cross products of 9 flops each.
563template<class EM, int RS, class EV, int SV> inline
564Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
565cross(const SymMat<3,EM,RS>& s, const Vec<3,EV,SV>& v) {
566 const EV& x=v[0]; const EV& y=v[1]; const EV& z=v[2];
567 const EM& a=s(0,0);
568 const EM& b=s(1,0); const EM& d=s(1,1);
569 const EM& c=s(2,0); const EM& e=s(2,1); const EM& f=s(2,2);
570
571 typedef typename CNT<EV>::template Result<EM>::Mul EResult;
572 const EResult xe=x*e, yc=y*c, zb=z*b;
573 return Mat<3,3,EResult>
574 ( zb-yc, x*c-z*a, y*a-x*b,
575 z*d-y*e, xe-zb, y*b-x*d,
576 z*e-y*f, x*f-z*c, yc-xe );
577}
578template<class EM, int RS, class EV, int SV> inline
579Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
580operator%(const SymMat<3,EM,RS>& s, const Vec<3,EV,SV>& v) {return cross(s,v);}
581
582// m = m % r
583template <int M, class EM, int CS, int RS, class ER, int S> inline
584Mat<M,3,typename CNT<EM>::template Result<ER>::Mul> // packed
586 return cross(m,r.positionalTranspose());
587}
588template <int M, class EM, int CS, int RS, class ER, int S> inline
589Mat<M,3,typename CNT<EM>::template Result<ER>::Mul> // packed
590operator%(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {return cross(m,r);}
591
592// m = s % r
593template<class EM, int RS, class EV, int SV> inline
594Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
595cross(const SymMat<3,EM,RS>& s, const Row<3,EV,SV>& r) {
596 return cross(s,r.positionalTranspose());
597}
598template<class EM, int RS, class EV, int SV> inline
599Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
600operator%(const SymMat<3,EM,RS>& s, const Row<3,EV,SV>& r) {return cross(s,r);}
601
602// 2d cross product just returns a scalar. This is the z-value you
603// would get if the arguments were treated as 3-vectors with 0
604// z components.
605
606template <class E1, int S1, class E2, int S2> inline
607typename CNT<E1>::template Result<E2>::Mul
608cross(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
609 return a[0]*b[1]-a[1]*b[0];
610}
611template <class E1, int S1, class E2, int S2> inline
612typename CNT<E1>::template Result<E2>::Mul
613operator%(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
614
615template <class E1, int S1, class E2, int S2> inline
616typename CNT<E1>::template Result<E2>::Mul
617cross(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
618 return a[0]*b[1]-a[1]*b[0];
619}
620template <class E1, int S1, class E2, int S2> inline
621typename CNT<E1>::template Result<E2>::Mul
622operator%(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
623
624template <class E1, int S1, class E2, int S2> inline
625typename CNT<E1>::template Result<E2>::Mul
626cross(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {
627 return a[0]*b[1]-a[1]*b[0];
628}
629template <class E1, int S1, class E2, int S2> inline
630typename CNT<E1>::template Result<E2>::Mul
631operator%(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
632
633template <class E1, int S1, class E2, int S2> inline
634typename CNT<E1>::template Result<E2>::Mul
635cross(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {
636 return a[0]*b[1]-a[1]*b[0];
637}
638template <class E1, int S1, class E2, int S2> inline
639typename CNT<E1>::template Result<E2>::Mul
640operator%(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
641
642 // CROSS MAT
643
647template <class E, int S> inline
648Mat<3,3,E>
650 return Mat<3,3,E>(Row<3,E>( E(0), -v[2], v[1]),
651 Row<3,E>( v[2], E(0), -v[0]),
652 Row<3,E>(-v[1], v[0], E(0)));
653}
656template <class E, int S> inline
657Mat<3,3,E>
658crossMat(const Vec<3,negator<E>,S>& v) {
659 // Here the "-" operators are just recasts, but the casts
660 // to type E have to perform floating point negations.
661 return Mat<3,3,E>(Row<3,E>( E(0), -v[2], E(v[1])),
662 Row<3,E>( E(v[2]), E(0), -v[0]),
663 Row<3,E>(-v[1], E(v[0]), E(0)));
664}
665
667template <class E, int S> inline
670template <class E, int S> inline
671Mat<3,3,E> crossMat(const Row<3,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
672
676template <class E, int S> inline
678 return Row<2,E>(-v[1], v[0]);
679}
681template <class E, int S> inline
683 return Row<2,E>(-v[1], E(v[0]));
684}
685
687template <class E, int S> inline
690template <class E, int S> inline
691Row<2,E> crossMat(const Row<2,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
692
693 // CROSS MAT SQ
694
715template <class E, int S> inline
716SymMat<3,E>
718 const E xx = square(v[0]);
719 const E yy = square(v[1]);
720 const E zz = square(v[2]);
721 const E nx = -v[0];
722 const E ny = -v[1];
723 return SymMat<3,E>( yy+zz,
724 nx*v[1], xx+zz,
725 nx*v[2], ny*v[2], xx+yy );
726}
727// Specialize above for negated types. Returned matrix loses negator.
728// The total number of flops here is the same as above: 11 flops.
729template <class E, int S> inline
731crossMatSq(const Vec<3,negator<E>,S>& v) {
732 const E xx = square(v[0]);
733 const E yy = square(v[1]);
734 const E zz = square(v[2]);
735 const E y = v[1]; // requires a negation to convert to E
736 const E z = v[2];
737 // The negations in the arguments below are not floating point
738 // operations because the element type is already negated.
739 return SymMat<3,E>( yy+zz,
740 -v[0]*y, xx+zz,
741 -v[0]*z, -v[1]*z, xx+yy );
742}
743
744template <class E, int S> inline
746template <class E, int S> inline
747SymMat<3,E> crossMatSq(const Row<3,negator<E>,S>& r) {return crossMatSq(r.positionalTranspose());}
748
749
750 // DETERMINANT
751
753template <class E, int CS, int RS> inline
755 return m(0,0);
756}
757
759template <class E, int RS> inline
761 return s.diag()[0]; // s(0,0) is trouble for a 1x1 symmetric
762}
763
765template <class E, int CS, int RS> inline
767 // Constant element indices here allow the compiler to select
768 // exactly the right element at compile time.
769 return E(m(0,0)*m(1,1) - m(0,1)*m(1,0));
770}
771
773template <class E, int RS> inline
775 // For Hermitian matrix (i.e., E is complex or conjugate), s(0,1)
776 // and s(1,0) are complex conjugates of one another. Because of the
777 // constant indices here, the SymMat goes right to the correct
778 // element, so everything gets done inline here with no conditionals.
779 return E(s.getEltDiag(0)*s.getEltDiag(1) - s.getEltUpper(0,1)*s.getEltLower(1,0));
780}
781
783template <class E, int CS, int RS> inline
785 return E( m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1))
786 - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0))
787 + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0)));
788}
789
791template <class E, int RS> inline
793 return E( s.getEltDiag(0)*
794 (s.getEltDiag(1)*s.getEltDiag(2)-s.getEltUpper(1,2)*s.getEltLower(2,1))
795 - s.getEltUpper(0,1)*
796 (s.getEltLower(1,0)*s.getEltDiag(2)-s.getEltUpper(1,2)*s.getEltLower(2,0))
797 + s.getEltUpper(0,2)*
798 (s.getEltLower(1,0)*s.getEltLower(2,1)-s.getEltDiag(1)*s.getEltLower(2,0)));
799}
800
814template <int M, class E, int CS, int RS> inline
816 typename CNT<E>::StdNumber sign(1);
817 E result(0);
818 // We're always going to drop the first row.
819 const Mat<M-1,M,E,CS,RS>& m2 = m.template getSubMat<M-1,M>(1,0);
820 for (int j=0; j < M; ++j) {
821 // det() here is recursive but terminates at 3x3 above.
822 result += sign*m(0,j)*det(m2.dropCol(j));
823 sign = -sign;
824 }
825 return result;
826}
827
833template <int M, class E, int RS> inline
835 return det(Mat<M,M,E>(s));
836}
837
838
839 // INVERSE
840
842template <class E, int CS, int RS> inline
844 typedef typename Mat<1,1,E,CS,RS>::TInvert MInv;
845 return MInv( E(typename CNT<E>::StdNumber(1)/m(0,0)) );
846}
847
858template <int M, class E, int CS, int RS> inline
860 // Copy the source matrix, which has arbitrary row and column spacing,
861 // into the type for its inverse, which must be dense, columnwise
862 // storage. That means its column spacing will be M and row spacing
863 // will be 1, as Lapack expects for a Full matrix.
864 typename Mat<M,M,E,CS,RS>::TInvert inv = m; // dense, columnwise storage
865
866 // We'll perform the inversion ignoring negation and conjugation altogether,
867 // but the TInvert Mat type negates or conjugates the result. And because
868 // of the famous Sherman-Eastman theorem, we know that
869 // conj(inv(m))==inv(conj(m)), and of course -inv(m)==inv(-m).
870 typedef typename CNT<E>::StdNumber Raw; // Raw is E without negator or conjugate.
871 Raw* rawData = reinterpret_cast<Raw*>(&inv(0,0));
872 int ipiv[M], info;
873
874 // This replaces "inv" with its LU facorization and pivot matrix P, such that
875 // P*L*U = m (ignoring negation and conjugation).
876 Lapack::getrf<Raw>(M,M,rawData,M,&ipiv[0],info);
877 SimTK_ASSERT1(info>=0, "Argument %d to Lapack getrf routine was bad", -info);
878 SimTK_ERRCHK1_ALWAYS(info==0, "lapackInverse(Mat<>)",
879 "Matrix is singular so can't be inverted (Lapack getrf info=%d).", info);
880
881 // The workspace size must be at least M. For larger matrices, Lapack wants
882 // to do factorization in blocks of size NB in which case the workspace should
883 // be M*NB. However, we will assume that this matrix is small (in the sense
884 // that it fits entirely in cache) so the minimum workspace size is fine and
885 // we don't need an extra getri call to find the workspace size nor any heap
886 // allocation.
887
888 Raw work[M];
889 Lapack::getri<Raw>(M,rawData,M,&ipiv[0],&work[0],M,info);
890 SimTK_ASSERT1(info>=0, "Argument %d to Lapack getri routine was bad", -info);
891 SimTK_ERRCHK1_ALWAYS(info==0, "lapackInverse(Mat<>)",
892 "Matrix is singular so can't be inverted (Lapack getri info=%d).", info);
893 return inv;
894}
895
896
898template <class E, int CS, int RS> inline
900 typedef typename Mat<1,1,E,CS,RS>::TInvert MInv;
901 return MInv( E(typename CNT<E>::StdNumber(1)/m(0,0)) );
902}
903
905template <class E, int RS> inline
907 typedef typename SymMat<1,E,RS>::TInvert SInv;
908 return SInv( E(typename CNT<E>::StdNumber(1)/s.diag()[0]) );
909}
910
912template <class E, int CS, int RS> inline
914 const E d ( det(m) );
915 const typename CNT<E>::TInvert ood( typename CNT<E>::StdNumber(1)/d );
916 return typename Mat<2,2,E,CS,RS>::TInvert( E( ood*m(1,1)), E(-ood*m(0,1)),
917 E(-ood*m(1,0)), E( ood*m(0,0)));
918}
919
921template <class E, int RS> inline
923 const E d ( det(s) );
924 const typename CNT<E>::TInvert ood( typename CNT<E>::StdNumber(1)/d );
925 return typename SymMat<2,E,RS>::TInvert( E( ood*s(1,1)),
926 E(-ood*s(1,0)), E(ood*s(0,0)));
927}
928
933template <class E, int CS, int RS> inline
935 // Calculate determinants for each 2x2 submatrix with first row removed.
936 // (See the specialized 3x3 determinant routine above.) We're calculating
937 // this explicitly here because we can re-use the intermediate terms.
938 const E d00 (m(1,1)*m(2,2)-m(1,2)*m(2,1)),
939 nd01(m(1,2)*m(2,0)-m(1,0)*m(2,2)), // -d01
940 d02 (m(1,0)*m(2,1)-m(1,1)*m(2,0));
941
942 // This is the 3x3 determinant and its inverse.
943 const E d (m(0,0)*d00 + m(0,1)*nd01 + m(0,2)*d02);
944 const typename CNT<E>::TInvert
945 ood(typename CNT<E>::StdNumber(1)/d);
946
947 // The other six 2x2 determinants we can't re-use, but we can still
948 // avoid some copying by calculating them explicitly here.
949 const E nd10(m(0,2)*m(2,1)-m(0,1)*m(2,2)), // -d10
950 d11 (m(0,0)*m(2,2)-m(0,2)*m(2,0)),
951 nd12(m(0,1)*m(2,0)-m(0,0)*m(2,1)), // -d12
952 d20 (m(0,1)*m(1,2)-m(0,2)*m(1,1)),
953 nd21(m(0,2)*m(1,0)-m(0,0)*m(1,2)), // -d21
954 d22 (m(0,0)*m(1,1)-m(0,1)*m(1,0));
955
956 return typename Mat<3,3,E,CS,RS>::TInvert
957 ( E(ood* d00), E(ood*nd10), E(ood* d20),
958 E(ood*nd01), E(ood* d11), E(ood*nd21),
959 E(ood* d02), E(ood*nd12), E(ood* d22) );
960}
961
967template <class E, int RS> inline
969 // Calculate determinants for each 2x2 submatrix with first row removed.
970 // (See the specialized 3x3 determinant routine above.) We're calculating
971 // this explicitly here because we can re-use the intermediate terms.
972 const E d00 (s(1,1)*s(2,2)-s(1,2)*s(2,1)),
973 nd01(s(1,2)*s(2,0)-s(1,0)*s(2,2)), // -d01
974 d02 (s(1,0)*s(2,1)-s(1,1)*s(2,0));
975
976 // This is the 3x3 determinant and its inverse.
977 const E d (s(0,0)*d00 + s(0,1)*nd01 + s(0,2)*d02);
978 const typename CNT<E>::TInvert
979 ood(typename CNT<E>::StdNumber(1)/d);
980
981 // The other six 2x2 determinants we can't re-use, but we can still
982 // avoid some copying by calculating them explicitly here.
983 const E d11 (s(0,0)*s(2,2)-s(0,2)*s(2,0)),
984 nd12(s(0,1)*s(2,0)-s(0,0)*s(2,1)), // -d12
985 d22 (s(0,0)*s(1,1)-s(0,1)*s(1,0));
986
987 return typename SymMat<3,E,RS>::TInvert
988 ( E(ood* d00),
989 E(ood*nd01), E(ood* d11),
990 E(ood* d02), E(ood*nd12), E(ood* d22) );
991}
992
995template <int M, class E, int CS, int RS> inline
997 return lapackInverse(m);
998}
999
1000// Implement the Mat<>::invert() method using the above specialized
1001// inverse functions. This will only compile if M==N.
1002template <int M, int N, class ELT, int CS, int RS> inline
1005 return SimTK::inverse(*this);
1006}
1007
1008} //namespace SimTK
1009
1010
1011#endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
#define SimTK_ASSERT1(cond, msg, a1)
Definition: ExceptionMacros.h:374
Specialized information about Composite Numerical Types which allows us to define appropriate templat...
Definition: CompositeNumericalTypes.h:136
K::StdNumber StdNumber
Definition: CompositeNumericalTypes.h:163
K::TInvert TInvert
Definition: CompositeNumericalTypes.h:157
K::THerm THerm
Definition: CompositeNumericalTypes.h:144
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
TInvert invert() const
Definition: SmallMatrixMixed.h:1004
P E
Definition: Mat.h:99
Mat< N, M, EInvert, N, 1 > TInvert
Definition: Mat.h:171
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: Row.h:132
const TPosTrans & positionalTranspose() const
Definition: Row.h:494
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SymMat.h:87
const TDiag & diag() const
Definition: SymMat.h:822
const TDiag & getDiag() const
Definition: SymMat.h:818
const EHerm & getEltUpper(int i, int j) const
Definition: SymMat.h:842
SymMat< M, EInvert, 1 > TInvert
Definition: SymMat.h:162
const E & getEltDiag(int i) const
Definition: SymMat.h:834
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
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition: negator.h:75
const Real E
e = Real(exp(1))
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Vec< 3, typename CNT< E1 >::template Result< E2 >::Mul > operator%(const Vec< 3, E1, S1 > &a, const Vec< 3, E2, S2 > &b)
Definition: SmallMatrixMixed.h:419
SymMat< 3, E > crossMatSq(const Vec< 3, E, S > &v)
Calculate matrix S(v) such that S(v)*w = -v % (v % w) = (v % w) % v.
Definition: SmallMatrixMixed.h:717
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
Vec< 3, typename CNT< E1 >::template Result< E2 >::Mul > cross(const Vec< 3, E1, S1 > &a, const Vec< 3, E2, S2 > &b)
Definition: SmallMatrixMixed.h:413
Mat< M, M, typename CNT< E1 >::template Result< typename CNT< E2 >::THerm >::Mul > outer(const Vec< M, E1, S1 > &v, const Vec< M, E2, S2 > &w)
Definition: SmallMatrixMixed.h:147
CNT< typenameCNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition: SmallMatrixMixed.h:86
Mat< 1, 1, E, CS, RS >::TInvert inverse(const Mat< 1, 1, E, CS, RS > &m)
Specialized 1x1 Mat inverse: costs one divide.
Definition: SmallMatrixMixed.h:899
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
unsigned char square(unsigned char u)
Definition: Scalar.h:349
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition: SpatialAlgebra.h:791
E det(const Mat< 1, 1, E, CS, RS > &m)
Special case Mat 1x1 determinant. No computation.
Definition: SmallMatrixMixed.h:754
Mat< 1, 1, E, CS, RS >::TInvert lapackInverse(const Mat< 1, 1, E, CS, RS > &m)
Specialized 1x1 lapackInverse(): costs one divide.
Definition: SmallMatrixMixed.h:843
bool operator!=(const L &left, const R &right)
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:641
Mat< 3, 3, E > crossMat(const Vec< 3, E, S > &v)
Calculate matrix M(v) such that M(v)*w = v % w.
Definition: SmallMatrixMixed.h:649