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