src/Geometry/OrthoMethods.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
0005 // Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
0006 //
0007 // This Source Code Form is subject to the terms of the Mozilla
0008 // Public License v. 2.0. If a copy of the MPL was not distributed
0009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0010
0011 #ifndef EIGEN_ORTHOMETHODS_H
0012 #define EIGEN_ORTHOMETHODS_H
0013
0014 namespace RivetEigen {
0015
0016 /** \geometry_module \ingroup Geometry_Module
0017   *
0018   * \returns the cross product of \c *this and \a other
0019   *
0020   * Here is a very good explanation of cross-product: http://xkcd.com/199/
0021   *
0022   * With complex numbers, the cross product is implemented as
0023   * \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$
0024   *
0025   * \sa MatrixBase::cross3()
0026   */
0027 template<typename Derived>
0028 template<typename OtherDerived>
0029 #ifndef EIGEN_PARSED_BY_DOXYGEN
0030 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
0031 typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type
0032 #else
0033 typename MatrixBase<Derived>::PlainObject
0034 #endif
0035 MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
0036 {
0037   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
0038   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
0039
0040   // Note that there is no need for an expression here since the compiler
0041   // optimize such a small temporary very well (even within a complex expression)
0042   typename internal::nested_eval<Derived,2>::type lhs(derived());
0043   typename internal::nested_eval<OtherDerived,2>::type rhs(other.derived());
0044   return typename cross_product_return_type<OtherDerived>::type(
0045     numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
0046     numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
0047     numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0))
0048   );
0049 }
0050
0051 namespace internal {
0052
0053 template< int Arch,typename VectorLhs,typename VectorRhs,
0054           typename Scalar = typename VectorLhs::Scalar,
0055           bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)>
0056 struct cross3_impl {
0057   EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type<VectorLhs>::type
0058   run(const VectorLhs& lhs, const VectorRhs& rhs)
0059   {
0060     return typename internal::plain_matrix_type<VectorLhs>::type(
0061       numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)),
0062       numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)),
0063       numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)),
0064       0
0065     );
0066   }
0067 };
0068
0069 }
0070
0071 /** \geometry_module \ingroup Geometry_Module
0072   *
0073   * \returns the cross product of \c *this and \a other using only the x, y, and z coefficients
0074   *
0075   * The size of \c *this and \a other must be four. This function is especially useful
0076   * when using 4D vectors instead of 3D ones to get advantage of SSE/AltiVec vectorization.
0077   *
0078   * \sa MatrixBase::cross()
0079   */
0080 template<typename Derived>
0081 template<typename OtherDerived>
0082 EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::PlainObject
0083 MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const
0084 {
0085   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4)
0086   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4)
0087
0088   typedef typename internal::nested_eval<Derived,2>::type DerivedNested;
0089   typedef typename internal::nested_eval<OtherDerived,2>::type OtherDerivedNested;
0090   DerivedNested lhs(derived());
0091   OtherDerivedNested rhs(other.derived());
0092
0093   return internal::cross3_impl<Architecture::Target,
0094                         typename internal::remove_all<DerivedNested>::type,
0095                         typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs);
0096 }
0097
0098 /** \geometry_module \ingroup Geometry_Module
0099   *
0100   * \returns a matrix expression of the cross product of each column or row
0101   * of the referenced expression with the \a other vector.
0102   *
0103   * The referenced matrix must have one dimension equal to 3.
0104   * The result matrix has the same dimensions than the referenced one.
0105   *
0106   * \sa MatrixBase::cross() */
0107 template<typename ExpressionType, int Direction>
0108 template<typename OtherDerived>
0109 EIGEN_DEVICE_FUNC
0110 const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType
0111 VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const
0112 {
0113   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
0114   EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
0115     YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
0116
0117   typename internal::nested_eval<ExpressionType,2>::type mat(_expression());
0118   typename internal::nested_eval<OtherDerived,2>::type vec(other.derived());
0119
0120   CrossReturnType res(_expression().rows(),_expression().cols());
0121   if(Direction==Vertical)
0122   {
0123     eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows");
0124     res.row(0) = (mat.row(1) * vec.coeff(2) - mat.row(2) * vec.coeff(1)).conjugate();
0125     res.row(1) = (mat.row(2) * vec.coeff(0) - mat.row(0) * vec.coeff(2)).conjugate();
0126     res.row(2) = (mat.row(0) * vec.coeff(1) - mat.row(1) * vec.coeff(0)).conjugate();
0127   }
0128   else
0129   {
0130     eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns");
0131     res.col(0) = (mat.col(1) * vec.coeff(2) - mat.col(2) * vec.coeff(1)).conjugate();
0132     res.col(1) = (mat.col(2) * vec.coeff(0) - mat.col(0) * vec.coeff(2)).conjugate();
0133     res.col(2) = (mat.col(0) * vec.coeff(1) - mat.col(1) * vec.coeff(0)).conjugate();
0134   }
0135   return res;
0136 }
0137
0138 namespace internal {
0139
0140 template<typename Derived, int Size = Derived::SizeAtCompileTime>
0141 struct unitOrthogonal_selector
0142 {
0143   typedef typename plain_matrix_type<Derived>::type VectorType;
0144   typedef typename traits<Derived>::Scalar Scalar;
0145   typedef typename NumTraits<Scalar>::Real RealScalar;
0146   typedef Matrix<Scalar,2,1> Vector2;
0147   EIGEN_DEVICE_FUNC
0148   static inline VectorType run(const Derived& src)
0149   {
0150     VectorType perp = VectorType::Zero(src.size());
0151     Index maxi = 0;
0152     Index sndi = 0;
0153     src.cwiseAbs().maxCoeff(&maxi);
0154     if (maxi==0)
0155       sndi = 1;
0156     RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm();
0157     perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm;
0158     perp.coeffRef(sndi) =  numext::conj(src.coeff(maxi)) * invnm;
0159
0160     return perp;
0161    }
0162 };
0163
0164 template<typename Derived>
0165 struct unitOrthogonal_selector<Derived,3>
0166 {
0167   typedef typename plain_matrix_type<Derived>::type VectorType;
0168   typedef typename traits<Derived>::Scalar Scalar;
0169   typedef typename NumTraits<Scalar>::Real RealScalar;
0170   EIGEN_DEVICE_FUNC
0171   static inline VectorType run(const Derived& src)
0172   {
0173     VectorType perp;
0174     /* Let us compute the crossed product of *this with a vector
0175      * that is not too close to being colinear to *this.
0176      */
0177
0178     /* unless the x and y coords are both close to zero, we can
0179      * simply take ( -y, x, 0 ) and normalize it.
0180      */
0181     if((!isMuchSmallerThan(src.x(), src.z()))
0182     || (!isMuchSmallerThan(src.y(), src.z())))
0183     {
0184       RealScalar invnm = RealScalar(1)/src.template head<2>().norm();
0185       perp.coeffRef(0) = -numext::conj(src.y())*invnm;
0186       perp.coeffRef(1) = numext::conj(src.x())*invnm;
0187       perp.coeffRef(2) = 0;
0188     }
0189     /* if both x and y are close to zero, then the vector is close
0190      * to the z-axis, so it's far from colinear to the x-axis for instance.
0191      * So we take the crossed product with (1,0,0) and normalize it.
0192      */
0193     else
0194     {
0195       RealScalar invnm = RealScalar(1)/src.template tail<2>().norm();
0196       perp.coeffRef(0) = 0;
0197       perp.coeffRef(1) = -numext::conj(src.z())*invnm;
0198       perp.coeffRef(2) = numext::conj(src.y())*invnm;
0199     }
0200
0201     return perp;
0202    }
0203 };
0204
0205 template<typename Derived>
0206 struct unitOrthogonal_selector<Derived,2>
0207 {
0208   typedef typename plain_matrix_type<Derived>::type VectorType;
0209   EIGEN_DEVICE_FUNC
0210   static inline VectorType run(const Derived& src)
0211   { return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); }
0212 };
0213
0214 } // end namespace internal
0215
0216 /** \geometry_module \ingroup Geometry_Module
0217   *
0218   * \returns a unit vector which is orthogonal to \c *this
0219   *
0220   * The size of \c *this must be at least 2. If the size is exactly 2,
0221   * then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized().
0222   *
0223   * \sa cross()
0224   */
0225 template<typename Derived>
0226 EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject
0227 MatrixBase<Derived>::unitOrthogonal() const
0228 {
0229   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
0230   return internal::unitOrthogonal_selector<Derived>::run(derived());
0231 }
0232
0233 } // end namespace RivetEigen
0234
0235 #endif // EIGEN_ORTHOMETHODS_H