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_HYPERPLANE_H
00027 #define EIGEN_HYPERPLANE_H
00028
00046 template <typename _Scalar, int _AmbientDim>
00047 class Hyperplane
00048 {
00049 public:
00050 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
00051 enum { AmbientDimAtCompileTime = _AmbientDim };
00052 typedef _Scalar Scalar;
00053 typedef typename NumTraits<Scalar>::Real RealScalar;
00054 typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
00055 typedef Matrix<Scalar,int(AmbientDimAtCompileTime)==Dynamic
00056 ? Dynamic
00057 : int(AmbientDimAtCompileTime)+1,1> Coefficients;
00058 typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
00059
00061 inline explicit Hyperplane() {}
00062
00065 inline explicit Hyperplane(int _dim) : m_coeffs(_dim+1) {}
00066
00070 inline Hyperplane(const VectorType& n, const VectorType& e)
00071 : m_coeffs(n.size()+1)
00072 {
00073 normal() = n;
00074 offset() = -e.dot(n);
00075 }
00076
00081 inline Hyperplane(const VectorType& n, Scalar d)
00082 : m_coeffs(n.size()+1)
00083 {
00084 normal() = n;
00085 offset() = d;
00086 }
00087
00091 static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
00092 {
00093 Hyperplane result(p0.size());
00094 result.normal() = (p1 - p0).unitOrthogonal();
00095 result.offset() = -result.normal().dot(p0);
00096 return result;
00097 }
00098
00102 static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
00103 {
00104 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
00105 Hyperplane result(p0.size());
00106 result.normal() = (p2 - p0).cross(p1 - p0).normalized();
00107 result.offset() = -result.normal().dot(p0);
00108 return result;
00109 }
00110
00115
00116 explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
00117 {
00118 normal() = parametrized.direction().unitOrthogonal();
00119 offset() = -normal().dot(parametrized.origin());
00120 }
00121
00122 ~Hyperplane() {}
00123
00125 inline int dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : AmbientDimAtCompileTime; }
00126
00128 void normalize(void)
00129 {
00130 m_coeffs /= normal().norm();
00131 }
00132
00136 inline Scalar signedDistance(const VectorType& p) const { return p.dot(normal()) + offset(); }
00137
00141 inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); }
00142
00145 inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
00146
00150 inline const NormalReturnType normal() const { return NormalReturnType(m_coeffs,0,0,dim(),1); }
00151
00155 inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
00156
00160 inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
00161
00164 inline Scalar& offset() { return m_coeffs(dim()); }
00165
00169 inline const Coefficients& coeffs() const { return m_coeffs; }
00170
00174 inline Coefficients& coeffs() { return m_coeffs; }
00175
00182 VectorType intersection(const Hyperplane& other)
00183 {
00184 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00185 Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
00186
00187
00188 if(ei_isMuchSmallerThan(det, Scalar(1)))
00189 {
00190 if(ei_abs(coeffs().coeff(1))>ei_abs(coeffs().coeff(0)))
00191 return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
00192 else
00193 return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
00194 }
00195 else
00196 {
00197 Scalar invdet = Scalar(1) / det;
00198 return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
00199 invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
00200 }
00201 }
00202
00209 template<typename XprType>
00210 inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
00211 {
00212 if (traits==Affine)
00213 normal() = mat.inverse().transpose() * normal();
00214 else if (traits==Isometry)
00215 normal() = mat * normal();
00216 else
00217 {
00218 ei_assert("invalid traits value in Hyperplane::transform()");
00219 }
00220 return *this;
00221 }
00222
00230 inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime>& t,
00231 TransformTraits traits = Affine)
00232 {
00233 transform(t.linear(), traits);
00234 offset() -= t.translation().dot(normal());
00235 return *this;
00236 }
00237
00243 template<typename NewScalarType>
00244 inline typename ei_cast_return_type<Hyperplane,
00245 Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
00246 {
00247 return typename ei_cast_return_type<Hyperplane,
00248 Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
00249 }
00250
00252 template<typename OtherScalarType>
00253 inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime>& other)
00254 { m_coeffs = other.coeffs().template cast<Scalar>(); }
00255
00260 bool isApprox(const Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00261 { return m_coeffs.isApprox(other.m_coeffs, prec); }
00262
00263 protected:
00264
00265 Coefficients m_coeffs;
00266 };
00267
00268 #endif // EIGEN_HYPERPLANE_H