Home | History | Annotate | Download | only in Geometry 
      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 (at) inria.fr>
      5 // Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1 (at) gmail.com>
      6 //
      7 // This Source Code Form is subject to the terms of the Mozilla
      8 // Public License v. 2.0. If a copy of the MPL was not distributed
      9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
     10 
     11 #ifndef EIGEN_ORTHOMETHODS_H
     12 #define EIGEN_ORTHOMETHODS_H
     13 
     14 namespace Eigen {
     15 
     16 /** \geometry_module \ingroup Geometry_Module
     17   *
     18   * \returns the cross product of \c *this and \a other
     19   *
     20   * Here is a very good explanation of cross-product: http://xkcd.com/199/
     21   *
     22   * With complex numbers, the cross product is implemented as
     23   * \f$ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} - \mathbf{b} \times \mathbf{c})\f$
     24   *
     25   * \sa MatrixBase::cross3()
     26   */
     27 template<typename Derived>
     28 template<typename OtherDerived>
     29 #ifndef EIGEN_PARSED_BY_DOXYGEN
     30 EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type
     31 #else
     32 inline typename MatrixBase<Derived>::PlainObject
     33 #endif
     34 MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
     35 {
     36   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
     37   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
     38 
     39   // Note that there is no need for an expression here since the compiler
     40   // optimize such a small temporary very well (even within a complex expression)
     41   typename internal::nested_eval<Derived,2>::type lhs(derived());
     42   typename internal::nested_eval<OtherDerived,2>::type rhs(other.derived());
     43   return typename cross_product_return_type<OtherDerived>::type(
     44     numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
     45     numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
     46     numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))
     47   );
     48 }
     49 
     50 namespace internal {
     51 
     52 template< int Arch,typename VectorLhs,typename VectorRhs,
     53           typename Scalar = typename VectorLhs::Scalar,
     54           bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)>
     55 struct cross3_impl {
     56   EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type<VectorLhs>::type
     57   run(const VectorLhs& lhs, const VectorRhs& rhs)
     58   {
     59     return typename internal::plain_matrix_type<VectorLhs>::type(
     60       numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
     61       numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
     62       numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)),
     63       0
     64     );
     65   }
     66 };
     67 
     68 }
     69 
     70 /** \geometry_module \ingroup Geometry_Module
     71   *
     72   * \returns the cross product of \c *this and \a other using only the x, y, and z coefficients
     73   *
     74   * The size of \c *this and \a other must be four. This function is especially useful
     75   * when using 4D vectors instead of 3D ones to get advantage of SSE/AltiVec vectorization.
     76   *
     77   * \sa MatrixBase::cross()
     78   */
     79 template<typename Derived>
     80 template<typename OtherDerived>
     81 EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::PlainObject
     82 MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const
     83 {
     84   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4)
     85   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4)
     86 
     87   typedef typename internal::nested_eval<Derived,2>::type DerivedNested;
     88   typedef typename internal::nested_eval<OtherDerived,2>::type OtherDerivedNested;
     89   DerivedNested lhs(derived());
     90   OtherDerivedNested rhs(other.derived());
     91 
     92   return internal::cross3_impl<Architecture::Target,
     93                         typename internal::remove_all<DerivedNested>::type,
     94                         typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs);
     95 }
     96 
     97 /** \geometry_module \ingroup Geometry_Module
     98   *
     99   * \returns a matrix expression of the cross product of each column or row
    100   * of the referenced expression with the \a other vector.
    101   *
    102   * The referenced matrix must have one dimension equal to 3.
    103   * The result matrix has the same dimensions than the referenced one.
    104   *
    105   * \sa MatrixBase::cross() */
    106 template<typename ExpressionType, int Direction>
    107 template<typename OtherDerived>
    108 EIGEN_DEVICE_FUNC
    109 const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType
    110 VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const
    111 {
    112   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
    113   EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
    114     YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
    115 
    116   typename internal::nested_eval<ExpressionType,2>::type mat(_expression());
    117   typename internal::nested_eval<OtherDerived,2>::type vec(other.derived());
    118 
    119   CrossReturnType res(_expression().rows(),_expression().cols());
    120   if(Direction==Vertical)
    121   {
    122     eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows");
    123     res.row(0) = (mat.row(1) * vec.coeff(2) - mat.row(2) * vec.coeff(1)).conjugate();
    124     res.row(1) = (mat.row(2) * vec.coeff(0) - mat.row(0) * vec.coeff(2)).conjugate();
    125     res.row(2) = (mat.row(0) * vec.coeff(1) - mat.row(1) * vec.coeff(0)).conjugate();
    126   }
    127   else
    128   {
    129     eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns");
    130     res.col(0) = (mat.col(1) * vec.coeff(2) - mat.col(2) * vec.coeff(1)).conjugate();
    131     res.col(1) = (mat.col(2) * vec.coeff(0) - mat.col(0) * vec.coeff(2)).conjugate();
    132     res.col(2) = (mat.col(0) * vec.coeff(1) - mat.col(1) * vec.coeff(0)).conjugate();
    133   }
    134   return res;
    135 }
    136 
    137 namespace internal {
    138 
    139 template<typename Derived, int Size = Derived::SizeAtCompileTime>
    140 struct unitOrthogonal_selector
    141 {
    142   typedef typename plain_matrix_type<Derived>::type VectorType;
    143   typedef typename traits<Derived>::Scalar Scalar;
    144   typedef typename NumTraits<Scalar>::Real RealScalar;
    145   typedef Matrix<Scalar,2,1> Vector2;
    146   EIGEN_DEVICE_FUNC
    147   static inline VectorType run(const Derived& src)
    148   {
    149     VectorType perp = VectorType::Zero(src.size());
    150     Index maxi = 0;
    151     Index sndi = 0;
    152     src.cwiseAbs().maxCoeff(&maxi);
    153     if (maxi==0)
    154       sndi = 1;
    155     RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm();
    156     perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm;
    157     perp.coeffRef(sndi) =  numext::conj(src.coeff(maxi)) * invnm;
    158 
    159     return perp;
    160    }
    161 };
    162 
    163 template<typename Derived>
    164 struct unitOrthogonal_selector<Derived,3>
    165 {
    166   typedef typename plain_matrix_type<Derived>::type VectorType;
    167   typedef typename traits<Derived>::Scalar Scalar;
    168   typedef typename NumTraits<Scalar>::Real RealScalar;
    169   EIGEN_DEVICE_FUNC
    170   static inline VectorType run(const Derived& src)
    171   {
    172     VectorType perp;
    173     /* Let us compute the crossed product of *this with a vector
    174      * that is not too close to being colinear to *this.
    175      */
    176 
    177     /* unless the x and y coords are both close to zero, we can
    178      * simply take ( -y, x, 0 ) and normalize it.
    179      */
    180     if((!isMuchSmallerThan(src.x(), src.z()))
    181     || (!isMuchSmallerThan(src.y(), src.z())))
    182     {
    183       RealScalar invnm = RealScalar(1)/src.template head<2>().norm();
    184       perp.coeffRef(0) = -numext::conj(src.y())*invnm;
    185       perp.coeffRef(1) = numext::conj(src.x())*invnm;
    186       perp.coeffRef(2) = 0;
    187     }
    188     /* if both x and y are close to zero, then the vector is close
    189      * to the z-axis, so it's far from colinear to the x-axis for instance.
    190      * So we take the crossed product with (1,0,0) and normalize it.
    191      */
    192     else
    193     {
    194       RealScalar invnm = RealScalar(1)/src.template tail<2>().norm();
    195       perp.coeffRef(0) = 0;
    196       perp.coeffRef(1) = -numext::conj(src.z())*invnm;
    197       perp.coeffRef(2) = numext::conj(src.y())*invnm;
    198     }
    199 
    200     return perp;
    201    }
    202 };
    203 
    204 template<typename Derived>
    205 struct unitOrthogonal_selector<Derived,2>
    206 {
    207   typedef typename plain_matrix_type<Derived>::type VectorType;
    208   EIGEN_DEVICE_FUNC
    209   static inline VectorType run(const Derived& src)
    210   { return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); }
    211 };
    212 
    213 } // end namespace internal
    214 
    215 /** \geometry_module \ingroup Geometry_Module
    216   *
    217   * \returns a unit vector which is orthogonal to \c *this
    218   *
    219   * The size of \c *this must be at least 2. If the size is exactly 2,
    220   * then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized().
    221   *
    222   * \sa cross()
    223   */
    224 template<typename Derived>
    225 EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject
    226 MatrixBase<Derived>::unitOrthogonal() const
    227 {
    228   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
    229   return internal::unitOrthogonal_selector<Derived>::run(derived());
    230 }
    231 
    232 } // end namespace Eigen
    233 
    234 #endif // EIGEN_ORTHOMETHODS_H
    235