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
15namespace 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
38template<typename _Scalar> class AlignedVector3;
39
40namespace internal {
41template<typename _Scalar> struct traits<AlignedVector3<_Scalar> >
42  : traits<Matrix<_Scalar,3,1,0,4,1> >
43{
44};
45}
46
47template<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, 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
191