00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef EIGEN_ORTHOMETHODS_H
00027 #define EIGEN_ORTHOMETHODS_H
00028
00035 template<typename Derived>
00036 template<typename OtherDerived>
00037 inline typename MatrixBase<Derived>::PlainMatrixType
00038 MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
00039 {
00040 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
00041
00042
00043
00044 const typename ei_nested<Derived,2>::type lhs(derived());
00045 const typename ei_nested<OtherDerived,2>::type rhs(other.derived());
00046 return typename ei_plain_matrix_type<Derived>::type(
00047 lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1),
00048 lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2),
00049 lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)
00050 );
00051 }
00052
00053 template<typename Derived, int Size = Derived::SizeAtCompileTime>
00054 struct ei_unitOrthogonal_selector
00055 {
00056 typedef typename ei_plain_matrix_type<Derived>::type VectorType;
00057 typedef typename ei_traits<Derived>::Scalar Scalar;
00058 typedef typename NumTraits<Scalar>::Real RealScalar;
00059 inline static VectorType run(const Derived& src)
00060 {
00061 VectorType perp(src.size());
00062
00063
00064
00065
00066
00067
00068
00069 if((!ei_isMuchSmallerThan(src.x(), src.z()))
00070 || (!ei_isMuchSmallerThan(src.y(), src.z())))
00071 {
00072 RealScalar invnm = RealScalar(1)/src.template start<2>().norm();
00073 perp.coeffRef(0) = -ei_conj(src.y())*invnm;
00074 perp.coeffRef(1) = ei_conj(src.x())*invnm;
00075 perp.coeffRef(2) = 0;
00076 }
00077
00078
00079
00080
00081 else
00082 {
00083 RealScalar invnm = RealScalar(1)/src.template end<2>().norm();
00084 perp.coeffRef(0) = 0;
00085 perp.coeffRef(1) = -ei_conj(src.z())*invnm;
00086 perp.coeffRef(2) = ei_conj(src.y())*invnm;
00087 }
00088 if( (Derived::SizeAtCompileTime!=Dynamic && Derived::SizeAtCompileTime>3)
00089 || (Derived::SizeAtCompileTime==Dynamic && src.size()>3) )
00090 perp.end(src.size()-3).setZero();
00091
00092 return perp;
00093 }
00094 };
00095
00096 template<typename Derived>
00097 struct ei_unitOrthogonal_selector<Derived,2>
00098 {
00099 typedef typename ei_plain_matrix_type<Derived>::type VectorType;
00100 inline static VectorType run(const Derived& src)
00101 { return VectorType(-ei_conj(src.y()), ei_conj(src.x())).normalized(); }
00102 };
00103
00111 template<typename Derived>
00112 typename MatrixBase<Derived>::PlainMatrixType
00113 MatrixBase<Derived>::unitOrthogonal() const
00114 {
00115 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
00116 return ei_unitOrthogonal_selector<Derived>::run(derived());
00117 }
00118
00119 #endif // EIGEN_ORTHOMETHODS_H