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 
00036 template<typename Derived>
00037 template<typename OtherDerived>
00038 inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type
00039 MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
00040 {
00041   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
00042   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
00043 
00044   // Note that there is no need for an expression here since the compiler
00045   // optimize such a small temporary very well (even within a complex expression)
00046   const typename internal::nested<Derived,2>::type lhs(derived());
00047   const typename internal::nested<OtherDerived,2>::type rhs(other.derived());
00048   return typename cross_product_return_type<OtherDerived>::type(
00049     internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
00050     internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
00051     internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))
00052   );
00053 }
00054 
00055 namespace internal {
00056 
00057 template< int Arch,typename VectorLhs,typename VectorRhs,
00058           typename Scalar = typename VectorLhs::Scalar,
00059           bool Vectorizable = (VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit>
00060 struct cross3_impl {
00061   inline static typename internal::plain_matrix_type<VectorLhs>::type
00062   run(const VectorLhs& lhs, const VectorRhs& rhs)
00063   {
00064     return typename internal::plain_matrix_type<VectorLhs>::type(
00065       internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
00066       internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
00067       internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)),
00068       0
00069     );
00070   }
00071 };
00072 
00073 }
00074 
00084 template<typename Derived>
00085 template<typename OtherDerived>
00086 inline typename MatrixBase<Derived>::PlainObject
00087 MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const
00088 {
00089   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4)
00090   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4)
00091 
00092   typedef typename internal::nested<Derived,2>::type DerivedNested;
00093   typedef typename internal::nested<OtherDerived,2>::type OtherDerivedNested;
00094   const DerivedNested lhs(derived());
00095   const OtherDerivedNested rhs(other.derived());
00096 
00097   return internal::cross3_impl<Architecture::Target,
00098                         typename internal::remove_all<DerivedNested>::type,
00099                         typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs);
00100 }
00101 
00111 template<typename ExpressionType, int Direction>
00112 template<typename OtherDerived>
00113 const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType
00114 VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const
00115 {
00116   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
00117   EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
00118     YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00119 
00120   CrossReturnType res(_expression().rows(),_expression().cols());
00121   if(Direction==Vertical)
00122   {
00123     eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows");
00124     res.row(0) = (_expression().row(1) * other.coeff(2) - _expression().row(2) * other.coeff(1)).conjugate();
00125     res.row(1) = (_expression().row(2) * other.coeff(0) - _expression().row(0) * other.coeff(2)).conjugate();
00126     res.row(2) = (_expression().row(0) * other.coeff(1) - _expression().row(1) * other.coeff(0)).conjugate();
00127   }
00128   else
00129   {
00130     eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns");
00131     res.col(0) = (_expression().col(1) * other.coeff(2) - _expression().col(2) * other.coeff(1)).conjugate();
00132     res.col(1) = (_expression().col(2) * other.coeff(0) - _expression().col(0) * other.coeff(2)).conjugate();
00133     res.col(2) = (_expression().col(0) * other.coeff(1) - _expression().col(1) * other.coeff(0)).conjugate();
00134   }
00135   return res;
00136 }
00137 
00138 namespace internal {
00139 
00140 template<typename Derived, int Size = Derived::SizeAtCompileTime>
00141 struct unitOrthogonal_selector
00142 {
00143   typedef typename plain_matrix_type<Derived>::type VectorType;
00144   typedef typename traits<Derived>::Scalar Scalar;
00145   typedef typename NumTraits<Scalar>::Real RealScalar;
00146   typedef typename Derived::Index Index;
00147   typedef Matrix<Scalar,2,1> Vector2;
00148   inline static VectorType run(const Derived& src)
00149   {
00150     VectorType perp = VectorType::Zero(src.size());
00151     Index maxi = 0;
00152     Index sndi = 0;
00153     src.cwiseAbs().maxCoeff(&maxi);
00154     if (maxi==0)
00155       sndi = 1;
00156     RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm();
00157     perp.coeffRef(maxi) = -conj(src.coeff(sndi)) * invnm;
00158     perp.coeffRef(sndi) =  conj(src.coeff(maxi)) * invnm;
00159 
00160     return perp;
00161    }
00162 };
00163 
00164 template<typename Derived>
00165 struct unitOrthogonal_selector<Derived,3>
00166 {
00167   typedef typename plain_matrix_type<Derived>::type VectorType;
00168   typedef typename traits<Derived>::Scalar Scalar;
00169   typedef typename NumTraits<Scalar>::Real RealScalar;
00170   inline static VectorType run(const Derived& src)
00171   {
00172     VectorType perp;
00173     /* Let us compute the crossed product of *this with a vector
00174      * that is not too close to being colinear to *this.
00175      */
00176 
00177     /* unless the x and y coords are both close to zero, we can
00178      * simply take ( -y, x, 0 ) and normalize it.
00179      */
00180     if((!isMuchSmallerThan(src.x(), src.z()))
00181     || (!isMuchSmallerThan(src.y(), src.z())))
00182     {
00183       RealScalar invnm = RealScalar(1)/src.template head<2>().norm();
00184       perp.coeffRef(0) = -conj(src.y())*invnm;
00185       perp.coeffRef(1) = conj(src.x())*invnm;
00186       perp.coeffRef(2) = 0;
00187     }
00188     /* if both x and y are close to zero, then the vector is close
00189      * to the z-axis, so it's far from colinear to the x-axis for instance.
00190      * So we take the crossed product with (1,0,0) and normalize it.
00191      */
00192     else
00193     {
00194       RealScalar invnm = RealScalar(1)/src.template tail<2>().norm();
00195       perp.coeffRef(0) = 0;
00196       perp.coeffRef(1) = -conj(src.z())*invnm;
00197       perp.coeffRef(2) = conj(src.y())*invnm;
00198     }
00199 
00200     return perp;
00201    }
00202 };
00203 
00204 template<typename Derived>
00205 struct unitOrthogonal_selector<Derived,2>
00206 {
00207   typedef typename plain_matrix_type<Derived>::type VectorType;
00208   inline static VectorType run(const Derived& src)
00209   { return VectorType(-conj(src.y()), conj(src.x())).normalized(); }
00210 };
00211 
00212 } // end namespace internal
00213 
00221 template<typename Derived>
00222 typename MatrixBase<Derived>::PlainObject
00223 MatrixBase<Derived>::unitOrthogonal() const
00224 {
00225   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
00226   return internal::unitOrthogonal_selector<Derived>::run(derived());
00227 }
00228 
00229 #endif // EIGEN_ORTHOMETHODS_H


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:33:08