vcglib/vcg/math/quadric.h

239 lines
7.4 KiB
C++

/****************************************************************************
* VCGLib o o *
* Visual and Computer Graphics Library o o *
* _ O _ *
* Copyright(C) 2004-2016 \/)\/ *
* Visual Computing Lab /\/| *
* ISTI - Italian National Research Council | *
* \ *
* All rights reserved. *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
* for more details. *
* *
****************************************************************************/
#ifndef __VCGLIB_QUADRIC
#define __VCGLIB_QUADRIC
#include <vcg/space/point3.h>
#include <vcg/space/plane3.h>
#include <vcg/math/matrix33.h>
#include <Eigen/Core>
namespace vcg {
namespace math {
/*
* This class encode a quadric function
* f(x) = xAx +bx + c
* where A is a symmetric 3x3 matrix, b a vector and c a scalar constant.
*/
template<typename _ScalarType>
class Quadric
{
public:
typedef _ScalarType ScalarType;
ScalarType a[6]; // Symmetric Matrix 3x3 : a11 a12 a13 a22 a23 a33
ScalarType b[3]; // Vector r3
ScalarType c; // Scalar (-1 means null/un-initialized quadric)
inline Quadric() { c = -1; }
bool IsValid() const { return c>=0; }
void SetInvalid() { c = -1.0; }
// Initialize the quadric to keep the squared distance from a given Plane
template< class PlaneType >
void ByPlane( const PlaneType & p )
{
a[0] = (ScalarType)p.Direction()[0]*p.Direction()[0]; // a11
a[1] = (ScalarType)p.Direction()[1]*p.Direction()[0]; // a12 (=a21)
a[2] = (ScalarType)p.Direction()[2]*p.Direction()[0]; // a13 (=a31)
a[3] = (ScalarType)p.Direction()[1]*p.Direction()[1]; // a22
a[4] = (ScalarType)p.Direction()[2]*p.Direction()[1]; // a23 (=a32)
a[5] = (ScalarType)p.Direction()[2]*p.Direction()[2]; // a33
b[0] = (ScalarType)(-2.0)*p.Offset()*p.Direction()[0];
b[1] = (ScalarType)(-2.0)*p.Offset()*p.Direction()[1];
b[2] = (ScalarType)(-2.0)*p.Offset()*p.Direction()[2];
c = (ScalarType)p.Offset()*p.Offset();
}
/*
* Initializes the quadric as the squared distance from a given line.
* Note that this code also works for a vcg::Ray<T>, even though the (squared) distance
* from a ray is different "before" its origin.
*/
template< class LineType >
void ByLine( const LineType & r ) // Init dato un raggio
{
ScalarType K = (ScalarType)(r.Origin()*r.Direction());
a[0] = (ScalarType)1.0-r.Direction()[0]*r.Direction()[0]; // a11
a[1] = (ScalarType)-r.Direction()[0]*r.Direction()[1]; // a12 (=a21)
a[2] = (ScalarType)-r.Direction()[0]*r.Direction()[2]; // a13 (=a31)
a[3] = (ScalarType)1.0-r.Direction()[1]*r.Direction()[1]; // a22
a[4] = (ScalarType)-r.Direction()[1]*r.Direction()[2]; // a23 (=a32)
a[5] = (ScalarType)1.0-r.Direction()[2]*r.Direction()[2]; // a33
b[0] = (ScalarType)2.0*(r.Direction()[0]*K - r.Origin()[0]);
b[1] = (ScalarType)2.0*(r.Direction()[1]*K - r.Origin()[1]);
b[2] = (ScalarType)2.0*(r.Direction()[2]*K - r.Origin()[2]);
c = -K*K + (ScalarType)(r.Origin()*r.Origin());
}
void SetZero()
{
a[0] = 0;
a[1] = 0;
a[2] = 0;
a[3] = 0;
a[4] = 0;
a[5] = 0;
b[0] = 0;
b[1] = 0;
b[2] = 0;
c = 0;
}
void operator = ( const Quadric & q )
{
assert( q.IsValid() );
a[0] = q.a[0];
a[1] = q.a[1];
a[2] = q.a[2];
a[3] = q.a[3];
a[4] = q.a[4];
a[5] = q.a[5];
b[0] = q.b[0];
b[1] = q.b[1];
b[2] = q.b[2];
c = q.c;
}
void operator += ( const Quadric & q )
{
assert( IsValid() );
assert( q.IsValid() );
a[0] += q.a[0];
a[1] += q.a[1];
a[2] += q.a[2];
a[3] += q.a[3];
a[4] += q.a[4];
a[5] += q.a[5];
b[0] += q.b[0];
b[1] += q.b[1];
b[2] += q.b[2];
c += q.c;
}
void operator *= ( const ScalarType & w ) // Amplifica una quadirca
{
assert( IsValid() );
a[0] *= w;
a[1] *= w;
a[2] *= w;
a[3] *= w;
a[4] *= w;
a[5] *= w;
b[0] *= w;
b[1] *= w;
b[2] *= w;
c *= w;
}
/* Evaluate a quadric over a point p.
*/
template <class ResultScalarType>
ResultScalarType Apply( const Point3<ResultScalarType> & p ) const
{
assert( IsValid() );
return ResultScalarType (
p[0]*p[0]*a[0] + 2*p[0]*p[1]*a[1] + 2*p[0]*p[2]*a[2] + p[0]*b[0]
+ p[1]*p[1]*a[3] + 2*p[1]*p[2]*a[4] + p[1]*b[1]
+ p[2]*p[2]*a[5] + p[2]*b[2] + c);
}
static double &RelativeErrorThr()
{
static double _err = 0.000001;
return _err;
}
// Find the point minimizing the quadric xAx + bx + c
// by solving the first derivative 2 Ax + b = 0
// return true if the found solution fits the system.
template <class ReturnScalarType>
bool Minimum(Point3<ReturnScalarType> &x)
{
Eigen::Matrix3d A;
Eigen::Vector3d be;
A << a[0], a[1], a[2],
a[1], a[3], a[4],
a[2], a[4], a[5];
be << -b[0]/2, -b[1]/2, -b[2]/2;
// Eigen::Vector3d xe = A.colPivHouseholderQr().solve(bv);
// Eigen::Vector3d xe = A.partialPivLu().solve(bv);
Eigen::Vector3d xe = A.fullPivLu().solve(be);
double relative_error = (A*xe - be).norm() / be.norm();
if(relative_error> Quadric<ScalarType>::RelativeErrorThr() )
return false;
x.FromEigenVector(xe);
return true;
}
template <class ReturnScalarType>
bool MinimumClosestToPoint(Point3<ReturnScalarType> &x, const Point3<ReturnScalarType> &pt)
{
const double qeps = 1e-3;
Eigen::Matrix3d A;
Eigen::Vector3d be;
A << a[0], a[1], a[2],
a[1], a[3], a[4],
a[2], a[4], a[5];
be << -b[0]/2, -b[1]/2, -b[2]/2;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Vector3d s = svd.singularValues();
for(int i=1;i<3;++i)
if(s[i]/s[0] > qeps) s[i]=1/s[i];
else s[i]=0;
s[0]=1/s[0];
Eigen::Vector3d xp;
pt.ToEigenVector(xp);
Eigen::Vector3d xe = xp + (svd.matrixV()*s.asDiagonal()*(svd.matrixU().transpose())) *(be - A*xp);
x.FromEigenVector(xe);
return true;
}
};
typedef Quadric<short> Quadrics;
typedef Quadric<int> Quadrici;
typedef Quadric<float> Quadricf;
typedef Quadric<double> Quadricd;
} // end namespace math
} // end namespace vcg
#endif