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