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