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, int _Options>
00047 class Hyperplane
00048 {
00049 public:
00050 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
00051 enum {
00052 AmbientDimAtCompileTime = _AmbientDim,
00053 Options = _Options
00054 };
00055 typedef _Scalar Scalar;
00056 typedef typename NumTraits<Scalar>::Real RealScalar;
00057 typedef DenseIndex Index;
00058 typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
00059 typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic
00060 ? Dynamic
00061 : Index(AmbientDimAtCompileTime)+1,1,Options> Coefficients;
00062 typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
00063 typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType;
00064
00066 inline explicit Hyperplane() {}
00067
00068 template<int OtherOptions>
00069 Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other)
00070 : m_coeffs(other.coeffs())
00071 {}
00072
00075 inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {}
00076
00080 inline Hyperplane(const VectorType& n, const VectorType& e)
00081 : m_coeffs(n.size()+1)
00082 {
00083 normal() = n;
00084 offset() = -n.dot(e);
00085 }
00086
00091 inline Hyperplane(const VectorType& n, Scalar d)
00092 : m_coeffs(n.size()+1)
00093 {
00094 normal() = n;
00095 offset() = d;
00096 }
00097
00101 static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
00102 {
00103 Hyperplane result(p0.size());
00104 result.normal() = (p1 - p0).unitOrthogonal();
00105 result.offset() = -p0.dot(result.normal());
00106 return result;
00107 }
00108
00112 static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
00113 {
00114 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
00115 Hyperplane result(p0.size());
00116 result.normal() = (p2 - p0).cross(p1 - p0).normalized();
00117 result.offset() = -p0.dot(result.normal());
00118 return result;
00119 }
00120
00125
00126 explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
00127 {
00128 normal() = parametrized.direction().unitOrthogonal();
00129 offset() = -parametrized.origin().dot(normal());
00130 }
00131
00132 ~Hyperplane() {}
00133
00135 inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); }
00136
00138 void normalize(void)
00139 {
00140 m_coeffs /= normal().norm();
00141 }
00142
00146 inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); }
00147
00151 inline Scalar absDistance(const VectorType& p) const { return internal::abs(signedDistance(p)); }
00152
00155 inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
00156
00160 inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); }
00161
00165 inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
00166
00170 inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
00171
00174 inline Scalar& offset() { return m_coeffs(dim()); }
00175
00179 inline const Coefficients& coeffs() const { return m_coeffs; }
00180
00184 inline Coefficients& coeffs() { return m_coeffs; }
00185
00192 VectorType intersection(const Hyperplane& other) const
00193 {
00194 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00195 Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
00196
00197
00198 if(internal::isMuchSmallerThan(det, Scalar(1)))
00199 {
00200 if(internal::abs(coeffs().coeff(1))>internal::abs(coeffs().coeff(0)))
00201 return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
00202 else
00203 return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
00204 }
00205 else
00206 {
00207 Scalar invdet = Scalar(1) / det;
00208 return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
00209 invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
00210 }
00211 }
00212
00219 template<typename XprType>
00220 inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
00221 {
00222 if (traits==Affine)
00223 normal() = mat.inverse().transpose() * normal();
00224 else if (traits==Isometry)
00225 normal() = mat * normal();
00226 else
00227 {
00228 eigen_assert("invalid traits value in Hyperplane::transform()");
00229 }
00230 return *this;
00231 }
00232
00240 template<int TrOptions>
00241 inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t,
00242 TransformTraits traits = Affine)
00243 {
00244 transform(t.linear(), traits);
00245 offset() -= normal().dot(t.translation());
00246 return *this;
00247 }
00248
00254 template<typename NewScalarType>
00255 inline typename internal::cast_return_type<Hyperplane,
00256 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const
00257 {
00258 return typename internal::cast_return_type<Hyperplane,
00259 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this);
00260 }
00261
00263 template<typename OtherScalarType,int OtherOptions>
00264 inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other)
00265 { m_coeffs = other.coeffs().template cast<Scalar>(); }
00266
00271 template<int OtherOptions>
00272 bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
00273 { return m_coeffs.isApprox(other.m_coeffs, prec); }
00274
00275 protected:
00276
00277 Coefficients m_coeffs;
00278 };
00279
00280 #endif // EIGEN_HYPERPLANE_H