/**
* @brief
* Construct a plane with a point and a normal.
* @param p
* the point
* @param n
* the normal
* @throw std::domain_error
* if the normal vector is the zero vector
* @remark
* The plane normal is normalized if necessary.
* @remark
* Let \f$X\f$ be the point and \f$\vec{n}\f$ the unnormalized plane normal, then the plane equation is given by
* \f{align*}{
* \hat{n} \cdot P + d = 0, \hat{n}=\frac{\vec{n}}{|\vec{n}|}, d = -\left(\hat{n} \cdot X\right)
* \f}
* \f$X\f$ is on the plane as
* \f{align*}{
* &\hat{n} \cdot X + d\\
* =&\hat{n} \cdot X + -(\hat{n} \cdot X)\\
* =&\hat{n} \cdot X - \hat{n} \cdot X\\
* =&0
* \f}
*/
Plane3(const VectorType& p, const VectorType& n)
: _n(n), _d(0.0f) {
if (_n.normalize() == 0.0f) {
throw std::domain_error("normal vector is zero vector");
}
_d = -_n.dot(p);
}
Plane::Plane(VectorType& normal, DREAM3D::SurfaceMesh::Vert_t& x) :
m_normal(normal),
m_center(x),
m_d(normal.dot(x))
{
ensure_invariant();
}
/**
* @brief
* Construct a plane with three non-collinear points.
* @param a, b, c
* the point
* @throw std::domain_error
* if the points are collinear
* @remark
* Assume \f$a\f$, \f$b\f$ and \f$c\f$ are not collinear. Let \f$u = b - a\f$,
* \f$v = c - a\f$, \f$n = u \times v\f$, \f$\hat{n} = \frac{n}{|n|}\f$ and
* \f$d = -\left(\hat{n} \cdot a\right)\f$.
* @remark
* We show that \f$a\f$, \f$b\f$ and \f$c\f$ are on the plane given by the
* equation \f$\hat{n} \cdot p + d = 0\f$. To show this for \f$a\f$, rewrite
* the plane equation to
* \f{eqnarray*}{
* \hat{n} \cdot p + -(\hat{n} \cdot a) = 0\\
* \f}
* shows for \f$p=a\f$ immediatly that \f$a\f$ is on the plane.
* @remark
* To show that \f$b\f$ and \f$c\f$ are on the plane, the plane equation is
* rewritten yet another time using the bilinearity property of the dot product
* and the definitions of \f$d\f$ and \f$\hat{n}\f$ its
* \f{align*}{
* &\hat{n} \cdot p + -(\hat{n} \cdot a) \;\;\text{Def. of } d\\
* =&\hat{n} \cdot p + \hat{n} \cdot (-a) \;\;\text{Bilinearity of the dot product}\\
* =&\hat{n} \cdot (p - a) \;\;\text{Bilinearity of the dot product}\\
* =&\frac{n}{|n|} \cdot (p - a) \;\;\text{Def. of } \hat{n}\\
* =&(\frac{1}{|n|}n) \cdot (p - a) \;\;\\
* =&\frac{1}{|n|}(n \cdot (p - a)) \;\;\text{Compatibility of the dot product w. scalar multiplication}\\
* =&n \cdot (p - a) \;\;\\
* =&(u \times v) \cdot (p - a) \;\;\text{Def. of } n
* \f}
* Let \f$p = b\f$ then
* \f{align*}{
* &(u \times v) \cdot (b - a) \;\text{Def. of } u\\
* =&(u \times v) \cdot u\\
* =&0
* \f}
* or let \f$p = c\f$ then
* \f{align*}{
* &(u \times v) \cdot (c - a) \;\text{Def. of } u\\
* =&(u \times v) \cdot v\\
* =&0
* \f}
* as \f$u \times v\f$ is orthogonal to both \f$u\f$ and \f$v\f$.
* This shows that \f$b\f$ and \f$c\f$ are on the plane.
*/
Plane3(const VectorType& a, const VectorType& b, const VectorType& c) {
auto u = b - a;
if (u == VectorType::zero()) {
throw std::domain_error("b = a");
}
auto v = c - a;
if (u == VectorType::zero()) {
throw std::domain_error("c = a");
}
_n = u.cross(v);
if (0.0f == _n.normalize()) {
/* u x v = 0 is only possible for u,v != 0 if u = v and thus b = c. */
throw std::domain_error("b = c");
}
_d = -_n.dot(a);
}
double gaussian_kl_divergence( const VectorType& mean0, const MatrixType& info0,
const VectorType& mean1, const MatrixType& info1 )
{
Eigen::LDLT<MatrixType> ldl0( info0 );
Eigen::LDLT<MatrixType> ldl1( info1 );
double det0 = ldl0.vectorD().prod();
double det1 = ldl1.vectorD().prod();
double detDiff = std::log( det0 ) - std::log( det1 );
MatrixType invProd = ldl0.solve( info1 );
VectorType meanDiff = mean1 - mean0;
double quadTerm = meanDiff.dot( info1 * meanDiff );
return 0.5 * ( invProd.trace() + quadTerm - mean0.size() + detDiff );
}
static ScalarType dot(std::size_t /*N*/, VectorType const & x, VectorType const & y)
{ return x.dot(y); }
/**
* @remark
* The first (slow) method to compute the translation of a plane \f$\hat{n} \cdot P + d = 0\f$
* is to compute a point on the plane, translate the point, and compute from the new point and
* and the old plane normal the new plane:
* To translate a plane \f$\hat{n} \cdot P + d = 0\f$, compute a point on the plane
* \f$X\f$ (i.e. a point \f$\hat{n} \cdot X + d = 0\f$) by
* \f{align*}{
* X = (-d) \cdot \hat{n}
* \f}
* Translate the point \f$X\f$ by \f$\vec{t}\f$ into a new point \f$X'\f$:
* \f{align*}{
* X' = X + \vec{t}
* \f}
* and compute the new plane
* \f{align*}{
* \hat{n} \cdot P + d' = 0, d' = -\left(\hat{n} \cdot X'\right)
* \f}
* @remark
* The above method is not the fastest method. Observing that the old and the new plane equation only
* differ by \f$d\f$ and \f$d'\f$, a faster method of translating a plane can be devised by computing
* \f$d'\f$ directly. Expanding \f$d'\f$ gives
* \f{align*}{
* d' =& -\left(\hat{n} \cdot X'\right)\\
* =& -\left[\hat{n} \cdot \left((-d) \cdot \hat{n} + \vec{t}\right)\right]\\
* =& -\left[(-d) \cdot \hat{n} \cdot \hat{n} + \hat{n} \cdot \vec{t}\right]\\
* =& -\left[-d + \hat{n} \cdot \vec{t}\right]\\
* =& d - \hat{n} \cdot \vec{t}
* \f}
* The new plane can then be computed by
* \f{align*}{
* \hat{n} \cdot P + d' = 0, d' = -d - \hat{n} \cdot \vec{t}
* \f}
* @param t
* the translation vector
* @copydoc Ego::Math::translatable
*/
void translate(const VectorType& t) override {
_d -= _n.dot(t);
}
/**
* @brief
* Get the distance of a point from this plane.
* @param point
* the point
* @return
* the distance of the point from the plane.
* The point is in the positive (negative) half-space of the plane if the distance is positive (negative).
* Otherwise the point is on the plane.
* @remark
* Let \f$\hat{n} \cdot P + d = 0\f$ be a plane and \f$v\f$ be some point.
* We claim that \f$d'=\hat{n} \cdot v + d\f$ is the signed distance of the
* point \f$v\f$ from the plane.
*
* To show this, assume \f$v\f$ is not in the plane. Then there
* exists a single point \f$u\f$ on the plane which is closest to
* \f$v\f$ such that \f$v\f$ can be expressed by translating \f$u\f$
* along the plane normal by the signed distance \f$d'\f$ from
* \f$u\f$ to \f$v\f$ i.e. \f$v = u + d' \hat{n}\f$. Obviously,
* if \f$v\f$ is in the positive (negative) half-space of the plane, then
* \f$d'>0\f$ (\f$d' < 0\f$). We obtain now
* \f{align*}{
* &\hat{n} \cdot v + d\\
* = &\hat{n} \cdot (u + d' \hat{n}) + d\\
* = &\hat{n} \cdot u + d' (\hat{n} \cdot \hat{n}) + d\\
* = &\hat{n} \cdot u + d' + d\\
* = &\hat{n} \cdot u + d + d'\\
* \f}
* However, as \f$u\f$ is on the plane
* \f{align*}{
* = &\hat{n} \cdot u + d + d'\\
* = &d'
* \f}
*/
ScalarType distance(const VectorType& point) const {
return _n.dot(point) + _d;
}
double log_like(VectorType arg_vec){
return - arg_vec.dot(arg_vec);
}
Plane::Plane(VectorType& normal, VectorType& x)
: m_normal(normal), m_center(x), m_d(normal.dot(x))
{
ensure_invariant();
}
inline void Plane<N, T>::setNormalAndPoint(const VectorType& normal, const VectorType& point)
{
this->normal = normal;
dist = -(normal.dot(point));
}
NumericType linear_kernel::eval(const VectorType & x, const VectorType & y) const
{
return x.dot(y);
}
