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@inria.fr> 5// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@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 14namespace 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 */ 23template<typename Derived> 24template<typename OtherDerived> 25inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type 26MatrixBase<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 42namespace internal { 43 44template< int Arch,typename VectorLhs,typename VectorRhs, 45 typename Scalar = typename VectorLhs::Scalar, 46 bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)> 47struct 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 */ 71template<typename Derived> 72template<typename OtherDerived> 73inline typename MatrixBase<Derived>::PlainObject 74MatrixBase<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() */ 98template<typename ExpressionType, int Direction> 99template<typename OtherDerived> 100const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType 101VectorwiseOp<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 125namespace internal { 126 127template<typename Derived, int Size = Derived::SizeAtCompileTime> 128struct 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 151template<typename Derived> 152struct 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 191template<typename Derived> 192struct 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 */ 208template<typename Derived> 209typename MatrixBase<Derived>::PlainObject 210MatrixBase<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