void _computeCovarianceMat(const RDGeom::Point3DConstPtrVect &refPoints,
const RDGeom::Point3DConstPtrVect &probePoints,
const DoubleVector &weights, double covMat[3][3]) {
unsigned int i, j;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
covMat[i][j] = 0.0;
}
}
unsigned int npt = refPoints.size();
CHECK_INVARIANT(npt == probePoints.size(), "Number of points mismatch");
CHECK_INVARIANT(npt == weights.size(),
"Number of points and number of weights do not match");
const double *wData = weights.getData();
const RDGeom::Point3D *rpt, *ppt;
double w;
for (i = 0; i < npt; i++) {
rpt = refPoints[i];
ppt = probePoints[i];
w = wData[i];
covMat[0][0] += w * (ppt->x) * (rpt->x);
covMat[0][1] += w * (ppt->x) * (rpt->y);
covMat[0][2] += w * (ppt->x) * (rpt->z);
covMat[1][0] += w * (ppt->y) * (rpt->x);
covMat[1][1] += w * (ppt->y) * (rpt->y);
covMat[1][2] += w * (ppt->y) * (rpt->z);
covMat[2][0] += w * (ppt->z) * (rpt->x);
covMat[2][1] += w * (ppt->z) * (rpt->y);
covMat[2][2] += w * (ppt->z) * (rpt->z);
}
}
double _sumOfWeights(const DoubleVector &weights) {
const double *wData = weights.getData();
double res = 0.0;
for (unsigned int i = 0; i < weights.size(); i++) {
CHECK_INVARIANT(wData[i] > 0.0, "Negative weight specified for a point");
res += wData[i];
}
return res;
}
double _weightedSumOfLenSq(const RDGeom::Point3DConstPtrVect &points,
const DoubleVector &weights) {
PRECONDITION(points.size() == weights.size(), "");
double res = 0.0;
RDGeom::Point3DConstPtrVect_CI pti;
const double *wData = weights.getData();
unsigned int i = 0;
for (pti = points.begin(); pti != points.end(); pti++) {
res += (wData[i] * ((*pti)->lengthSq()));
i++;
}
return res;
}
RDGeom::Point3D _weightedSumOfPoints(const RDGeom::Point3DConstPtrVect &points,
const DoubleVector &weights) {
PRECONDITION(points.size() == weights.size(), "");
RDGeom::Point3DConstPtrVect_CI pti;
RDGeom::Point3D tmpPt, res;
const double *wData = weights.getData();
unsigned int i = 0;
for (pti = points.begin(); pti != points.end(); pti++) {
tmpPt = (*(*pti));
tmpPt *= wData[i];
res += tmpPt;
i++;
}
return res;
}