@Override
public double computeDistance(PointVectorNN pv) {
n.set(pv.p.x - model.center.x, pv.p.y - model.center.y, pv.p.z - model.center.z);
n.normalize();
if (Math.abs(n.dot(pv.normal)) < tolAngleCosine) return Double.MAX_VALUE;
return Math.abs(Distance3D_F64.distance(model, pv.p));
}
@Override
public boolean generate(List<PointVectorNN> dataSet, Cylinder3D_F64 output) {
PointVectorNN pa = dataSet.get(0);
PointVectorNN pb = dataSet.get(1);
PointVectorNN pc = dataSet.get(2);
lineA.p = pa.p;
lineA.slope = pa.normal;
lineB.p = pb.p;
lineB.slope = pb.normal;
// With perfect data, the closest point between the two lines defined by a point and its normal
// will lie on the axis of the cylinder
ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p);
// The cylinder's axis will be along the cross product of the two normal vectors
GeometryMath_F64.cross(pa.normal, pb.normal, output.line.slope);
// take the normal of the slope so that it can detect if it has a value of zero, which happens
// if the
// two input vectors are the same
double n = output.line.slope.norm();
// n should actually always be one since the point normals are normalized to one.
if (n < 1e-8) return false;
output.line.slope.x /= n;
output.line.slope.y /= n;
output.line.slope.z /= n;
// set the radius to be the average distance point is from the cylinder's axis
double ra = Distance3D_F64.distance(output.line, pa.p);
double rb = Distance3D_F64.distance(output.line, pb.p);
double rc = Distance3D_F64.distance(output.line, pc.p);
output.radius = (ra + rb) / 2.0;
// sanity check using the model parameters
if (!check.valid(output)) return false;
// sanity check the model using points
return checkModel(output, pc, ra, rb, rc);
}