CylinderFit3<Real>::CylinderFit3 (int numPoints, const Vector3<Real>* points,
Vector3<Real>& center, Vector3<Real>& axis, Real& radius, Real& height,
bool inputsAreInitialGuess)
{
Real invRSqr = (Real)1;
if (!inputsAreInitialGuess)
{
// Find the least-squares line that fits the data and use it as an
// initial guess for the cylinder axis.
Line3<Real> line = OrthogonalLineFit3(numPoints, points);
center = line.Origin;
axis = line.Direction;
}
mError = Math<Real>::MAX_REAL;
const int iMax = 8;
int i;
for (i = 0; i < iMax; ++i)
{
mError = UpdateInvRSqr(numPoints, points, center, axis, invRSqr);
mError = UpdateDirection(numPoints, points, center, axis, invRSqr);
mError = UpdateCenter(numPoints, points, center, axis, invRSqr);
}
// Compute the radius.
radius = Math<Real>::InvSqrt(invRSqr);
// Project points onto fitted axis to determine extent of cylinder along
// the axis.
Real tMin = axis.Dot(points[0] - center);
Real tMax = tMin;
for (i = 1; i < numPoints; ++i)
{
Real t = axis.Dot(points[i] - center);
if (t < tMin)
{
tMin = t;
}
else if (t > tMax)
{
tMax = t;
}
}
// Compute the height. Adjust the center to point that projects to
// midpoint of extent.
height = tMax - tMin;
center += (((Real)0.5)*(tMin + tMax))*axis;
}
CylinderFit3<Real>::CylinderFit3 (int iQuantity, const Vector3<Real>* akPoint,
Vector3<Real>& rkC, Vector3<Real>& rkU, Real& rfR, Real& rfH,
bool bInputsAreInitialGuess)
{
Real fInvRSqr = (Real)1.0;
if (!bInputsAreInitialGuess)
{
// Find the least-squares line that fits the data and use it as an
// initial guess for the cylinder axis.
Line3<Real> kLine = OrthogonalLineFit3(iQuantity,akPoint);
rkC = kLine.Origin;
rkU = kLine.Direction;
}
m_fError = Math<Real>::MAX_REAL;
const int iMax = 8;
int i;
for (i = 0; i < iMax; i++)
{
m_fError = UpdateInvRSqr(iQuantity,akPoint,rkC,rkU,fInvRSqr);
m_fError = UpdateDirection(iQuantity,akPoint,rkC,rkU,fInvRSqr);
m_fError = UpdateCenter(iQuantity,akPoint,rkC,rkU,fInvRSqr);
}
// Compute the radius.
rfR = Math<Real>::InvSqrt(fInvRSqr);
// Project points onto fitted axis to determine extent of cylinder along
// the axis.
Real fTMin = rkU.Dot(akPoint[0]-rkC), fTMax = fTMin;
for (i = 1; i < iQuantity; i++)
{
Real fT = rkU.Dot(akPoint[i]-rkC);
if (fT < fTMin)
{
fTMin = fT;
}
else if (fT > fTMax)
{
fTMax = fT;
}
}
// Compute the height. Adjust the center to point that projects to
// midpoint of extent.
rfH = fTMax - fTMin;
rkC += ((Real)0.5)*(fTMin+fTMax)*rkU;
}
Cylinder3<Real> ContCylinder (int iQuantity, const Vector3<Real>* akPoint)
{
Cylinder3<Real> kCylinder;
Line3<Real> kLine = OrthogonalLineFit3(iQuantity,akPoint);
Real fMaxRadiusSqr = (Real)0.0;
int i;
for (i = 0; i < iQuantity; i++)
{
Real fRadiusSqr = DistVector3Line3<Real>(akPoint[i],
kLine).GetSquared();
if (fRadiusSqr > fMaxRadiusSqr)
{
fMaxRadiusSqr = fRadiusSqr;
}
}
Vector3<Real> kDiff = akPoint[0] - kLine.Origin;
Real fWMin = kLine.Direction.Dot(kDiff), fWMax = fWMin;
for (i = 1; i < iQuantity; i++)
{
kDiff = akPoint[i] - kLine.Origin;
Real fW = kLine.Direction.Dot(kDiff);
if (fW < fWMin)
{
fWMin = fW;
}
else if (fW > fWMax)
{
fWMax = fW;
}
}
kCylinder.Segment.Origin = kLine.Origin +
(((Real)0.5)*(fWMax+fWMin))*kLine.Direction;
kCylinder.Segment.Direction = kLine.Direction;
kCylinder.Radius = Math<Real>::Sqrt(fMaxRadiusSqr);
kCylinder.Height = fWMax - fWMin;
return kCylinder;
}
Cylinder3<Real> ContCylinder ( int numPoints, const Vector3<Real>* points )
{
Cylinder3<Real> cylinder;
Line3<Real> line = OrthogonalLineFit3( numPoints, points );
Real maxRadiusSqr = ( Real )0;
int i;
for ( i = 0; i < numPoints; ++i )
{
Real radiusSqr = DistPoint3Line3<Real>( points[i], line ).GetSquared();
if ( radiusSqr > maxRadiusSqr )
{
maxRadiusSqr = radiusSqr;
}
}
Vector3<Real> diff = points[0] - line.Origin;
Real wMin = line.Direction.Dot( diff );
Real wMax = wMin;
for ( i = 1; i < numPoints; ++i )
{
diff = points[i] - line.Origin;
Real w = line.Direction.Dot( diff );
if ( w < wMin )
{
wMin = w;
}
else if ( w > wMax )
{
wMax = w;
}
}
cylinder.Axis.Origin = line.Origin +
( ( ( Real )0.5 ) * ( wMax + wMin ) ) * line.Direction;
cylinder.Axis.Direction = line.Direction;
cylinder.Radius = Math<Real>::Sqrt( maxRadiusSqr );
cylinder.Height = wMax - wMin;
return cylinder;
}
