Plane(ATy_ A, BTy_ B, CTy_ C, DTy_ D) : normal(A, B, C)
{
ValueType nlen = static_cast<ValueType>(1) / static_cast<ValueType>(normal.length());
distanceConstant = -D * nlen;
// renormalize normal
normal = normal * nlen;
}
/**
* @brief
* Create a plane from a plane equation \f$a + b + c + d = 0\f$.
* @param a, b, c, d
* the plane equation
* @remark
* Given a plane equation \f$a + b + c + d = 0\f$, the normal-distance form
* \f$(\hat{n}',d')\f$ is defined as \f$\hat{n}' = \frac{\vec{n}}{|\vec{n}'|}\f$
* and \f$d' = \frac{d}{|\vec{n}'|}\f$ where \f$\vec{n}' = (a,b,c)\f$.
*/
Plane3(const ScalarType& a, const ScalarType& b, const ScalarType& c, const ScalarType& d)
: _n(a, b, c), _d(d) {
auto l = _n.length();
if (0.0f == l) {
throw std::domain_error("normal vector is zero vector");
}
auto il = 1.0f / l;
_n *= il;
_d *= il;
}
Vector4d plane_eq() const
{
Vector4d eq;
double s = 1.0 / normal.length();//sqrt( normal.x * normal.x + normal.y * normal.y + normal.z * normal.z );
Vector3d v(normal * distanceConstant);
eq.x = normal.x * s;
eq.y = normal.y * s;
eq.z = normal.z * s;
eq.w = -( eq.x * v.x + eq.y * v.y + eq.z * v.z );
return eq;
}
