FreeCAD / src /Mod /Mesh /App /WildMagic4 /Wm4ApprCylinderFit3.cpp

Upload folder using huggingface_hub

985c397 verified 10 months ago

9.32 kB

// SPDX-License-Identifier: LGPL-2.1-or-later

// Wild Magic Source Code
// David Eberly
// http://www.geometrictools.com
// Copyright (c) 1998-2007
//
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.  The license is available for reading at
// either of the locations:
//     http://www.gnu.org/copyleft/lgpl.html
//     http://www.geometrictools.com/License/WildMagicLicense.pdf
// The license applies to versions 0 through 4 of Wild Magic.
//
// Version: 4.0.0 (2006/06/28)

#include "Wm4FoundationPCH.h"
#include "Wm4ApprCylinderFit3.h"
#include "Wm4ApprLineFit3.h"
#include "Wm4PolynomialRoots.h"

namespace Wm4
{
//----------------------------------------------------------------------------
template <class Real>
CylinderFit3<Real>::CylinderFit3 (int iQuantity, const Vector3<Real>* akPoint,
    Vector3<Real>& rkC, Vector3<Real>& rkU, Real& rfR, Real& rfH,
    bool bInputsAreInitialGuess)
{
    Real fInvRSqr;
    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;
}
//----------------------------------------------------------------------------
template <class Real>
CylinderFit3<Real>::operator Real ()
{
    return m_fError;
}
//----------------------------------------------------------------------------
template <class Real>
Real CylinderFit3<Real>::UpdateInvRSqr (int iQuantity,
    const Vector3<Real>* akPoint, const Vector3<Real>& rkC,
    const Vector3<Real>& rkU, Real& rfInvRSqr)
{
    Real fASum = (Real)0.0, fAASum = (Real)0.0;
    for (int i = 0; i < iQuantity; i++)
    {
        Vector3<Real> kDelta = akPoint[i] - rkC;
        Vector3<Real> kDxU = kDelta.Cross(rkU);
        Real fL2 = kDxU.SquaredLength();
        fASum += fL2;
        fAASum += fL2*fL2;
    }

    rfInvRSqr = fASum/fAASum;
    Real fMin = (Real)1.0 - rfInvRSqr*fASum/(Real)iQuantity;
    return fMin;
}
//----------------------------------------------------------------------------
template <class Real>
Real CylinderFit3<Real>::UpdateDirection (int iQuantity,
    const Vector3<Real>* akPoint, const Vector3<Real>& rkC,
    Vector3<Real>& rkU, Real& rfInvRSqr)
{
    Real fInvQuantity = ((Real)1.0)/(Real)iQuantity;
    int i;
    Vector3<Real> kDelta, kDxU, kDxVDir;
    Real fA, fB, fC;

    // compute direction of steepest descent
    Vector3<Real> kVDir = Vector3<Real>::ZERO;
    Real fAMean = (Real)0.0, fAAMean = (Real)0.0;
    for (i = 0; i < iQuantity; i++)
    {
        kDelta = akPoint[i] - rkC;
        kDxU = kDelta.Cross(rkU);
        fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
        fAMean += fA;
        fAAMean += fA*fA;
        kVDir.X() += fA*(rkU.X()*(kDelta.Y()*kDelta.Y() +
            kDelta.Z()*kDelta.Z()) - kDelta.X()*(rkU.Y()*kDelta.Y() +
            rkU.Z()*kDelta.Z()));
        kVDir.Y() += fA*(rkU.Y()*(kDelta.X()*kDelta.X() +
            kDelta.Z()*kDelta.Z()) - kDelta.Y()*(rkU.X()*kDelta.X() +
            rkU.Z()*kDelta.Z()));
        kVDir.Z() += fA*(rkU.Z()*(kDelta.X()*kDelta.X() + 
            kDelta.Y()*kDelta.Y()) - kDelta.Z()*(rkU.X()*kDelta.X() +
            rkU.Y()*kDelta.Y()));
    }
    fAMean *= fInvQuantity;
    fAAMean *= fInvQuantity;
    if (kVDir.Normalize() < Math<Real>::ZERO_TOLERANCE)
    {
        return fAAMean;
    }

    // compute 4th-degree polynomial for line of steepest descent
    Real fABMean = (Real)0.0, fACMean = (Real)0.0;
    Real fBBMean = (Real)0.0, fBCMean = (Real)0.0, fCCMean = (Real)0.0;
    for (i = 0; i < iQuantity; i++)
    {
        kDelta = akPoint[i] - rkC;
        kDxU = kDelta.Cross(rkU);
        kDxVDir = kDelta.Cross(kVDir);
        fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
        fB = rfInvRSqr*(kDxU.Dot(kDxVDir));
        fC = rfInvRSqr*kDxVDir.SquaredLength();
        fABMean += fA*fB;
        fACMean += fA*fC;
        fBBMean += fB*fB;
        fBCMean += fB*fC;
        fCCMean += fC*fC;
    }
    fABMean *= fInvQuantity;
    fACMean *= fInvQuantity;
    fBBMean *= fInvQuantity;
    fBCMean *= fInvQuantity;
    fCCMean *= fInvQuantity;

    Polynomial1<Real> kPoly(4);
    kPoly[0] = fAAMean;
    kPoly[1] = -((Real)4.0)*fABMean;
    kPoly[2] = ((Real)2.0)*fACMean + ((Real)4.0)*fBBMean;
    kPoly[3] = -((Real)4.0)*fBCMean;
    kPoly[4] = fCCMean;

    Polynomial1<Real> kDPoly = kPoly.GetDerivative();

    PolynomialRoots<Real> kPR(Math<Real>::ZERO_TOLERANCE);
    kPR.FindA(kDPoly[0],kDPoly[1],kDPoly[2],kDPoly[3]);
    int iCount = kPR.GetCount();
    const Real* afRoot = kPR.GetRoots();

    Real fMin = kPoly((Real)0.0);
    int iMin = -1;
    for (i = 0; i < iCount; i++)
    {
        Real fValue = kPoly(afRoot[i]);
        if (fValue < fMin)
        {
            fMin = fValue;
            iMin = i;
        }
    }

    if (iMin >= 0)
    {
        rkU -= afRoot[iMin]*kVDir;
        Real fLength = rkU.Normalize();
        rfInvRSqr *= fLength*fLength;
    }

    return fMin;
}
//----------------------------------------------------------------------------
template <class Real>
Real CylinderFit3<Real>::UpdateCenter (int iQuantity,
    const Vector3<Real>* akPoint, Vector3<Real>& rkC,
    const Vector3<Real>& rkU, const Real& rfInvRSqr)
{
    Real fInvQuantity = ((Real)1.0)/(Real)iQuantity;
    int i;
    Vector3<Real> kDelta, kDxU, kDxCDir;
    Real fA, fB, fC;

    // compute direction of steepest descent
    Vector3<Real> kCDir = Vector3<Real>::ZERO;
    Real fAMean = (Real)0.0, fAAMean = (Real)0.0;
    for (i = 0; i < iQuantity; i++)
    {
        kDelta = akPoint[i] - rkC;
        kDxU = kDelta.Cross(rkU);
        fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
        fAMean += fA;
        fAAMean += fA*fA;
        kCDir += fA*(kDelta-rkU.Dot(kDelta)*rkU); // |U|=1 assumed
    }
    fAMean *= fInvQuantity;
    fAAMean *= fInvQuantity;
    if (kCDir.Normalize() < Math<Real>::ZERO_TOLERANCE)
    {
        return fAAMean;
    }

    // compute 4th-degree polynomial for line of steepest descent
    kDxCDir = kCDir.Cross(rkU);
    fC = kDxCDir.SquaredLength()*fInvQuantity*rfInvRSqr;
    Real fBMean = (Real)0.0,  fABMean = (Real)0.0, fBBMean = (Real)0.0;
    for (i = 0; i < iQuantity; i++)
    {
        kDelta = akPoint[i] - rkC;
        kDxU = kDelta.Cross(rkU);
        fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
        fB = rfInvRSqr*(kDxU.Dot(kDxCDir));
        fBMean += fB;
        fABMean += fA*fB;
        fBBMean += fB*fB;
    }
    fBMean *= fInvQuantity;
    fABMean *= fInvQuantity;
    fBBMean *= fInvQuantity;

    Polynomial1<Real> kPoly(4);
    kPoly[0] = fAAMean;
    kPoly[1] = ((Real)4.0)*fABMean;
    kPoly[2] = ((Real)2.0)*fC*fAMean + ((Real)4.0)*fBBMean;
    kPoly[3] = ((Real)4.0)*fC*fBMean;
    kPoly[4] = fC*fC;

    Polynomial1<Real> kDPoly = kPoly.GetDerivative();

    PolynomialRoots<Real> kPR(Math<Real>::ZERO_TOLERANCE);
    kPR.FindA(kDPoly[0],kDPoly[1],kDPoly[2],kDPoly[3]);
    int iCount = kPR.GetCount();
    const Real* afRoot = kPR.GetRoots();

    Real fMin = kPoly((Real)0.0);
    int iMin = -1;
    for (i = 0; i < iCount; i++)
    {
        Real fValue = kPoly(afRoot[i]);
        if (fValue < fMin)
        {
            fMin = fValue;
            iMin = i;
        }
    }

    if (iMin >= 0)
    {
        rkC -= afRoot[iMin]*kCDir;
    }

    return fMin;
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// explicit instantiation
//----------------------------------------------------------------------------
template WM4_FOUNDATION_ITEM
class CylinderFit3<float>;

template WM4_FOUNDATION_ITEM
class CylinderFit3<double>;
//----------------------------------------------------------------------------
}

	// SPDX-License-Identifier: LGPL-2.1-or-later

	// Wild Magic Source Code
	// David Eberly
	// http://www.geometrictools.com
	// Copyright (c) 1998-2007
	//
	// This library is free software; you can redistribute it and/or modify it
	// under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation; either version 2.1 of the License, or (at
	// your option) any later version. The license is available for reading at
	// either of the locations:
	// http://www.gnu.org/copyleft/lgpl.html
	// http://www.geometrictools.com/License/WildMagicLicense.pdf
	// The license applies to versions 0 through 4 of Wild Magic.
	//
	// Version: 4.0.0 (2006/06/28)

	#include "Wm4FoundationPCH.h"
	#include "Wm4ApprCylinderFit3.h"
	#include "Wm4ApprLineFit3.h"
	#include "Wm4PolynomialRoots.h"

	namespace Wm4
	{
	//----------------------------------------------------------------------------
	template <class Real>
	CylinderFit3<Real>::CylinderFit3 (int iQuantity, const Vector3<Real>* akPoint,
	Vector3<Real>& rkC, Vector3<Real>& rkU, Real& rfR, Real& rfH,
	bool bInputsAreInitialGuess)
	{
	Real fInvRSqr;
	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;
	}
	//----------------------------------------------------------------------------
	template <class Real>
	CylinderFit3<Real>::operator Real ()
	{
	return m_fError;
	}
	//----------------------------------------------------------------------------
	template <class Real>
	Real CylinderFit3<Real>::UpdateInvRSqr (int iQuantity,
	const Vector3<Real>* akPoint, const Vector3<Real>& rkC,
	const Vector3<Real>& rkU, Real& rfInvRSqr)
	{
	Real fASum = (Real)0.0, fAASum = (Real)0.0;
	for (int i = 0; i < iQuantity; i++)
	{
	Vector3<Real> kDelta = akPoint[i] - rkC;
	Vector3<Real> kDxU = kDelta.Cross(rkU);
	Real fL2 = kDxU.SquaredLength();
	fASum += fL2;
	fAASum += fL2*fL2;
	}

	rfInvRSqr = fASum/fAASum;
	Real fMin = (Real)1.0 - rfInvRSqr*fASum/(Real)iQuantity;
	return fMin;
	}
	//----------------------------------------------------------------------------
	template <class Real>
	Real CylinderFit3<Real>::UpdateDirection (int iQuantity,
	const Vector3<Real>* akPoint, const Vector3<Real>& rkC,
	Vector3<Real>& rkU, Real& rfInvRSqr)
	{
	Real fInvQuantity = ((Real)1.0)/(Real)iQuantity;
	int i;
	Vector3<Real> kDelta, kDxU, kDxVDir;
	Real fA, fB, fC;

	// compute direction of steepest descent
	Vector3<Real> kVDir = Vector3<Real>::ZERO;
	Real fAMean = (Real)0.0, fAAMean = (Real)0.0;
	for (i = 0; i < iQuantity; i++)
	{
	kDelta = akPoint[i] - rkC;
	kDxU = kDelta.Cross(rkU);
	fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
	fAMean += fA;
	fAAMean += fA*fA;
	kVDir.X() += fA(rkU.X()(kDelta.Y()*kDelta.Y() +
	kDelta.Z()kDelta.Z()) - kDelta.X()(rkU.Y()*kDelta.Y() +
	rkU.Z()*kDelta.Z()));
	kVDir.Y() += fA(rkU.Y()(kDelta.X()*kDelta.X() +
	kDelta.Z()kDelta.Z()) - kDelta.Y()(rkU.X()*kDelta.X() +
	rkU.Z()*kDelta.Z()));
	kVDir.Z() += fA(rkU.Z()(kDelta.X()*kDelta.X() +
	kDelta.Y()kDelta.Y()) - kDelta.Z()(rkU.X()*kDelta.X() +
	rkU.Y()*kDelta.Y()));
	}
	fAMean *= fInvQuantity;
	fAAMean *= fInvQuantity;
	if (kVDir.Normalize() < Math<Real>::ZERO_TOLERANCE)
	{
	return fAAMean;
	}

	// compute 4th-degree polynomial for line of steepest descent
	Real fABMean = (Real)0.0, fACMean = (Real)0.0;
	Real fBBMean = (Real)0.0, fBCMean = (Real)0.0, fCCMean = (Real)0.0;
	for (i = 0; i < iQuantity; i++)
	{
	kDelta = akPoint[i] - rkC;
	kDxU = kDelta.Cross(rkU);
	kDxVDir = kDelta.Cross(kVDir);
	fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
	fB = rfInvRSqr*(kDxU.Dot(kDxVDir));
	fC = rfInvRSqr*kDxVDir.SquaredLength();
	fABMean += fA*fB;
	fACMean += fA*fC;
	fBBMean += fB*fB;
	fBCMean += fB*fC;
	fCCMean += fC*fC;
	}
	fABMean *= fInvQuantity;
	fACMean *= fInvQuantity;
	fBBMean *= fInvQuantity;
	fBCMean *= fInvQuantity;
	fCCMean *= fInvQuantity;

	Polynomial1<Real> kPoly(4);
	kPoly[0] = fAAMean;
	kPoly[1] = -((Real)4.0)*fABMean;
	kPoly[2] = ((Real)2.0)fACMean + ((Real)4.0)fBBMean;
	kPoly[3] = -((Real)4.0)*fBCMean;
	kPoly[4] = fCCMean;

	Polynomial1<Real> kDPoly = kPoly.GetDerivative();

	PolynomialRoots<Real> kPR(Math<Real>::ZERO_TOLERANCE);
	kPR.FindA(kDPoly[0],kDPoly[1],kDPoly[2],kDPoly[3]);
	int iCount = kPR.GetCount();
	const Real* afRoot = kPR.GetRoots();

	Real fMin = kPoly((Real)0.0);
	int iMin = -1;
	for (i = 0; i < iCount; i++)
	{
	Real fValue = kPoly(afRoot[i]);
	if (fValue < fMin)
	{
	fMin = fValue;
	iMin = i;
	}
	}

	if (iMin >= 0)
	{
	rkU -= afRoot[iMin]*kVDir;
	Real fLength = rkU.Normalize();
	rfInvRSqr = fLengthfLength;
	}

	return fMin;
	}
	//----------------------------------------------------------------------------
	template <class Real>
	Real CylinderFit3<Real>::UpdateCenter (int iQuantity,
	const Vector3<Real>* akPoint, Vector3<Real>& rkC,
	const Vector3<Real>& rkU, const Real& rfInvRSqr)
	{
	Real fInvQuantity = ((Real)1.0)/(Real)iQuantity;
	int i;
	Vector3<Real> kDelta, kDxU, kDxCDir;
	Real fA, fB, fC;

	// compute direction of steepest descent
	Vector3<Real> kCDir = Vector3<Real>::ZERO;
	Real fAMean = (Real)0.0, fAAMean = (Real)0.0;
	for (i = 0; i < iQuantity; i++)
	{
	kDelta = akPoint[i] - rkC;
	kDxU = kDelta.Cross(rkU);
	fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
	fAMean += fA;
	fAAMean += fA*fA;
	kCDir += fA(kDelta-rkU.Dot(kDelta)rkU); // \|U\|=1 assumed
	}
	fAMean *= fInvQuantity;
	fAAMean *= fInvQuantity;
	if (kCDir.Normalize() < Math<Real>::ZERO_TOLERANCE)
	{
	return fAAMean;
	}

	// compute 4th-degree polynomial for line of steepest descent
	kDxCDir = kCDir.Cross(rkU);
	fC = kDxCDir.SquaredLength()fInvQuantityrfInvRSqr;
	Real fBMean = (Real)0.0, fABMean = (Real)0.0, fBBMean = (Real)0.0;
	for (i = 0; i < iQuantity; i++)
	{
	kDelta = akPoint[i] - rkC;
	kDxU = kDelta.Cross(rkU);
	fA = rfInvRSqr*kDxU.SquaredLength() - (Real)1.0;
	fB = rfInvRSqr*(kDxU.Dot(kDxCDir));
	fBMean += fB;
	fABMean += fA*fB;
	fBBMean += fB*fB;
	}
	fBMean *= fInvQuantity;
	fABMean *= fInvQuantity;
	fBBMean *= fInvQuantity;

	Polynomial1<Real> kPoly(4);
	kPoly[0] = fAAMean;
	kPoly[1] = ((Real)4.0)*fABMean;
	kPoly[2] = ((Real)2.0)fCfAMean + ((Real)4.0)*fBBMean;
	kPoly[3] = ((Real)4.0)fCfBMean;
	kPoly[4] = fC*fC;

	Polynomial1<Real> kDPoly = kPoly.GetDerivative();

	PolynomialRoots<Real> kPR(Math<Real>::ZERO_TOLERANCE);
	kPR.FindA(kDPoly[0],kDPoly[1],kDPoly[2],kDPoly[3]);
	int iCount = kPR.GetCount();
	const Real* afRoot = kPR.GetRoots();

	Real fMin = kPoly((Real)0.0);
	int iMin = -1;
	for (i = 0; i < iCount; i++)
	{
	Real fValue = kPoly(afRoot[i]);
	if (fValue < fMin)
	{
	fMin = fValue;
	iMin = i;
	}
	}

	if (iMin >= 0)
	{
	rkC -= afRoot[iMin]*kCDir;
	}

	return fMin;
	}
	//----------------------------------------------------------------------------

	//----------------------------------------------------------------------------
	// explicit instantiation
	//----------------------------------------------------------------------------
	template WM4_FOUNDATION_ITEM
	class CylinderFit3<float>;

	template WM4_FOUNDATION_ITEM
	class CylinderFit3<double>;
	//----------------------------------------------------------------------------
	}