Simbody  3.8
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 
33 namespace SimTK {
34 
35  // COMPARISON
36 
37 // m==s
38 template <int M, class EL, int CSL, int RSL, class ER, int RSR> inline
39 bool operator==(const Mat<M,M,EL,CSL,RSL>& l, const SymMat<M,ER,RSR>& r) {
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
51 template <int M, class EL, int CSL, int RSL, class ER, int RSR> inline
52 bool operator!=(const Mat<M,M,EL,CSL,RSL>& l, const SymMat<M,ER,RSR>& r) {
53  return !(l==r);
54 }
55 
56 // s==m
57 template <int M, class EL, int RSL, class ER, int CSR, int RSR> inline
58 bool operator==(const SymMat<M,EL,RSL>& l, const Mat<M,M,ER,CSR,RSR>& r) {
59  return r==l;
60 }
61 // s!=m
62 template <int M, class EL, int RSL, class ER, int CSR, int RSR> inline
63 bool operator!=(const SymMat<M,EL,RSL>& l, const Mat<M,M,ER,CSR,RSR>& r) {
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 
84 template <int M, class E1, int S1, class E2, int S2> inline
85 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
86 dot(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 }
90 template <class E1, int S1, class E2, int S2> inline
91 typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
92 dot(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)
98 template <int N, class E1, int S1, class E2, int S2> inline
99 typename CNT<E1>::template Result<E2>::Mul
100 operator*(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
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 }
104 template <class E1, int S1, class E2, int S2> inline
105 typename CNT<E1>::template Result<E2>::Mul
106 operator*(const Row<1,E1,S1>& r, const Vec<1,E2,S2>& v) {
107  typename CNT<E1>::template Result<E2>::Mul sum(r[0]*v[0]);
108  return sum;
109 }
110 
111 // Alternate acceptable forms for dot().
112 template <int N, class E1, int S1, class E2, int S2> inline
113 typename CNT<E1>::template Result<E2>::Mul
114 dot(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
115  return dot(r.positionalTranspose(),v);
116 }
117 template <int M, class E1, int S1, class E2, int S2> inline
118 typename CNT<E1>::template Result<E2>::Mul
119 dot(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
120  return dot(v,r.positionalTranspose());
121 }
122 template <int N, class E1, int S1, class E2, int S2> inline
123 typename CNT<E1>::template Result<E2>::Mul
124 dot(const Row<N,E1,S1>& r, const Row<N,E2,S2>& s) {
125  return dot(r.positionalTranspose(),s.positionalTranspose());
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 
145 template <int M, class E1, int S1, class E2, int S2> inline
146 Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul>
147 outer(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)
155 template <int M, class E1, int S1, class E2, int S2> inline
156 typename Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::Mul
157 operator*(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
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().
163 template <int M, class E1, int S1, class E2, int S2> inline
164 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
165 outer(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
166  return outer(v,r.positionalTranspose());
167 }
168 template <int M, class E1, int S1, class E2, int S2> inline
169 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
170 outer(const Row<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
171  return outer(r.positionalTranspose(),v);
172 }
173 template <int M, class E1, int S1, class E2, int S2> inline
174 Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
175 outer(const Row<M,E1,S1>& r, const Row<M,E2,S2>& s) {
176  return outer(r.positionalTranspose(),s.positionalTranspose());
177 }
178 
179  // MAT*VEC, ROW*MAT (conforming)
180 
181 // vec = mat * vec (conforming)
182 template <int M, int N, class ME, int CS, int RS, class E, int S> inline
183 typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul
184 operator*(const Mat<M,N,ME,CS,RS>& m,const Vec<N,E,S>& v) {
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)
192 template <int M, class E, int S, int N, class ME, int CS, int RS> inline
193 typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul
194 operator*(const Row<M,E,S>& r, const Mat<M,N,ME,CS,RS>& m) {
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)
204 template <int N, class ME, int RS, class E, int S> inline
205 typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul
206 operator*(const SymMat<N,ME,RS>& m,const Vec<N,E,S>& v) {
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.
221 template <class ME, int RS, class E, int S> inline
222 typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul
223 operator*(const SymMat<1,ME,RS>& m,const Vec<1,E,S>& v) {
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.
230 template <class ME, int RS, class E, int S> inline
231 typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul
232 operator*(const SymMat<2,ME,RS>& m,const Vec<2,E,S>& v) {
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.
240 template <class ME, int RS, class E, int S> inline
241 typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul
242 operator*(const SymMat<3,ME,RS>& m,const Vec<3,E,S>& v) {
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)
251 template <int M, class E, int S, class ME, int RS> inline
252 typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul
253 operator*(const Row<M,E,S>& r, const SymMat<M,ME,RS>& m) {
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.
268 template <class E, int S, class ME, int RS> inline
269 typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul
270 operator*(const Row<1,E,S>& r, const SymMat<1,ME,RS>& m) {
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.
277 template <class E, int S, class ME, int RS> inline
278 typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul
279 operator*(const Row<2,E,S>& r, const SymMat<2,ME,RS>& m) {
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.
287 template <class E, int S, class ME, int RS> inline
288 typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul
289 operator*(const Row<3,E,S>& r, const SymMat<3,ME,RS>& m) {
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)
303 template <int M, class E1, int S1, int N, class E2, int S2> inline
304 typename Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulNon
305 operator*(const Vec<M,E1,S1>& v, const Row<N,E2,S2>& r) {
306  return Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulOpNonConforming::perform(v,r);
307 }
308 // elementwise multiply (vec * nonconforming mat)
309 template <int M, class E1, int S1, int MM, int NN, class E2, int CS2, int RS2> inline
310 typename Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >::MulNon
311 operator*(const Vec<M,E1,S1>& v, const Mat<MM,NN,E2,CS2,RS2>& m) {
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)
316 template <int M, class E1, int S1, int MM, class E2, int RS2> inline
317 typename Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >::MulNon
318 operator*(const Vec<M,E1,S1>& v, const SymMat<MM,E2,RS2>& m) {
319  return Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >
320  ::MulOpNonConforming::perform(v,m);
321 }
322 // elementwise multiply (vec * nonconforming vec)
323 template <int M, class E1, int S1, int MM, class E2, int S2> inline
324 typename Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >::MulNon
325 operator*(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)
334 template <int M, class E, int S, int MM, int NN, class ME, int CS, int RS> inline
335 typename Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >::MulNon
336 operator*(const Row<M,E,S>& r, const Mat<MM,NN,ME,CS,RS>& m) {
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)
341 template <int N, class E1, int S1, int M, class E2, int S2> inline
342 typename Row<N,E1,S1>::template Result<Vec<M,E2,S2> >::MulNon
343 operator*(const Row<N,E1,S1>& r, const Vec<M,E2,S2>& v) {
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)
348 template <int N1, class E1, int S1, int N2, class E2, int S2> inline
349 typename Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >::MulNon
350 operator*(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)
358 template <int M, int N, class ME, int CS, int RS, int MM, class E, int S> inline
359 typename Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >::MulNon
360 operator*(const Mat<M,N,ME,CS,RS>& m,const Vec<MM,E,S>& v) {
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)
365 template <int M, int N, class ME, int CS, int RS, int NN, class E, int S> inline
366 typename Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >::MulNon
367 operator*(const Mat<M,N,ME,CS,RS>& m,const Row<NN,E,S>& r) {
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)
373 template <int M, int N, class ME, int CS, int RS, int Dim, class E, int S> inline
374 typename Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >::MulNon
375 operator*(const Mat<M,N,ME,CS,RS>& m,const SymMat<Dim,E,S>& sy) {
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
411 template <class E1, int S1, class E2, int S2> inline
412 Vec<3,typename CNT<E1>::template Result<E2>::Mul>
413 cross(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 }
417 template <class E1, int S1, class E2, int S2> inline
418 Vec<3,typename CNT<E1>::template Result<E2>::Mul>
419 operator%(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
420 
421 // r = v % r
422 template <class E1, int S1, class E2, int S2> inline
423 Row<3,typename CNT<E1>::template Result<E2>::Mul>
424 cross(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 }
428 template <class E1, int S1, class E2, int S2> inline
429 Row<3,typename CNT<E1>::template Result<E2>::Mul>
430 operator%(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
431 
432 // r = r % v
433 template <class E1, int S1, class E2, int S2> inline
434 Row<3,typename CNT<E1>::template Result<E2>::Mul>
435 cross(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 }
439 template <class E1, int S1, class E2, int S2> inline
440 Row<3,typename CNT<E1>::template Result<E2>::Mul>
441 operator%(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
442 
443 // r = r % r
444 template <class E1, int S1, class E2, int S2> inline
445 Row<3,typename CNT<E1>::template Result<E2>::Mul>
446 cross(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 }
450 template <class E1, int S1, class E2, int S2> inline
451 Row<3,typename CNT<E1>::template Result<E2>::Mul>
452 operator%(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.
465 template <class E1, int S1, int N, class E2, int CS, int RS> inline
466 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> // packed
467 cross(const Vec<3,E1,S1>& v, const Mat<3,N,E2,CS,RS>& m) {
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 }
473 template <class E1, int S1, int N, class E2, int CS, int RS> inline
474 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
475 operator%(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.
479 template <class E1, int S1, int N, class E2, int S2, int S3> inline
480 Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
481 cross(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
488 template <class E1, int S1, class E2, int S2, int S3> inline
489 Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
490 cross(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 }
496 template <class E1, int S1, int N, class E2, int S2, int S3> inline
497 Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
498 operator%(const Vec<3,E1,S1>& v, const Row<N,Vec<3,E2,S2>,S3>& m)
499 { return cross(v,m); }
500 template <class E1, int S1, class E2, int S2, int S3> inline
501 Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
502 operator%(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.
508 template<class EV, int SV, class EM, int RS> inline
509 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
510 cross(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 }
523 template <class EV, int SV, class EM, int RS> inline
524 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
525 operator%(const Vec<3,EV,SV>& v, const SymMat<3,EM,RS>& s) {return cross(v,s);}
526 
527 // m = r % m
528 template <class E1, int S1, int N, class E2, int CS, int RS> inline
529 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> // packed
530 cross(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {
531  return cross(r.positionalTranspose(), m);
532 }
533 template <class E1, int S1, int N, class E2, int CS, int RS> inline
534 Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
535 operator%(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {return cross(r,m);}
536 
537 // m = r % s
538 template<class EV, int SV, class EM, int RS> inline
539 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
540 cross(const Row<3,EV,SV>& r, const SymMat<3,EM,RS>& s) {
541  return cross(r.positionalTranspose(), s);
542 }
543 template<class EV, int SV, class EM, int RS> inline
544 Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
545 operator%(const Row<3,EV,SV>& r, const SymMat<3,EM,RS>& s) {return cross(r,s);}
546 
547 // m = m % v
548 template <int M, class EM, int CS, int RS, class EV, int S> inline
549 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> // packed
550 cross(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {
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 }
556 template <int M, class EM, int CS, int RS, class EV, int S> inline
557 Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> // packed
558 operator%(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.
563 template<class EM, int RS, class EV, int SV> inline
564 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
565 cross(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 }
578 template<class EM, int RS, class EV, int SV> inline
579 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
580 operator%(const SymMat<3,EM,RS>& s, const Vec<3,EV,SV>& v) {return cross(s,v);}
581 
582 // m = m % r
583 template <int M, class EM, int CS, int RS, class ER, int S> inline
584 Mat<M,3,typename CNT<EM>::template Result<ER>::Mul> // packed
585 cross(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {
586  return cross(m,r.positionalTranspose());
587 }
588 template <int M, class EM, int CS, int RS, class ER, int S> inline
589 Mat<M,3,typename CNT<EM>::template Result<ER>::Mul> // packed
590 operator%(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {return cross(m,r);}
591 
592 // m = s % r
593 template<class EM, int RS, class EV, int SV> inline
594 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
595 cross(const SymMat<3,EM,RS>& s, const Row<3,EV,SV>& r) {
596  return cross(s,r.positionalTranspose());
597 }
598 template<class EM, int RS, class EV, int SV> inline
599 Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
600 operator%(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 
606 template <class E1, int S1, class E2, int S2> inline
607 typename CNT<E1>::template Result<E2>::Mul
608 cross(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
609  return a[0]*b[1]-a[1]*b[0];
610 }
611 template <class E1, int S1, class E2, int S2> inline
612 typename CNT<E1>::template Result<E2>::Mul
613 operator%(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
614 
615 template <class E1, int S1, class E2, int S2> inline
616 typename CNT<E1>::template Result<E2>::Mul
617 cross(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
618  return a[0]*b[1]-a[1]*b[0];
619 }
620 template <class E1, int S1, class E2, int S2> inline
621 typename CNT<E1>::template Result<E2>::Mul
622 operator%(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
623 
624 template <class E1, int S1, class E2, int S2> inline
625 typename CNT<E1>::template Result<E2>::Mul
626 cross(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {
627  return a[0]*b[1]-a[1]*b[0];
628 }
629 template <class E1, int S1, class E2, int S2> inline
630 typename CNT<E1>::template Result<E2>::Mul
631 operator%(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
632 
633 template <class E1, int S1, class E2, int S2> inline
634 typename CNT<E1>::template Result<E2>::Mul
635 cross(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {
636  return a[0]*b[1]-a[1]*b[0];
637 }
638 template <class E1, int S1, class E2, int S2> inline
639 typename CNT<E1>::template Result<E2>::Mul
640 operator%(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
641 
642  // CROSS MAT
643 
647 template <class E, int S> inline
648 Mat<3,3,E>
649 crossMat(const Vec<3,E,S>& v) {
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 }
656 template <class E, int S> inline
657 Mat<3,3,E>
658 crossMat(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 
667 template <class E, int S> inline
668 Mat<3,3,E> crossMat(const Row<3,E,S>& r) {return crossMat(r.positionalTranspose());}
670 template <class E, int S> inline
671 Mat<3,3,E> crossMat(const Row<3,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
672 
676 template <class E, int S> inline
677 Row<2,E> crossMat(const Vec<2,E,S>& v) {
678  return Row<2,E>(-v[1], v[0]);
679 }
681 template <class E, int S> inline
682 Row<2,E> crossMat(const Vec<2,negator<E>,S>& v) {
683  return Row<2,E>(-v[1], E(v[0]));
684 }
685 
687 template <class E, int S> inline
688 Row<2,E> crossMat(const Row<2,E,S>& r) {return crossMat(r.positionalTranspose());}
690 template <class E, int S> inline
691 Row<2,E> crossMat(const Row<2,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
692 
693  // CROSS MAT SQ
694 
715 template <class E, int S> inline
716 SymMat<3,E>
717 crossMatSq(const Vec<3,E,S>& v) {
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.
729 template <class E, int S> inline
730 SymMat<3,E>
731 crossMatSq(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 
744 template <class E, int S> inline
745 SymMat<3,E> crossMatSq(const Row<3,E,S>& r) {return crossMatSq(r.positionalTranspose());}
746 template <class E, int S> inline
747 SymMat<3,E> crossMatSq(const Row<3,negator<E>,S>& r) {return crossMatSq(r.positionalTranspose());}
748 
749 
750  // DETERMINANT
751 
753 template <class E, int CS, int RS> inline
754 E det(const Mat<1,1,E,CS,RS>& m) {
755  return m(0,0);
756 }
757 
759 template <class E, int RS> inline
760 E det(const SymMat<1,E,RS>& s) {
761  return s.diag()[0]; // s(0,0) is trouble for a 1x1 symmetric
762 }
763 
765 template <class E, int CS, int RS> inline
766 E det(const Mat<2,2,E,CS,RS>& m) {
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 
773 template <class E, int RS> inline
774 E det(const SymMat<2,E,RS>& s) {
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 
783 template <class E, int CS, int RS> inline
784 E det(const Mat<3,3,E,CS,RS>& m) {
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 
791 template <class E, int RS> inline
792 E det(const SymMat<3,E,RS>& s) {
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 
814 template <int M, class E, int CS, int RS> inline
815 E det(const Mat<M,M,E,CS,RS>& m) {
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 
833 template <int M, class E, int RS> inline
834 E det(const SymMat<M,E,RS>& s) {
835  return det(Mat<M,M,E>(s));
836 }
837 
838 
839  // INVERSE
840 
842 template <class E, int CS, int RS> inline
843 typename Mat<1,1,E,CS,RS>::TInvert lapackInverse(const Mat<1,1,E,CS,RS>& m) {
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 
858 template <int M, class E, int CS, int RS> inline
859 typename Mat<M,M,E,CS,RS>::TInvert lapackInverse(const Mat<M,M,E,CS,RS>& m) {
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 
898 template <class E, int CS, int RS> inline
899 typename Mat<1,1,E,CS,RS>::TInvert inverse(const Mat<1,1,E,CS,RS>& m) {
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 
905 template <class E, int RS> inline
906 typename SymMat<1,E,RS>::TInvert inverse(const SymMat<1,E,RS>& s) {
907  typedef typename SymMat<1,E,RS>::TInvert SInv;
908  return SInv( E(typename CNT<E>::StdNumber(1)/s.diag()[0]) );
909 }
910 
912 template <class E, int CS, int RS> inline
913 typename Mat<2,2,E,CS,RS>::TInvert inverse(const Mat<2,2,E,CS,RS>& m) {
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 
921 template <class E, int RS> inline
922 typename SymMat<2,E,RS>::TInvert inverse(const SymMat<2,E,RS>& s) {
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 
933 template <class E, int CS, int RS> inline
934 typename Mat<3,3,E,CS,RS>::TInvert inverse(const Mat<3,3,E,CS,RS>& m) {
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 
967 template <class E, int RS> inline
968 typename SymMat<3,E,RS>::TInvert inverse(const SymMat<3,E,RS>& s) {
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 
995 template <int M, class E, int CS, int RS> inline
996 typename Mat<M,M,E,CS,RS>::TInvert inverse(const Mat<M,M,E,CS,RS>& m) {
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.
1002 template <int M, int N, class ELT, int CS, int RS> inline
1003 typename Mat<M,N,ELT,CS,RS>::TInvert
1004 Mat<M,N,ELT,CS,RS>::invert() const {
1005  return SimTK::inverse(*this);
1006 }
1007 
1008 } //namespace SimTK
1009 
1010 
1011 #endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
SimTK_ERRCHK1_ALWAYS
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
SimTK_ASSERT1
#define SimTK_ASSERT1(cond, msg, a1)
Definition: ExceptionMacros.h:374
SimTK::CNT
Specialized information about Composite Numerical Types which allows us to define appropriate templat...
Definition: CompositeNumericalTypes.h:136
SimTK::CNT::StdNumber
K::StdNumber StdNumber
Definition: CompositeNumericalTypes.h:163
SimTK::CNT::TInvert
K::TInvert TInvert
Definition: CompositeNumericalTypes.h:157
SimTK::CNT::THerm
K::THerm THerm
Definition: CompositeNumericalTypes.h:144
SimTK::Mat
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
SimTK::Mat::invert
TInvert invert() const
Definition: SmallMatrixMixed.h:1004
SimTK::Mat::TInvert
Mat< N, M, EInvert, N, 1 > TInvert
Definition: Mat.h:171
SimTK::Row
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: Row.h:132
SimTK::Row::positionalTranspose
const TPosTrans & positionalTranspose() const
Definition: Row.h:494
SimTK::SymMat
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition: SymMat.h:87
SimTK::SymMat::diag
const TDiag & diag() const
Definition: SymMat.h:822
SimTK::SymMat::getEltUpper
const EHerm & getEltUpper(int i, int j) const
Definition: SymMat.h:842
SimTK::SymMat::getDiag
const TDiag & getDiag() const
Definition: SymMat.h:818
SimTK::SymMat::TInvert
SymMat< M, EInvert, 1 > TInvert
Definition: SymMat.h:162
SimTK::SymMat::getEltLower
const E & getEltLower(int i, int j) const
Definition: SymMat.h:838
SimTK::SymMat::getEltDiag
const E & getEltDiag(int i) const
Definition: SymMat.h:834
SimTK::Vec
This is a fixed-length column vector designed for no-overhead inline computation.
Definition: Vec.h:184
SimTK::negator
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition: negator.h:75
SimTK::E
const Real E
e = Real(exp(1))
SimTK
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
SimTK::outer
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
SimTK::lapackInverse
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
SimTK::inverse
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
SimTK::sign
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
SimTK::sum
ELEM sum(const VectorBase< ELEM > &v)
Definition: VectorMath.h:147
SimTK::operator%
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
SimTK::crossMatSq
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
SimTK::crossMat
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
SimTK::operator*
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
SimTK::square
unsigned char square(unsigned char u)
Definition: Scalar.h:349
SimTK::operator==
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition: SpatialAlgebra.h:791
SimTK::det
E det(const Mat< 1, 1, E, CS, RS > &m)
Special case Mat 1x1 determinant. No computation.
Definition: SmallMatrixMixed.h:754
SimTK::operator!=
bool operator!=(const L &left, const R &right)
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:645
SimTK::cross
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
SimTK::dot
CNT< typename CNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition: SmallMatrixMixed.h:86