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