OrthoMethods.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
6 //
7 // Eigen is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation; either
10 // version 3 of the License, or (at your option) any later version.
11 //
12 // Alternatively, you can redistribute it and/or
13 // modify it under the terms of the GNU General Public License as
14 // published by the Free Software Foundation; either version 2 of
15 // the License, or (at your option) any later version.
16 //
17 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
19 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
20 // GNU General Public License for more details.
21 //
22 // You should have received a copy of the GNU Lesser General Public
23 // License and a copy of the GNU General Public License along with
24 // Eigen. If not, see <http://www.gnu.org/licenses/>.
25 
26 #ifndef EIGEN_ORTHOMETHODS_H
27 #define EIGEN_ORTHOMETHODS_H
28 
29 namespace Eigen {
30 
38 template<typename Derived>
39 template<typename OtherDerived>
40 inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type
42 {
45 
46  // Note that there is no need for an expression here since the compiler
47  // optimize such a small temporary very well (even within a complex expression)
48  typename internal::nested<Derived,2>::type lhs(derived());
49  typename internal::nested<OtherDerived,2>::type rhs(other.derived());
50  return typename cross_product_return_type<OtherDerived>::type(
51  internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
52  internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
53  internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))
54  );
55 }
56 
57 namespace internal {
58 
59 template< int Arch,typename VectorLhs,typename VectorRhs,
60  typename Scalar = typename VectorLhs::Scalar,
61  bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)>
62 struct cross3_impl {
63  static inline typename internal::plain_matrix_type<VectorLhs>::type
64  run(const VectorLhs& lhs, const VectorRhs& rhs)
65  {
66  return typename internal::plain_matrix_type<VectorLhs>::type(
67  internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
68  internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
69  internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)),
70  0
71  );
72  }
73 };
74 
75 }
76 
86 template<typename Derived>
87 template<typename OtherDerived>
88 inline typename MatrixBase<Derived>::PlainObject
90 {
93 
94  typedef typename internal::nested<Derived,2>::type DerivedNested;
95  typedef typename internal::nested<OtherDerived,2>::type OtherDerivedNested;
96  const DerivedNested lhs(derived());
97  const OtherDerivedNested rhs(other.derived());
98 
99  return internal::cross3_impl<Architecture::Target,
100  typename internal::remove_all<DerivedNested>::type,
101  typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs);
102 }
103 
113 template<typename ExpressionType, int Direction>
114 template<typename OtherDerived>
117 {
119  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
120  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
121 
122  CrossReturnType res(_expression().rows(),_expression().cols());
123  if(Direction==Vertical)
124  {
125  eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows");
126  res.row(0) = (_expression().row(1) * other.coeff(2) - _expression().row(2) * other.coeff(1)).conjugate();
127  res.row(1) = (_expression().row(2) * other.coeff(0) - _expression().row(0) * other.coeff(2)).conjugate();
128  res.row(2) = (_expression().row(0) * other.coeff(1) - _expression().row(1) * other.coeff(0)).conjugate();
129  }
130  else
131  {
132  eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns");
133  res.col(0) = (_expression().col(1) * other.coeff(2) - _expression().col(2) * other.coeff(1)).conjugate();
134  res.col(1) = (_expression().col(2) * other.coeff(0) - _expression().col(0) * other.coeff(2)).conjugate();
135  res.col(2) = (_expression().col(0) * other.coeff(1) - _expression().col(1) * other.coeff(0)).conjugate();
136  }
137  return res;
138 }
139 
140 namespace internal {
141 
142 template<typename Derived, int Size = Derived::SizeAtCompileTime>
143 struct unitOrthogonal_selector
144 {
145  typedef typename plain_matrix_type<Derived>::type VectorType;
146  typedef typename traits<Derived>::Scalar Scalar;
147  typedef typename NumTraits<Scalar>::Real RealScalar;
148  typedef typename Derived::Index Index;
149  typedef Matrix<Scalar,2,1> Vector2;
150  static inline VectorType run(const Derived& src)
151  {
152  VectorType perp = VectorType::Zero(src.size());
153  Index maxi = 0;
154  Index sndi = 0;
155  src.cwiseAbs().maxCoeff(&maxi);
156  if (maxi==0)
157  sndi = 1;
158  RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm();
159  perp.coeffRef(maxi) = -conj(src.coeff(sndi)) * invnm;
160  perp.coeffRef(sndi) = conj(src.coeff(maxi)) * invnm;
161 
162  return perp;
163  }
164 };
165 
166 template<typename Derived>
167 struct unitOrthogonal_selector<Derived,3>
168 {
169  typedef typename plain_matrix_type<Derived>::type VectorType;
170  typedef typename traits<Derived>::Scalar Scalar;
171  typedef typename NumTraits<Scalar>::Real RealScalar;
172  static inline VectorType run(const Derived& src)
173  {
174  VectorType perp;
175  /* Let us compute the crossed product of *this with a vector
176  * that is not too close to being colinear to *this.
177  */
178 
179  /* unless the x and y coords are both close to zero, we can
180  * simply take ( -y, x, 0 ) and normalize it.
181  */
182  if((!isMuchSmallerThan(src.x(), src.z()))
183  || (!isMuchSmallerThan(src.y(), src.z())))
184  {
185  RealScalar invnm = RealScalar(1)/src.template head<2>().norm();
186  perp.coeffRef(0) = -conj(src.y())*invnm;
187  perp.coeffRef(1) = conj(src.x())*invnm;
188  perp.coeffRef(2) = 0;
189  }
190  /* if both x and y are close to zero, then the vector is close
191  * to the z-axis, so it's far from colinear to the x-axis for instance.
192  * So we take the crossed product with (1,0,0) and normalize it.
193  */
194  else
195  {
196  RealScalar invnm = RealScalar(1)/src.template tail<2>().norm();
197  perp.coeffRef(0) = 0;
198  perp.coeffRef(1) = -conj(src.z())*invnm;
199  perp.coeffRef(2) = conj(src.y())*invnm;
200  }
201 
202  return perp;
203  }
204 };
205 
206 template<typename Derived>
207 struct unitOrthogonal_selector<Derived,2>
208 {
209  typedef typename plain_matrix_type<Derived>::type VectorType;
210  static inline VectorType run(const Derived& src)
211  { return VectorType(-conj(src.y()), conj(src.x())).normalized(); }
212 };
213 
214 } // end namespace internal
215 
223 template<typename Derived>
224 typename MatrixBase<Derived>::PlainObject
226 {
228  return internal::unitOrthogonal_selector<Derived>::run(derived());
229 }
230 
231 } // end namespace Eigen
232 
233 #endif // EIGEN_ORTHOMETHODS_H