bool Plane3f::bestFit (int npoints, const Vec3f points[], float *sqsum_out)
{
QS_DEF(Array<double>, m);
m.clear_resize(npoints * 3);
int i, j, k;
Matr3x3d A, evec;
Vec3f c;
for (i = 0; i < npoints; i++)
{
c.add(points[i]);
}
c.scale(1.0f/npoints);
for (i = 0; i < npoints; i ++)
{
m[3 * i + 0] = points[i].x - c.x;
m[3 * i + 1] = points[i].y - c.y;
m[3 * i + 2] = points[i].z - c.z;
}
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
A.elements[i * 3 + j] = 0;
for (k = 0; k < npoints; k++)
{
A.elements[i * 3 + j] += m[k * 3 + i] * m[k * 3 + j];
}
}
}
A.eigenSystem(evec);
_norm.x = (float)evec.elements[2];
_norm.y = (float)evec.elements[5];
_norm.z = (float)evec.elements[8];
_d = - Vec3f::dot(_norm, c);
if (sqsum_out != 0)
{
*sqsum_out = 0;
for (i = 0; i < npoints; i++)
{
float d = distFromPoint(points[i]);
*sqsum_out += d * d;
}
}
return true;
}
void Molecule3dConstraintsChecker::_cache (int idx)
{
if (_cache_v.find(idx) || _cache_l.find(idx) || _cache_p.find(idx))
return;
const MC::Base &base = _constraints.at(idx);
switch (base.type)
{
case MC::POINT_ATOM:
{
int atom_idx = ((const Molecule3dConstraints::PointByAtom &)base).atom_idx;
_cache_v.insert(idx, _target->getAtomXyz(_mapping[atom_idx]));
break;
}
case MC::POINT_DISTANCE:
{
const MC::PointByDistance &constr = (const MC::PointByDistance &)base;
_cache(constr.beg_id);
_cache(constr.end_id);
const Vec3f &beg = _cache_v.at(constr.beg_id);
const Vec3f &end = _cache_v.at(constr.end_id);
Vec3f dir;
dir.diff(end, beg);
if (!dir.normalize())
throw Error("point-by-distance: degenerate case");
Vec3f res;
res.lineCombin(beg, dir, constr.distance);
_cache_v.insert(idx, res);
break;
}
case MC::POINT_PERCENTAGE:
{
const MC::PointByPercentage &constr = (const MC::PointByPercentage &)base;
_cache(constr.beg_id);
_cache(constr.end_id);
const Vec3f &beg = _cache_v.at(constr.beg_id);
const Vec3f &end = _cache_v.at(constr.end_id);
Vec3f dir;
dir.diff(end, beg);
if (!dir.normalize())
throw Error("point-by-percentage: degenerate case");
Vec3f res;
res.lineCombin2(beg, 1.f - constr.percentage, end, constr.percentage);
_cache_v.insert(idx, res);
break;
}
case MC::POINT_NORMALE:
{
const MC::PointByNormale &constr = (const MC::PointByNormale &)base;
_cache(constr.org_id);
_cache(constr.norm_id);
const Vec3f &org = _cache_v.at(constr.org_id);
const Line3f &norm = _cache_l.at(constr.norm_id);
Vec3f res;
res.lineCombin(org, norm.dir, constr.distance);
_cache_v.insert(idx, res);
break;
}
case MC::POINT_CENTROID:
{
const MC::Centroid &constr = (const MC::Centroid &)base;
Vec3f res;
if (constr.point_ids.size() < 1)
throw Error("centroid: have %d points", constr.point_ids.size());
for (int i = 0; i < constr.point_ids.size(); i++)
{
_cache(constr.point_ids[i]);
const Vec3f &pt = _cache_v.at(constr.point_ids[i]);
res.add(pt);
}
res.scale(1.f / constr.point_ids.size());
_cache_v.insert(idx, res);
break;
}
case MC::LINE_NORMALE:
{
const MC::Normale &constr = (const MC::Normale &)base;
_cache(constr.plane_id);
_cache(constr.point_id);
const Plane3f &plane = _cache_p.at(constr.plane_id);
const Vec3f &point = _cache_v.at(constr.point_id);
Vec3f projection;
Line3f res;
plane.projection(point, projection);
res.dir.copy(plane.getNorm());
res.org.copy(projection);
_cache_l.insert(idx, res);
break;
}
case MC::LINE_BEST_FIT:
{
const MC::BestFitLine &constr = (const MC::BestFitLine &)base;
if (constr.point_ids.size() < 2)
throw Error("best fit line: only %d points", constr.point_ids.size());
QS_DEF(Array<Vec3f>, points);
points.clear();
for (int i = 0; i < constr.point_ids.size(); i++)
{
_cache(constr.point_ids[i]);
points.push(_cache_v.at(constr.point_ids[i]));
}
Line3f res;
res.bestFit(points.size(), points.ptr(), 0);
_cache_l.insert(idx, res);
break;
}
case MC::PLANE_BEST_FIT:
{
const MC::BestFitPlane &constr = (const MC::BestFitPlane &)base;
if (constr.point_ids.size() < 3)
throw Error("best fit line: only %d points", constr.point_ids.size());
QS_DEF(Array<Vec3f>, points);
points.clear();
for (int i = 0; i < constr.point_ids.size(); i++)
{
_cache(constr.point_ids[i]);
points.push(_cache_v.at(constr.point_ids[i]));
}
Plane3f res;
res.bestFit(points.size(), points.ptr(), 0);
_cache_p.insert(idx, res);
break;
}
case MC::PLANE_POINT_LINE:
{
const MC::PlaneByPoint &constr = (const MC::PlaneByPoint &)base;
_cache(constr.point_id);
_cache(constr.line_id);
const Vec3f &point = _cache_v.at(constr.point_id);
const Line3f &line = _cache_l.at(constr.line_id);
Plane3f res;
res.byPointAndLine(point, line);
_cache_p.insert(idx, res);
break;
}
default:
throw Error("unknown constraint type %d", base.type);
}
}
