]>
Dogcows Code - chaz/yoink/blob - src/cml/mathlib/matrix_misc.h
1 /* -*- C++ -*- ------------------------------------------------------------
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
8 *-----------------------------------------------------------------------*/
16 #include <cml/mathlib/checking.h>
18 /* Miscellaneous matrix functions. */
22 /** Set a (possibly non-square) matrix to represent an identity transform */
23 template < typename E
, class A
, class B
, class L
> void
24 identity_transform(matrix
<E
,A
,B
,L
>& m
)
26 typedef matrix
<E
,A
,B
,L
> matrix_type
;
27 typedef typename
matrix_type::value_type value_type
;
29 for (size_t i
= 0; i
< m
.rows(); ++i
) {
30 for (size_t j
= 0; j
< m
.cols(); ++j
) {
31 m(i
,j
) = value_type((i
== j
) ? 1 : 0);
36 /** Trace of a square matrix */
37 template < class MatT
> typename
MatT::value_type
40 typedef typename
MatT::value_type value_type
;
43 detail::CheckMatSquare(m
);
45 value_type t
= value_type(0);
46 for (size_t i
= 0; i
< m
.rows(); ++i
) {
52 /** Trace of the upper-left 3x3 part of a matrix */
53 template < class MatT
> typename
MatT::value_type
54 trace_3x3(const MatT
& m
)
57 detail::CheckMatMin3x3(m
);
59 return m(0,0) + m(1,1) + m(2,2);
62 /** Trace of the upper-left 2x2 part of a matrix */
63 template < class MatT
> typename
MatT::value_type
64 trace_2x2(const MatT
& m
)
67 detail::CheckMatMin2x2(m
);
69 return m(0,0) + m(1,1);
72 /** 3D skew-symmetric matrix */
73 template < typename E
, class A
, class B
, class L
, class VecT
> void
74 matrix_skew_symmetric(matrix
<E
,A
,B
,L
>& m
, const VecT
& v
)
77 detail::CheckMatMin3x3(m
);
82 m
.set_basis_element(1,2, v
[0]);
83 m
.set_basis_element(2,1,-v
[0]);
84 m
.set_basis_element(2,0, v
[1]);
85 m
.set_basis_element(0,2,-v
[1]);
86 m
.set_basis_element(0,1, v
[2]);
87 m
.set_basis_element(1,0,-v
[2]);
90 /** 2D skew-symmetric matrix */
91 template < typename E
, class A
, class B
, class L
> void
92 matrix_skew_symmetric_2D(matrix
<E
,A
,B
,L
>& m
, E s
)
95 detail::CheckMatMin2x2(m
);
99 m
.set_basis_element(0,1, s
);
100 m
.set_basis_element(1,0,-s
);
103 /* @todo: Clean this up, and implement SRT as well */
105 /** Invert a matrix consisting of a 3D rotation and translation */
106 template < typename E
, class A
, class B
, class L
> void
107 matrix_invert_RT_only(matrix
<E
,A
,B
,L
>& m
)
109 typedef vector
< E
, fixed
<3> > vector_type
;
112 matrix_get_basis_vectors(m
,x
,y
,z
);
113 matrix_set_transposed_basis_vectors(m
,x
,y
,z
);
115 vector_type p
= matrix_get_translation(m
);
116 matrix_set_translation(m
,-dot(p
,x
),-dot(p
,y
),-dot(p
,z
));
119 /** Invert a matrix consisting of a 2D rotation and ranslation */
120 template < typename E
, class A
, class B
, class L
> void
121 matrix_invert_RT_only_2D(matrix
<E
,A
,B
,L
>& m
)
123 typedef vector
< E
, fixed
<2> > vector_type
;
126 matrix_get_basis_vectors_2D(m
,x
,y
);
127 matrix_set_transposed_basis_vectors_2D(m
,x
,y
);
129 vector_type p
= matrix_get_translation_2D(m
);
130 matrix_set_translation_2D(m
,-dot(p
,x
),-dot(p
,y
));
This page took 0.040564 seconds and 5 git commands to generate.