![]() |
Eigen
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> 00005 // Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com> 00006 // 00007 // This Source Code Form is subject to the terms of the Mozilla 00008 // Public License v. 2.0. If a copy of the MPL was not distributed 00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00010 00011 #ifndef EIGEN_ORTHOMETHODS_H 00012 #define EIGEN_ORTHOMETHODS_H 00013 00014 namespace Eigen { 00015 00027 template<typename Derived> 00028 template<typename OtherDerived> 00029 #ifndef EIGEN_PARSED_BY_DOXYGEN 00030 EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type 00031 #else 00032 inline typename MatrixBase<Derived>::PlainObject 00033 #endif 00034 MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const 00035 { 00036 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3) 00037 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3) 00038 00039 // Note that there is no need for an expression here since the compiler 00040 // optimize such a small temporary very well (even within a complex expression) 00041 typename internal::nested_eval<Derived,2>::type lhs(derived()); 00042 typename internal::nested_eval<OtherDerived,2>::type rhs(other.derived()); 00043 return typename cross_product_return_type<OtherDerived>::type( 00044 numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), 00045 numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), 00046 numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)) 00047 ); 00048 } 00049 00050 namespace internal { 00051 00052 template< int Arch,typename VectorLhs,typename VectorRhs, 00053 typename Scalar = typename VectorLhs::Scalar, 00054 bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)> 00055 struct cross3_impl { 00056 EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type<VectorLhs>::type 00057 run(const VectorLhs& lhs, const VectorRhs& rhs) 00058 { 00059 return typename internal::plain_matrix_type<VectorLhs>::type( 00060 numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), 00061 numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), 00062 numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)), 00063 0 00064 ); 00065 } 00066 }; 00067 00068 } 00069 00079 template<typename Derived> 00080 template<typename OtherDerived> 00081 EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::PlainObject 00082 MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const 00083 { 00084 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4) 00085 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4) 00086 00087 typedef typename internal::nested_eval<Derived,2>::type DerivedNested; 00088 typedef typename internal::nested_eval<OtherDerived,2>::type OtherDerivedNested; 00089 DerivedNested lhs(derived()); 00090 OtherDerivedNested rhs(other.derived()); 00091 00092 return internal::cross3_impl<Architecture::Target, 00093 typename internal::remove_all<DerivedNested>::type, 00094 typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs); 00095 } 00096 00106 template<typename ExpressionType, int Direction> 00107 template<typename OtherDerived> 00108 EIGEN_DEVICE_FUNC 00109 const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType 00110 VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const 00111 { 00112 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3) 00113 EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value), 00114 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 00115 00116 typename internal::nested_eval<ExpressionType,2>::type mat(_expression()); 00117 typename internal::nested_eval<OtherDerived,2>::type vec(other.derived()); 00118 00119 CrossReturnType res(_expression().rows(),_expression().cols()); 00120 if(Direction==Vertical) 00121 { 00122 eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows"); 00123 res.row(0) = (mat.row(1) * vec.coeff(2) - mat.row(2) * vec.coeff(1)).conjugate(); 00124 res.row(1) = (mat.row(2) * vec.coeff(0) - mat.row(0) * vec.coeff(2)).conjugate(); 00125 res.row(2) = (mat.row(0) * vec.coeff(1) - mat.row(1) * vec.coeff(0)).conjugate(); 00126 } 00127 else 00128 { 00129 eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns"); 00130 res.col(0) = (mat.col(1) * vec.coeff(2) - mat.col(2) * vec.coeff(1)).conjugate(); 00131 res.col(1) = (mat.col(2) * vec.coeff(0) - mat.col(0) * vec.coeff(2)).conjugate(); 00132 res.col(2) = (mat.col(0) * vec.coeff(1) - mat.col(1) * vec.coeff(0)).conjugate(); 00133 } 00134 return res; 00135 } 00136 00137 namespace internal { 00138 00139 template<typename Derived, int Size = Derived::SizeAtCompileTime> 00140 struct unitOrthogonal_selector 00141 { 00142 typedef typename plain_matrix_type<Derived>::type VectorType; 00143 typedef typename traits<Derived>::Scalar Scalar; 00144 typedef typename NumTraits<Scalar>::Real RealScalar; 00145 typedef Matrix<Scalar,2,1> Vector2; 00146 EIGEN_DEVICE_FUNC 00147 static inline VectorType run(const Derived& src) 00148 { 00149 VectorType perp = VectorType::Zero(src.size()); 00150 Index maxi = 0; 00151 Index sndi = 0; 00152 src.cwiseAbs().maxCoeff(&maxi); 00153 if (maxi==0) 00154 sndi = 1; 00155 RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm(); 00156 perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm; 00157 perp.coeffRef(sndi) = numext::conj(src.coeff(maxi)) * invnm; 00158 00159 return perp; 00160 } 00161 }; 00162 00163 template<typename Derived> 00164 struct unitOrthogonal_selector<Derived,3> 00165 { 00166 typedef typename plain_matrix_type<Derived>::type VectorType; 00167 typedef typename traits<Derived>::Scalar Scalar; 00168 typedef typename NumTraits<Scalar>::Real RealScalar; 00169 EIGEN_DEVICE_FUNC 00170 static inline VectorType run(const Derived& src) 00171 { 00172 VectorType perp; 00173 /* Let us compute the crossed product of *this with a vector 00174 * that is not too close to being colinear to *this. 00175 */ 00176 00177 /* unless the x and y coords are both close to zero, we can 00178 * simply take ( -y, x, 0 ) and normalize it. 00179 */ 00180 if((!isMuchSmallerThan(src.x(), src.z())) 00181 || (!isMuchSmallerThan(src.y(), src.z()))) 00182 { 00183 RealScalar invnm = RealScalar(1)/src.template head<2>().norm(); 00184 perp.coeffRef(0) = -numext::conj(src.y())*invnm; 00185 perp.coeffRef(1) = numext::conj(src.x())*invnm; 00186 perp.coeffRef(2) = 0; 00187 } 00188 /* if both x and y are close to zero, then the vector is close 00189 * to the z-axis, so it's far from colinear to the x-axis for instance. 00190 * So we take the crossed product with (1,0,0) and normalize it. 00191 */ 00192 else 00193 { 00194 RealScalar invnm = RealScalar(1)/src.template tail<2>().norm(); 00195 perp.coeffRef(0) = 0; 00196 perp.coeffRef(1) = -numext::conj(src.z())*invnm; 00197 perp.coeffRef(2) = numext::conj(src.y())*invnm; 00198 } 00199 00200 return perp; 00201 } 00202 }; 00203 00204 template<typename Derived> 00205 struct unitOrthogonal_selector<Derived,2> 00206 { 00207 typedef typename plain_matrix_type<Derived>::type VectorType; 00208 EIGEN_DEVICE_FUNC 00209 static inline VectorType run(const Derived& src) 00210 { return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); } 00211 }; 00212 00213 } // end namespace internal 00214 00224 template<typename Derived> 00225 EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject 00226 MatrixBase<Derived>::unitOrthogonal() const 00227 { 00228 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 00229 return internal::unitOrthogonal_selector<Derived>::run(derived()); 00230 } 00231 00232 } // end namespace Eigen 00233 00234 #endif // EIGEN_ORTHOMETHODS_H