OrthoMethods.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
00006 //
00007 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 #ifndef EIGEN_ORTHOMETHODS_H
00027 #define EIGEN_ORTHOMETHODS_H
00028 
00029 namespace Eigen { 
00030 
00038 template<typename Derived>
00039 template<typename OtherDerived>
00040 inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type
00041 MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
00042 {
00043   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
00044   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
00045 
00046   // Note that there is no need for an expression here since the compiler
00047   // optimize such a small temporary very well (even within a complex expression)
00048   typename internal::nested<Derived,2>::type lhs(derived());
00049   typename internal::nested<OtherDerived,2>::type rhs(other.derived());
00050   return typename cross_product_return_type<OtherDerived>::type(
00051     internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
00052     internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
00053     internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))
00054   );
00055 }
00056 
00057 namespace internal {
00058 
00059 template< int Arch,typename VectorLhs,typename VectorRhs,
00060           typename Scalar = typename VectorLhs::Scalar,
00061           bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)>
00062 struct cross3_impl {
00063   static inline typename internal::plain_matrix_type<VectorLhs>::type
00064   run(const VectorLhs& lhs, const VectorRhs& rhs)
00065   {
00066     return typename internal::plain_matrix_type<VectorLhs>::type(
00067       internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
00068       internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
00069       internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)),
00070       0
00071     );
00072   }
00073 };
00074 
00075 }
00076 
00086 template<typename Derived>
00087 template<typename OtherDerived>
00088 inline typename MatrixBase<Derived>::PlainObject
00089 MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const
00090 {
00091   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4)
00092   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4)
00093 
00094   typedef typename internal::nested<Derived,2>::type DerivedNested;
00095   typedef typename internal::nested<OtherDerived,2>::type OtherDerivedNested;
00096   const DerivedNested lhs(derived());
00097   const OtherDerivedNested rhs(other.derived());
00098 
00099   return internal::cross3_impl<Architecture::Target,
00100                         typename internal::remove_all<DerivedNested>::type,
00101                         typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs);
00102 }
00103 
00113 template<typename ExpressionType, int Direction>
00114 template<typename OtherDerived>
00115 const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType
00116 VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const
00117 {
00118   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
00119   EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
00120     YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00121 
00122   CrossReturnType res(_expression().rows(),_expression().cols());
00123   if(Direction==Vertical)
00124   {
00125     eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows");
00126     res.row(0) = (_expression().row(1) * other.coeff(2) - _expression().row(2) * other.coeff(1)).conjugate();
00127     res.row(1) = (_expression().row(2) * other.coeff(0) - _expression().row(0) * other.coeff(2)).conjugate();
00128     res.row(2) = (_expression().row(0) * other.coeff(1) - _expression().row(1) * other.coeff(0)).conjugate();
00129   }
00130   else
00131   {
00132     eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns");
00133     res.col(0) = (_expression().col(1) * other.coeff(2) - _expression().col(2) * other.coeff(1)).conjugate();
00134     res.col(1) = (_expression().col(2) * other.coeff(0) - _expression().col(0) * other.coeff(2)).conjugate();
00135     res.col(2) = (_expression().col(0) * other.coeff(1) - _expression().col(1) * other.coeff(0)).conjugate();
00136   }
00137   return res;
00138 }
00139 
00140 namespace internal {
00141 
00142 template<typename Derived, int Size = Derived::SizeAtCompileTime>
00143 struct unitOrthogonal_selector
00144 {
00145   typedef typename plain_matrix_type<Derived>::type VectorType;
00146   typedef typename traits<Derived>::Scalar Scalar;
00147   typedef typename NumTraits<Scalar>::Real RealScalar;
00148   typedef typename Derived::Index Index;
00149   typedef Matrix<Scalar,2,1> Vector2;
00150   static inline VectorType run(const Derived& src)
00151   {
00152     VectorType perp = VectorType::Zero(src.size());
00153     Index maxi = 0;
00154     Index sndi = 0;
00155     src.cwiseAbs().maxCoeff(&maxi);
00156     if (maxi==0)
00157       sndi = 1;
00158     RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm();
00159     perp.coeffRef(maxi) = -conj(src.coeff(sndi)) * invnm;
00160     perp.coeffRef(sndi) =  conj(src.coeff(maxi)) * invnm;
00161 
00162     return perp;
00163    }
00164 };
00165 
00166 template<typename Derived>
00167 struct unitOrthogonal_selector<Derived,3>
00168 {
00169   typedef typename plain_matrix_type<Derived>::type VectorType;
00170   typedef typename traits<Derived>::Scalar Scalar;
00171   typedef typename NumTraits<Scalar>::Real RealScalar;
00172   static inline VectorType run(const Derived& src)
00173   {
00174     VectorType perp;
00175     /* Let us compute the crossed product of *this with a vector
00176      * that is not too close to being colinear to *this.
00177      */
00178 
00179     /* unless the x and y coords are both close to zero, we can
00180      * simply take ( -y, x, 0 ) and normalize it.
00181      */
00182     if((!isMuchSmallerThan(src.x(), src.z()))
00183     || (!isMuchSmallerThan(src.y(), src.z())))
00184     {
00185       RealScalar invnm = RealScalar(1)/src.template head<2>().norm();
00186       perp.coeffRef(0) = -conj(src.y())*invnm;
00187       perp.coeffRef(1) = conj(src.x())*invnm;
00188       perp.coeffRef(2) = 0;
00189     }
00190     /* if both x and y are close to zero, then the vector is close
00191      * to the z-axis, so it's far from colinear to the x-axis for instance.
00192      * So we take the crossed product with (1,0,0) and normalize it.
00193      */
00194     else
00195     {
00196       RealScalar invnm = RealScalar(1)/src.template tail<2>().norm();
00197       perp.coeffRef(0) = 0;
00198       perp.coeffRef(1) = -conj(src.z())*invnm;
00199       perp.coeffRef(2) = conj(src.y())*invnm;
00200     }
00201 
00202     return perp;
00203    }
00204 };
00205 
00206 template<typename Derived>
00207 struct unitOrthogonal_selector<Derived,2>
00208 {
00209   typedef typename plain_matrix_type<Derived>::type VectorType;
00210   static inline VectorType run(const Derived& src)
00211   { return VectorType(-conj(src.y()), conj(src.x())).normalized(); }
00212 };
00213 
00214 } // end namespace internal
00215 
00223 template<typename Derived>
00224 typename MatrixBase<Derived>::PlainObject
00225 MatrixBase<Derived>::unitOrthogonal() const
00226 {
00227   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
00228   return internal::unitOrthogonal_selector<Derived>::run(derived());
00229 }
00230 
00231 } // end namespace Eigen
00232 
00233 #endif // EIGEN_ORTHOMETHODS_H