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 <gael.guennebaud@inria.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 #ifndef EIGEN_HYPERPLANE_H
12 #define EIGEN_HYPERPLANE_H
13 
14 namespace Eigen {
15 
33 template <typename _Scalar, int _AmbientDim, int _Options>
34 class Hyperplane
35 {
36 public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
38  enum {
39  AmbientDimAtCompileTime = _AmbientDim,
40  Options = _Options
41  };
42  typedef _Scalar Scalar;
44  typedef DenseIndex Index;
47  ? Dynamic
51 
53  inline Hyperplane() {}
54 
55  template<int OtherOptions>
57  : m_coeffs(other.coeffs())
58  {}
59 
62  inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {}
63 
67  inline Hyperplane(const VectorType& n, const VectorType& e)
68  : m_coeffs(n.size()+1)
69  {
70  normal() = n;
71  offset() = -n.dot(e);
72  }
73 
78  inline Hyperplane(const VectorType& n, const Scalar& d)
79  : m_coeffs(n.size()+1)
80  {
81  normal() = n;
82  offset() = d;
83  }
84 
88  static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
89  {
90  Hyperplane result(p0.size());
91  result.normal() = (p1 - p0).unitOrthogonal();
92  result.offset() = -p0.dot(result.normal());
93  return result;
94  }
95 
99  static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
100  {
102  Hyperplane result(p0.size());
103  result.normal() = (p2 - p0).cross(p1 - p0).normalized();
104  result.offset() = -p0.dot(result.normal());
105  return result;
106  }
107 
112  // FIXME to be consitent with the rest this could be implemented as a static Through function ??
114  {
115  normal() = parametrized.direction().unitOrthogonal();
116  offset() = -parametrized.origin().dot(normal());
117  }
118 
120 
122  inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); }
123 
125  void normalize(void)
126  {
127  m_coeffs /= normal().norm();
128  }
129 
133  inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); }
134 
138  inline Scalar absDistance(const VectorType& p) const { using std::abs; return abs(signedDistance(p)); }
139 
142  inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
143 
147  inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); }
148 
152  inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
153 
157  inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
158 
161  inline Scalar& offset() { return m_coeffs(dim()); }
162 
166  inline const Coefficients& coeffs() const { return m_coeffs; }
167 
171  inline Coefficients& coeffs() { return m_coeffs; }
172 
179  VectorType intersection(const Hyperplane& other) const
180  {
181  using std::abs;
183  Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
184  // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
185  // whether the two lines are approximately parallel.
187  { // special case where the two lines are approximately parallel. Pick any point on the first line.
188  if(abs(coeffs().coeff(1))>abs(coeffs().coeff(0)))
189  return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
190  else
191  return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
192  }
193  else
194  { // general case
195  Scalar invdet = Scalar(1) / det;
196  return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
197  invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
198  }
199  }
200 
207  template<typename XprType>
209  {
210  if (traits==Affine)
211  normal() = mat.inverse().transpose() * normal();
212  else if (traits==Isometry)
213  normal() = mat * normal();
214  else
215  {
216  eigen_assert(0 && "invalid traits value in Hyperplane::transform()");
217  }
218  return *this;
219  }
220 
228  template<int TrOptions>
230  TransformTraits traits = Affine)
231  {
232  transform(t.linear(), traits);
233  offset() -= normal().dot(t.translation());
234  return *this;
235  }
236 
242  template<typename NewScalarType>
243  inline typename internal::cast_return_type<Hyperplane,
245  {
246  return typename internal::cast_return_type<Hyperplane,
248  }
249 
251  template<typename OtherScalarType,int OtherOptions>
253  { m_coeffs = other.coeffs().template cast<Scalar>(); }
254 
259  template<int OtherOptions>
261  { return m_coeffs.isApprox(other.m_coeffs, prec); }
262 
263 protected:
264 
265  Coefficients m_coeffs;
266 };
267 
268 } // end namespace Eigen
269 
270 #endif // EIGEN_HYPERPLANE_H
const NormalReturnType normal() const
Scalar signedDistance(const VectorType &p) const
Hyperplane(const Hyperplane< OtherScalarType, AmbientDimAtCompileTime, OtherOptions > &other)
const Block< const Coefficients, AmbientDimAtCompileTime, 1 > ConstNormalReturnType
NormalReturnType normal()
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
#define Hyperplane
Definition: All.h:57
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
Block< Coefficients, AmbientDimAtCompileTime, 1 > NormalReturnType
bool isMuchSmallerThan(const Scalar &x, const OtherScalar &y, typename NumTraits< Scalar >::Real precision=NumTraits< Scalar >::dummy_precision())
Hyperplane(const VectorType &n, const Scalar &d)
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)
VectorType intersection(const Hyperplane &other) const
const Coefficients & coeffs() const
Hyperplane(const Hyperplane< Scalar, AmbientDimAtCompileTime, OtherOptions > &other)
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
NumTraits< Scalar >::Real RealScalar
EIGEN_STRONG_INLINE const Scalar & coeff(Index rowId, Index colId) const
ConstNormalReturnType normal() const
Provides a generic way to set and pass user-specified options.
Definition: options.hpp:65
static Hyperplane Through(const VectorType &p0, const VectorType &p1)
VectorType projection(const VectorType &p) const
bool isApprox(const Hyperplane< Scalar, AmbientDimAtCompileTime, OtherOptions > &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
ConstTranslationPart translation() const
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: XprHelper.h:27
Matrix< Scalar, AmbientDimAtCompileTime, 1 > VectorType
Coefficients & coeffs()
Expression of a fixed-size or dynamic-size block.
Definition: Core/Block.h:102
ConstLinearPart linear() const
internal::cast_return_type< Hyperplane, Hyperplane< NewScalarType, AmbientDimAtCompileTime, Options > >::type cast() const
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
#define eigen_assert(x)
const internal::inverse_impl< Derived > inverse() const
Definition: Inverse.h:320
Hyperplane(const VectorType &n, const VectorType &e)
Matrix< Scalar, Index(AmbientDimAtCompileTime)==Dynamic?Dynamic:Index(AmbientDimAtCompileTime)+1, 1, Options > Coefficients
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Hyperplane & transform(const Transform< Scalar, AmbientDimAtCompileTime, Affine, TrOptions > &t, TransformTraits traits=Affine)
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