1 | // This file is part of Eigen, a lightweight C++ template library
|
---|
2 | // for linear algebra.
|
---|
3 | //
|
---|
4 | // Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
|
---|
5 | //
|
---|
6 | // This Source Code Form is subject to the terms of the Mozilla
|
---|
7 | // Public License v. 2.0. If a copy of the MPL was not distributed
|
---|
8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
---|
9 |
|
---|
10 | #ifndef EIGEN_ALIGNED_VECTOR3
|
---|
11 | #define EIGEN_ALIGNED_VECTOR3
|
---|
12 |
|
---|
13 | #include <Eigen/Geometry>
|
---|
14 |
|
---|
15 | namespace Eigen {
|
---|
16 |
|
---|
17 | /**
|
---|
18 | * \defgroup AlignedVector3_Module Aligned vector3 module
|
---|
19 | *
|
---|
20 | * \code
|
---|
21 | * #include <unsupported/Eigen/AlignedVector3>
|
---|
22 | * \endcode
|
---|
23 | */
|
---|
24 | //@{
|
---|
25 |
|
---|
26 |
|
---|
27 | /** \class AlignedVector3
|
---|
28 | *
|
---|
29 | * \brief A vectorization friendly 3D vector
|
---|
30 | *
|
---|
31 | * This class represents a 3D vector internally using a 4D vector
|
---|
32 | * such that vectorization can be seamlessly enabled. Of course,
|
---|
33 | * the same result can be achieved by directly using a 4D vector.
|
---|
34 | * This class makes this process simpler.
|
---|
35 | *
|
---|
36 | */
|
---|
37 | // TODO specialize Cwise
|
---|
38 | template<typename _Scalar> class AlignedVector3;
|
---|
39 |
|
---|
40 | namespace internal {
|
---|
41 | template<typename _Scalar> struct traits<AlignedVector3<_Scalar> >
|
---|
42 | : traits<Matrix<_Scalar,3,1,0,4,1> >
|
---|
43 | {
|
---|
44 | };
|
---|
45 | }
|
---|
46 |
|
---|
47 | template<typename _Scalar> class AlignedVector3
|
---|
48 | : public MatrixBase<AlignedVector3<_Scalar> >
|
---|
49 | {
|
---|
50 | typedef Matrix<_Scalar,4,1> CoeffType;
|
---|
51 | CoeffType m_coeffs;
|
---|
52 | public:
|
---|
53 |
|
---|
54 | typedef MatrixBase<AlignedVector3<_Scalar> > Base;
|
---|
55 | EIGEN_DENSE_PUBLIC_INTERFACE(AlignedVector3)
|
---|
56 | using Base::operator*;
|
---|
57 |
|
---|
58 | inline Index rows() const { return 3; }
|
---|
59 | inline Index cols() const { return 1; }
|
---|
60 |
|
---|
61 | inline const Scalar& coeff(Index row, Index col) const
|
---|
62 | { return m_coeffs.coeff(row, col); }
|
---|
63 |
|
---|
64 | inline Scalar& coeffRef(Index row, Index col)
|
---|
65 | { return m_coeffs.coeffRef(row, col); }
|
---|
66 |
|
---|
67 | inline const Scalar& coeff(Index index) const
|
---|
68 | { return m_coeffs.coeff(index); }
|
---|
69 |
|
---|
70 | inline Scalar& coeffRef(Index index)
|
---|
71 | { return m_coeffs.coeffRef(index);}
|
---|
72 |
|
---|
73 |
|
---|
74 | inline AlignedVector3(const Scalar& x, const Scalar& y, const Scalar& z)
|
---|
75 | : m_coeffs(x, y, z, Scalar(0))
|
---|
76 | {}
|
---|
77 |
|
---|
78 | inline AlignedVector3(const AlignedVector3& other)
|
---|
79 | : Base(), m_coeffs(other.m_coeffs)
|
---|
80 | {}
|
---|
81 |
|
---|
82 | template<typename XprType, int Size=XprType::SizeAtCompileTime>
|
---|
83 | struct generic_assign_selector {};
|
---|
84 |
|
---|
85 | template<typename XprType> struct generic_assign_selector<XprType,4>
|
---|
86 | {
|
---|
87 | inline static void run(AlignedVector3& dest, const XprType& src)
|
---|
88 | {
|
---|
89 | dest.m_coeffs = src;
|
---|
90 | }
|
---|
91 | };
|
---|
92 |
|
---|
93 | template<typename XprType> struct generic_assign_selector<XprType,3>
|
---|
94 | {
|
---|
95 | inline static void run(AlignedVector3& dest, const XprType& src)
|
---|
96 | {
|
---|
97 | dest.m_coeffs.template head<3>() = src;
|
---|
98 | dest.m_coeffs.w() = Scalar(0);
|
---|
99 | }
|
---|
100 | };
|
---|
101 |
|
---|
102 | template<typename Derived>
|
---|
103 | inline explicit AlignedVector3(const MatrixBase<Derived>& other)
|
---|
104 | {
|
---|
105 | generic_assign_selector<Derived>::run(*this,other.derived());
|
---|
106 | }
|
---|
107 |
|
---|
108 | inline AlignedVector3& operator=(const AlignedVector3& other)
|
---|
109 | { m_coeffs = other.m_coeffs; return *this; }
|
---|
110 |
|
---|
111 |
|
---|
112 | inline AlignedVector3 operator+(const AlignedVector3& other) const
|
---|
113 | { return AlignedVector3(m_coeffs + other.m_coeffs); }
|
---|
114 |
|
---|
115 | inline AlignedVector3& operator+=(const AlignedVector3& other)
|
---|
116 | { m_coeffs += other.m_coeffs; return *this; }
|
---|
117 |
|
---|
118 | inline AlignedVector3 operator-(const AlignedVector3& other) const
|
---|
119 | { return AlignedVector3(m_coeffs - other.m_coeffs); }
|
---|
120 |
|
---|
121 | inline AlignedVector3 operator-=(const AlignedVector3& other)
|
---|
122 | { m_coeffs -= other.m_coeffs; return *this; }
|
---|
123 |
|
---|
124 | inline AlignedVector3 operator*(const Scalar& s) const
|
---|
125 | { return AlignedVector3(m_coeffs * s); }
|
---|
126 |
|
---|
127 | inline friend AlignedVector3 operator*(const Scalar& s,const AlignedVector3& vec)
|
---|
128 | { return AlignedVector3(s * vec.m_coeffs); }
|
---|
129 |
|
---|
130 | inline AlignedVector3& operator*=(const Scalar& s)
|
---|
131 | { m_coeffs *= s; return *this; }
|
---|
132 |
|
---|
133 | inline AlignedVector3 operator/(const Scalar& s) const
|
---|
134 | { return AlignedVector3(m_coeffs / s); }
|
---|
135 |
|
---|
136 | inline AlignedVector3& operator/=(const Scalar& s)
|
---|
137 | { m_coeffs /= s; return *this; }
|
---|
138 |
|
---|
139 | inline Scalar dot(const AlignedVector3& other) const
|
---|
140 | {
|
---|
141 | eigen_assert(m_coeffs.w()==Scalar(0));
|
---|
142 | eigen_assert(other.m_coeffs.w()==Scalar(0));
|
---|
143 | return m_coeffs.dot(other.m_coeffs);
|
---|
144 | }
|
---|
145 |
|
---|
146 | inline void normalize()
|
---|
147 | {
|
---|
148 | m_coeffs /= norm();
|
---|
149 | }
|
---|
150 |
|
---|
151 | inline AlignedVector3 normalized()
|
---|
152 | {
|
---|
153 | return AlignedVector3(m_coeffs / norm());
|
---|
154 | }
|
---|
155 |
|
---|
156 | inline Scalar sum() const
|
---|
157 | {
|
---|
158 | eigen_assert(m_coeffs.w()==Scalar(0));
|
---|
159 | return m_coeffs.sum();
|
---|
160 | }
|
---|
161 |
|
---|
162 | inline Scalar squaredNorm() const
|
---|
163 | {
|
---|
164 | eigen_assert(m_coeffs.w()==Scalar(0));
|
---|
165 | return m_coeffs.squaredNorm();
|
---|
166 | }
|
---|
167 |
|
---|
168 | inline Scalar norm() const
|
---|
169 | {
|
---|
170 | using std::sqrt;
|
---|
171 | return sqrt(squaredNorm());
|
---|
172 | }
|
---|
173 |
|
---|
174 | inline AlignedVector3 cross(const AlignedVector3& other) const
|
---|
175 | {
|
---|
176 | return AlignedVector3(m_coeffs.cross3(other.m_coeffs));
|
---|
177 | }
|
---|
178 |
|
---|
179 | template<typename Derived>
|
---|
180 | inline bool isApprox(const MatrixBase<Derived>& other, const RealScalar& eps=NumTraits<Scalar>::dummy_precision()) const
|
---|
181 | {
|
---|
182 | return m_coeffs.template head<3>().isApprox(other,eps);
|
---|
183 | }
|
---|
184 | };
|
---|
185 |
|
---|
186 | //@}
|
---|
187 |
|
---|
188 | }
|
---|
189 |
|
---|
190 | #endif // EIGEN_ALIGNED_VECTOR3
|
---|