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
00029 namespace Eigen {
00030
00048 template <typename _Scalar, int _AmbientDim, int _Options>
00049 class Hyperplane
00050 {
00051 public:
00052 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
00053 enum {
00054 AmbientDimAtCompileTime = _AmbientDim,
00055 Options = _Options
00056 };
00057 typedef _Scalar Scalar;
00058 typedef typename NumTraits<Scalar>::Real RealScalar;
00059 typedef DenseIndex Index;
00060 typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
00061 typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic
00062 ? Dynamic
00063 : Index(AmbientDimAtCompileTime)+1,1,Options> Coefficients;
00064 typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
00065 typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType;
00066
00068 inline explicit Hyperplane() {}
00069
00070 template<int OtherOptions>
00071 Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other)
00072 : m_coeffs(other.coeffs())
00073 {}
00074
00077 inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {}
00078
00082 inline Hyperplane(const VectorType& n, const VectorType& e)
00083 : m_coeffs(n.size()+1)
00084 {
00085 normal() = n;
00086 offset() = -n.dot(e);
00087 }
00088
00093 inline Hyperplane(const VectorType& n, Scalar d)
00094 : m_coeffs(n.size()+1)
00095 {
00096 normal() = n;
00097 offset() = d;
00098 }
00099
00103 static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
00104 {
00105 Hyperplane result(p0.size());
00106 result.normal() = (p1 - p0).unitOrthogonal();
00107 result.offset() = -p0.dot(result.normal());
00108 return result;
00109 }
00110
00114 static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
00115 {
00116 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
00117 Hyperplane result(p0.size());
00118 result.normal() = (p2 - p0).cross(p1 - p0).normalized();
00119 result.offset() = -p0.dot(result.normal());
00120 return result;
00121 }
00122
00127
00128 explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
00129 {
00130 normal() = parametrized.direction().unitOrthogonal();
00131 offset() = -parametrized.origin().dot(normal());
00132 }
00133
00134 ~Hyperplane() {}
00135
00137 inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); }
00138
00140 void normalize(void)
00141 {
00142 m_coeffs /= normal().norm();
00143 }
00144
00148 inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); }
00149
00153 inline Scalar absDistance(const VectorType& p) const { return internal::abs(signedDistance(p)); }
00154
00157 inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
00158
00162 inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); }
00163
00167 inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
00168
00172 inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
00173
00176 inline Scalar& offset() { return m_coeffs(dim()); }
00177
00181 inline const Coefficients& coeffs() const { return m_coeffs; }
00182
00186 inline Coefficients& coeffs() { return m_coeffs; }
00187
00194 VectorType intersection(const Hyperplane& other) const
00195 {
00196 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00197 Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
00198
00199
00200 if(internal::isMuchSmallerThan(det, Scalar(1)))
00201 {
00202 if(internal::abs(coeffs().coeff(1))>internal::abs(coeffs().coeff(0)))
00203 return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
00204 else
00205 return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
00206 }
00207 else
00208 {
00209 Scalar invdet = Scalar(1) / det;
00210 return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
00211 invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
00212 }
00213 }
00214
00221 template<typename XprType>
00222 inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
00223 {
00224 if (traits==Affine)
00225 normal() = mat.inverse().transpose() * normal();
00226 else if (traits==Isometry)
00227 normal() = mat * normal();
00228 else
00229 {
00230 eigen_assert(0 && "invalid traits value in Hyperplane::transform()");
00231 }
00232 return *this;
00233 }
00234
00242 template<int TrOptions>
00243 inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t,
00244 TransformTraits traits = Affine)
00245 {
00246 transform(t.linear(), traits);
00247 offset() -= normal().dot(t.translation());
00248 return *this;
00249 }
00250
00256 template<typename NewScalarType>
00257 inline typename internal::cast_return_type<Hyperplane,
00258 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const
00259 {
00260 return typename internal::cast_return_type<Hyperplane,
00261 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this);
00262 }
00263
00265 template<typename OtherScalarType,int OtherOptions>
00266 inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other)
00267 { m_coeffs = other.coeffs().template cast<Scalar>(); }
00268
00273 template<int OtherOptions>
00274 bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
00275 { return m_coeffs.isApprox(other.m_coeffs, prec); }
00276
00277 protected:
00278
00279 Coefficients m_coeffs;
00280 };
00281
00282 }
00283
00284 #endif // EIGEN_HYPERPLANE_H