Eigen2Support/Geometry/Hyperplane.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
5 // Copyright (C) 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 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
12 
13 namespace Eigen {
14 
32 template <typename _Scalar, int _AmbientDim>
34 {
35 public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
37  enum { AmbientDimAtCompileTime = _AmbientDim };
38  typedef _Scalar Scalar;
42  ? Dynamic
45 
47  inline Hyperplane() {}
48 
51  inline explicit Hyperplane(int _dim) : m_coeffs(_dim+1) {}
52 
56  inline Hyperplane(const VectorType& n, const VectorType& e)
57  : m_coeffs(n.size()+1)
58  {
59  normal() = n;
60  offset() = -e.eigen2_dot(n);
61  }
62 
67  inline Hyperplane(const VectorType& n, Scalar d)
68  : m_coeffs(n.size()+1)
69  {
70  normal() = n;
71  offset() = d;
72  }
73 
77  static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
78  {
79  Hyperplane result(p0.size());
80  result.normal() = (p1 - p0).unitOrthogonal();
81  result.offset() = -result.normal().eigen2_dot(p0);
82  return result;
83  }
84 
88  static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
89  {
91  Hyperplane result(p0.size());
92  result.normal() = (p2 - p0).cross(p1 - p0).normalized();
93  result.offset() = -result.normal().eigen2_dot(p0);
94  return result;
95  }
96 
101  // FIXME to be consitent with the rest this could be implemented as a static Through function ??
103  {
104  normal() = parametrized.direction().unitOrthogonal();
105  offset() = -normal().eigen2_dot(parametrized.origin());
106  }
107 
109 
111  inline int dim() const { return int(AmbientDimAtCompileTime)==Dynamic ? m_coeffs.size()-1 : int(AmbientDimAtCompileTime); }
112 
114  void normalize(void)
115  {
116  m_coeffs /= normal().norm();
117  }
118 
122  inline Scalar signedDistance(const VectorType& p) const { return p.eigen2_dot(normal()) + offset(); }
123 
127  inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); }
128 
131  inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
132 
136  inline const NormalReturnType normal() const { return NormalReturnType(*const_cast<Coefficients*>(&m_coeffs),0,0,dim(),1); }
137 
141  inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
142 
146  inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
147 
150  inline Scalar& offset() { return m_coeffs(dim()); }
151 
155  inline const Coefficients& coeffs() const { return m_coeffs; }
156 
160  inline Coefficients& coeffs() { return m_coeffs; }
161 
168  VectorType intersection(const Hyperplane& other)
169  {
171  Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
172  // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
173  // whether the two lines are approximately parallel.
174  if(ei_isMuchSmallerThan(det, Scalar(1)))
175  { // special case where the two lines are approximately parallel. Pick any point on the first line.
176  if(ei_abs(coeffs().coeff(1))>ei_abs(coeffs().coeff(0)))
177  return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
178  else
179  return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
180  }
181  else
182  { // general case
183  Scalar invdet = Scalar(1) / det;
184  return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
185  invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
186  }
187  }
188 
195  template<typename XprType>
197  {
198  if (traits==Affine)
199  normal() = mat.inverse().transpose() * normal();
200  else if (traits==Isometry)
201  normal() = mat * normal();
202  else
203  {
204  ei_assert("invalid traits value in Hyperplane::transform()");
205  }
206  return *this;
207  }
208 
217  TransformTraits traits = Affine)
218  {
219  transform(t.linear(), traits);
220  offset() -= t.translation().eigen2_dot(normal());
221  return *this;
222  }
223 
229  template<typename NewScalarType>
230  inline typename internal::cast_return_type<Hyperplane,
232  {
233  return typename internal::cast_return_type<Hyperplane,
235  }
236 
238  template<typename OtherScalarType>
240  { m_coeffs = other.coeffs().template cast<Scalar>(); }
241 
246  bool isApprox(const Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
247  { return m_coeffs.isApprox(other.m_coeffs, prec); }
248 
249 protected:
250 
252 };
253 
254 } // end namespace Eigen
Hyperplane & transform(const Transform< Scalar, AmbientDimAtCompileTime > &t, TransformTraits traits=Affine)
const NormalReturnType normal() const
Scalar signedDistance(const VectorType &p) const
#define ei_assert
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
Block< Coefficients, AmbientDimAtCompileTime, 1 > NormalReturnType
Hyperplane(const Hyperplane< OtherScalarType, AmbientDimAtCompileTime > &other)
Hyperplane & transform(const MatrixBase< XprType > &mat, TransformTraits traits=Affine)
static Hyperplane Through(const VectorType &p0, const VectorType &p1, const VectorType &p2)
Hyperplane(const ParametrizedLine< Scalar, AmbientDimAtCompileTime > &parametrized)
const Coefficients & coeffs() const
NumTraits< Scalar >::Real RealScalar
EIGEN_STRONG_INLINE const Scalar & coeff(Index rowId, Index colId) const
Matrix< Scalar, int(AmbientDimAtCompileTime)==Dynamic?Dynamic:int(AmbientDimAtCompileTime)+1, 1 > Coefficients
bool ei_isMuchSmallerThan(const Scalar &x, const OtherScalar &y, typename NumTraits< Scalar >::Real precision=NumTraits< Scalar >::dummy_precision())
Hyperplane(const VectorType &n, Scalar d)
static Hyperplane Through(const VectorType &p0, const VectorType &p1)
VectorType projection(const VectorType &p) const
VectorType intersection(const Hyperplane &other)
ConstTranslationPart translation() const
Matrix< Scalar, AmbientDimAtCompileTime, 1 > VectorType
NumTraits< T >::Real ei_abs(const T &x)
Expression of a fixed-size or dynamic-size block.
Definition: Core/Block.h:102
internal::cast_return_type< Hyperplane, Hyperplane< NewScalarType, AmbientDimAtCompileTime > >::type cast() const
ConstLinearPart linear() const
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
const internal::inverse_impl< Derived > inverse() const
Definition: Inverse.h:320
bool isApprox(const Hyperplane &other, typename NumTraits< Scalar >::Real prec=precision< Scalar >()) const
Hyperplane(const VectorType &n, const VectorType &e)
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Represents an homogeneous transformation in a N dimensional space.
#define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(TYPE, SIZE)
Definition: StaticAssert.h:141
Scalar absDistance(const VectorType &p) const


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:41