120ca87015
git-svn-id: https://free-cad.svn.sourceforge.net/svnroot/free-cad/trunk@5000 e8eeb9e2-ec13-0410-a4a9-efa5cf37419d
1741 lines
51 KiB
C++
1741 lines
51 KiB
C++
// 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 "Wm4PolynomialRoots.h"
|
|
|
|
namespace Wm4
|
|
{
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
PolynomialRoots<Real>::PolynomialRoots (Real fEpsilon)
|
|
{
|
|
assert(fEpsilon >= (Real)0.0);
|
|
m_fEpsilon = fEpsilon;
|
|
m_iMaxIterations = 128;
|
|
m_iCount = 0;
|
|
m_iMaxRoot = 4; // default support for degree <= 4
|
|
m_afRoot = WM4_NEW Real[m_iMaxRoot];
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
PolynomialRoots<Real>::~PolynomialRoots ()
|
|
{
|
|
WM4_DELETE[] m_afRoot;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
int PolynomialRoots<Real>::GetCount () const
|
|
{
|
|
return m_iCount;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
const Real* PolynomialRoots<Real>::GetRoots () const
|
|
{
|
|
return m_afRoot;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetRoot (int i) const
|
|
{
|
|
assert(0 <= i && i < m_iCount);
|
|
if (0 <= i && i < m_iCount)
|
|
{
|
|
return m_afRoot[i];
|
|
}
|
|
|
|
return Math<Real>::MAX_REAL;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real& PolynomialRoots<Real>::Epsilon ()
|
|
{
|
|
return m_fEpsilon;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::Epsilon () const
|
|
{
|
|
return m_fEpsilon;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
int& PolynomialRoots<Real>::MaxIterations ()
|
|
{
|
|
return m_iMaxIterations;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
int PolynomialRoots<Real>::MaxIterations () const
|
|
{
|
|
return m_iMaxIterations;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// degree 1
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindA (Real fC0, Real fC1)
|
|
{
|
|
if (Math<Real>::FAbs(fC1) >= m_fEpsilon)
|
|
{
|
|
m_afRoot[0] = -fC0/fC1;
|
|
m_iCount = 1;
|
|
return true;
|
|
}
|
|
|
|
m_iCount = 0;
|
|
return false;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetBound (Real fC0, Real fC1)
|
|
{
|
|
if (Math<Real>::FAbs(fC1) <= m_fEpsilon)
|
|
{
|
|
// polynomial is constant, return invalid bound
|
|
return -(Real)1.0;
|
|
}
|
|
|
|
Real fInvC1 = ((Real)1.0)/fC1;
|
|
Real fMax = Math<Real>::FAbs(fC0)*fInvC1;
|
|
return (Real)1.0 + fMax;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// degree 2
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindA (Real fC0, Real fC1, Real fC2)
|
|
{
|
|
if (Math<Real>::FAbs(fC2) <= m_fEpsilon)
|
|
{
|
|
// polynomial is linear
|
|
return FindA(fC0,fC1);
|
|
}
|
|
|
|
Real fDiscr = fC1*fC1 - 4.0f*fC0*fC2;
|
|
if (Math<Real>::FAbs(fDiscr) <= m_fEpsilon)
|
|
{
|
|
fDiscr = (Real)0.0;
|
|
}
|
|
|
|
if (fDiscr < (Real)0.0)
|
|
{
|
|
m_iCount = 0;
|
|
return false;
|
|
}
|
|
|
|
Real fTmp = ((Real)0.5)/fC2;
|
|
|
|
if (fDiscr > (Real)0.0)
|
|
{
|
|
fDiscr = Math<Real>::Sqrt(fDiscr);
|
|
m_afRoot[0] = fTmp*(-fC1 - fDiscr);
|
|
m_afRoot[1] = fTmp*(-fC1 + fDiscr);
|
|
m_iCount = 2;
|
|
}
|
|
else
|
|
{
|
|
m_afRoot[0] = -fTmp*fC1;
|
|
m_iCount = 1;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetBound (Real fC0, Real fC1, Real fC2)
|
|
{
|
|
if (Math<Real>::FAbs(fC2) <= m_fEpsilon)
|
|
{
|
|
// polynomial is linear
|
|
return (FindA(fC0,fC1) ? m_afRoot[0] : Math<Real>::MAX_REAL);
|
|
}
|
|
|
|
Real fInvC2 = ((Real)1.0)/fC2;
|
|
Real fTmp0 = Math<Real>::FAbs(fC0)*fInvC2;
|
|
Real fTmp1 = Math<Real>::FAbs(fC1)*fInvC2;
|
|
Real fMax = ( fTmp0 >= fTmp1 ? fTmp0 : fTmp1 );
|
|
return (Real)1.0 + fMax;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// degree 3
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindA (Real fC0, Real fC1, Real fC2, Real fC3)
|
|
{
|
|
if (Math<Real>::FAbs(fC3) <= m_fEpsilon)
|
|
{
|
|
// polynomial is quadratic
|
|
return FindA(fC0,fC1,fC2);
|
|
}
|
|
|
|
// make polynomial monic, x^3+c2*x^2+c1*x+c0
|
|
Real fInvC3 = ((Real)1.0)/fC3;
|
|
fC0 *= fInvC3;
|
|
fC1 *= fInvC3;
|
|
fC2 *= fInvC3;
|
|
|
|
// convert to y^3+a*y+b = 0 by x = y-c2/3
|
|
const Real fThird = (Real)1.0/(Real)3.0;
|
|
const Real fTwentySeventh = (Real)1.0/(Real)27.0;
|
|
Real fOffset = fThird*fC2;
|
|
Real fA = fC1 - fC2*fOffset;
|
|
Real fB = fC0+fC2*(((Real)2.0)*fC2*fC2-((Real)9.0)*fC1)*fTwentySeventh;
|
|
Real fHalfB = ((Real)0.5)*fB;
|
|
|
|
Real fDiscr = fHalfB*fHalfB + fA*fA*fA*fTwentySeventh;
|
|
if (Math<Real>::FAbs(fDiscr) <= m_fEpsilon)
|
|
{
|
|
fDiscr = (Real)0.0;
|
|
}
|
|
|
|
if (fDiscr > (Real)0.0) // 1 real, 2 complex roots
|
|
{
|
|
fDiscr = Math<Real>::Sqrt(fDiscr);
|
|
Real fTemp = -fHalfB + fDiscr;
|
|
if (fTemp >= (Real)0.0)
|
|
{
|
|
m_afRoot[0] = Math<Real>::Pow(fTemp,fThird);
|
|
}
|
|
else
|
|
{
|
|
m_afRoot[0] = -Math<Real>::Pow(-fTemp,fThird);
|
|
}
|
|
fTemp = -fHalfB - fDiscr;
|
|
if ( fTemp >= (Real)0.0 )
|
|
{
|
|
m_afRoot[0] += Math<Real>::Pow(fTemp,fThird);
|
|
}
|
|
else
|
|
{
|
|
m_afRoot[0] -= Math<Real>::Pow(-fTemp,fThird);
|
|
}
|
|
m_afRoot[0] -= fOffset;
|
|
m_iCount = 1;
|
|
}
|
|
else if (fDiscr < (Real)0.0)
|
|
{
|
|
const Real fSqrt3 = Math<Real>::Sqrt((Real)3.0);
|
|
Real fDist = Math<Real>::Sqrt(-fThird*fA);
|
|
Real fAngle = fThird*Math<Real>::ATan2(Math<Real>::Sqrt(-fDiscr),
|
|
-fHalfB);
|
|
Real fCos = Math<Real>::Cos(fAngle);
|
|
Real fSin = Math<Real>::Sin(fAngle);
|
|
m_afRoot[0] = ((Real)2.0)*fDist*fCos-fOffset;
|
|
m_afRoot[1] = -fDist*(fCos+fSqrt3*fSin)-fOffset;
|
|
m_afRoot[2] = -fDist*(fCos-fSqrt3*fSin)-fOffset;
|
|
m_iCount = 3;
|
|
}
|
|
else
|
|
{
|
|
Real fTemp;
|
|
if (fHalfB >= (Real)0.0)
|
|
{
|
|
fTemp = -Math<Real>::Pow(fHalfB,fThird);
|
|
}
|
|
else
|
|
{
|
|
fTemp = Math<Real>::Pow(-fHalfB,fThird);
|
|
}
|
|
m_afRoot[0] = ((Real)2.0)*fTemp-fOffset;
|
|
m_afRoot[1] = -fTemp-fOffset;
|
|
m_afRoot[2] = m_afRoot[1];
|
|
m_iCount = 3;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindE (Real fC0, Real fC1, Real fC2, Real fC3,
|
|
bool bDoBalancing)
|
|
{
|
|
if (Math<Real>::FAbs(fC3) <= m_fEpsilon)
|
|
{
|
|
// polynomial is quadratic
|
|
return FindA(fC0,fC1,fC2);
|
|
}
|
|
|
|
// make polynomial monic, x^3+c2*x^2+c1*x+c0
|
|
Real fInvC3 = ((Real)1.0)/fC3;
|
|
fC0 *= fInvC3;
|
|
fC1 *= fInvC3;
|
|
fC2 *= fInvC3;
|
|
|
|
// construct the 3-by-3 companion matrix
|
|
GMatrix<Real> kMat(3,3); // initialized to zero
|
|
kMat[1][0] = (Real)1.0;
|
|
kMat[2][1] = (Real)1.0;
|
|
kMat[0][2] = -fC0;
|
|
kMat[1][2] = -fC1;
|
|
kMat[2][2] = -fC2;
|
|
|
|
if (bDoBalancing)
|
|
{
|
|
BalanceCompanion3(kMat);
|
|
}
|
|
|
|
return QRIteration3(kMat);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetBound (Real fC0, Real fC1, Real fC2, Real fC3)
|
|
{
|
|
if (Math<Real>::FAbs(fC3) <= m_fEpsilon)
|
|
{
|
|
// polynomial is quadratic
|
|
return GetBound(fC0,fC1,fC2);
|
|
}
|
|
|
|
Real fInvC3 = ((Real)1.0)/fC3;
|
|
Real fMax = Math<Real>::FAbs(fC0)*fInvC3;
|
|
|
|
Real fTmp = Math<Real>::FAbs(fC1)*fInvC3;
|
|
if (fTmp > fMax)
|
|
{
|
|
fMax = fTmp;
|
|
}
|
|
|
|
fTmp = Math<Real>::FAbs(fC2)*fInvC3;
|
|
if (fTmp > fMax)
|
|
{
|
|
fMax = fTmp;
|
|
}
|
|
|
|
return (Real)1.0 + fMax;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::SpecialCubic (Real fA, Real fB, Real fC)
|
|
{
|
|
// Solve A*r^3 + B*r = C where A > 0 and B > 0.
|
|
//
|
|
// Let r = D*sinh(u) where D = sqrt(4*B/(3*A)). Then
|
|
// sinh(3*u) = 4*[sinh(u)]^3+3*sinh(u) = E where E = 4*C/(A*D^3).
|
|
// sinh(3*u) = E has solution u = (1/3)*log(E+sqrt(E^2+1)). This
|
|
// leads to sinh(u) = ((E+sqrt(E^2+1))^{1/3}-(E+sqrt(E^2+1))^{-1/3})/2.
|
|
// Therefore, r = D*((E+sqrt(E^2+1))^{1/3}-(E+sqrt(E^2+1))^{-1/3})/2.
|
|
|
|
const Real fThird = (Real)1.0/(Real)3.0;
|
|
Real fD = Math<Real>::Sqrt(((Real)4.0)*fThird*fB/fA);
|
|
Real fE = ((Real)4.0)*fC/(fA*fD*fD*fD);
|
|
Real fF = Math<Real>::Pow(fE+Math<Real>::Sqrt(fE*fE+(Real)1.0),fThird);
|
|
Real fRoot = ((Real)0.5)*fD*(fF-((Real)1.0)/fF);
|
|
return fRoot;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetRowNorm (int iRow, GMatrix<Real>& rkMat)
|
|
{
|
|
Real fNorm = Math<Real>::FAbs(rkMat[iRow][0]);
|
|
for (int iCol = 1; iCol < rkMat.GetColumns(); iCol++)
|
|
{
|
|
Real fAbs = Math<Real>::FAbs(rkMat[iRow][iCol]);
|
|
if (fAbs > fNorm)
|
|
{
|
|
fNorm = fAbs;
|
|
}
|
|
}
|
|
return fNorm;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetColNorm (int iCol, GMatrix<Real>& rkMat)
|
|
{
|
|
Real fNorm = Math<Real>::FAbs(rkMat[0][iCol]);
|
|
for (int iRow = 1; iRow < rkMat.GetRows(); iRow++)
|
|
{
|
|
Real fAbs = Math<Real>::FAbs(rkMat[iRow][iCol]);
|
|
if (fAbs > fNorm)
|
|
{
|
|
fNorm = fAbs;
|
|
}
|
|
}
|
|
return fNorm;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::ScaleRow (int iRow, Real fScale,
|
|
GMatrix<Real>& rkMat)
|
|
{
|
|
for (int iCol = 0; iCol < rkMat.GetColumns(); iCol++)
|
|
{
|
|
rkMat[iRow][iCol] *= fScale;
|
|
}
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::ScaleCol (int iCol, Real fScale,
|
|
GMatrix<Real>& rkMat)
|
|
{
|
|
for (int iRow = 0; iRow < rkMat.GetRows(); iRow++)
|
|
{
|
|
rkMat[iRow][iCol] *= fScale;
|
|
}
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::IsBalanced3 (GMatrix<Real>& rkMat)
|
|
{
|
|
const Real fTolerance = (Real)0.001;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
Real fRowNorm = GetRowNorm(i,rkMat);
|
|
Real fColNorm = GetColNorm(i,rkMat);
|
|
Real fTest = Math<Real>::FAbs((Real)1.0 - fColNorm/fRowNorm);
|
|
if (fTest > fTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::Balance3 (GMatrix<Real>& rkMat)
|
|
{
|
|
const int iMax = 16;
|
|
int i;
|
|
for (i = 0; i < iMax; i++)
|
|
{
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
Real fRowNorm = GetRowNorm(j,rkMat);
|
|
Real fColNorm = GetColNorm(j,rkMat);
|
|
Real fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
Real fInvScale = ((Real)1.0)/fScale;
|
|
ScaleRow(j,fScale,rkMat);
|
|
ScaleCol(j,fInvScale,rkMat);
|
|
}
|
|
|
|
if (IsBalanced3(rkMat))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
assert(i < iMax);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::IsBalancedCompanion3 (Real fA10, Real fA21,
|
|
Real fA02, Real fA12, Real fA22)
|
|
{
|
|
const Real fTolerance = (Real)0.001;
|
|
|
|
// row/col 0
|
|
Real fRowNorm = fA02;
|
|
Real fColNorm = fA10;
|
|
Real fTest = Math<Real>::FAbs((Real)1.0 - fColNorm/fRowNorm);
|
|
if (fTest > fTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// row/col 1
|
|
fRowNorm = (fA10 >= fA12 ? fA10 : fA12);
|
|
fColNorm = fA21;
|
|
fTest = Math<Real>::FAbs((Real)1.0 - fColNorm/fRowNorm);
|
|
if (fTest > fTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// row/col 2
|
|
fRowNorm = (fA21 >= fA22 ? fA21 : fA22);
|
|
fColNorm = (fA02 >= fA12 ? fA02 : fA12);
|
|
if (fA22 > fColNorm)
|
|
{
|
|
fColNorm = fA22;
|
|
}
|
|
fTest = Math<Real>::FAbs((Real)1.0 - fColNorm/fRowNorm);
|
|
return fTest <= fTolerance;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::BalanceCompanion3 (GMatrix<Real>& rkMat)
|
|
{
|
|
Real fA10 = Math<Real>::FAbs(rkMat[1][0]);
|
|
Real fA21 = Math<Real>::FAbs(rkMat[2][1]);
|
|
Real fA02 = Math<Real>::FAbs(rkMat[0][2]);
|
|
Real fA12 = Math<Real>::FAbs(rkMat[1][2]);
|
|
Real fA22 = Math<Real>::FAbs(rkMat[2][2]);
|
|
Real fRowNorm, fColNorm, fScale, fInvScale;
|
|
|
|
const int iMax = 16;
|
|
int i;
|
|
for (i = 0; i < iMax; i++)
|
|
{
|
|
// balance row/col 0
|
|
fRowNorm = fA02;
|
|
fColNorm = fA10;
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fA02 *= fScale;
|
|
fA10 = fA02;
|
|
|
|
// balance row/col 1
|
|
fRowNorm = (fA10 >= fA12 ? fA10 : fA12);
|
|
fColNorm = fA21;
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fInvScale = ((Real)1.0)/fScale;
|
|
fA10 *= fScale;
|
|
fA12 *= fScale;
|
|
fA21 *= fInvScale;
|
|
|
|
// balance row/col 2
|
|
fRowNorm = (fA21 >= fA22 ? fA21 : fA22);
|
|
fColNorm = (fA02 >= fA12 ? fA02 : fA12);
|
|
if (fA22 > fColNorm)
|
|
{
|
|
fColNorm = fA22;
|
|
}
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fInvScale = ((Real)1.0)/fScale;
|
|
fA21 *= fScale;
|
|
fA02 *= fInvScale;
|
|
fA12 *= fInvScale;
|
|
|
|
if (IsBalancedCompanion3(fA10,fA21,fA02,fA12,fA22))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
assert(i < iMax);
|
|
|
|
rkMat[1][0] = (rkMat[1][0] >= (Real)0.0 ? fA10 : -fA10);
|
|
rkMat[2][1] = (rkMat[2][1] >= (Real)0.0 ? fA21 : -fA21);
|
|
rkMat[0][2] = (rkMat[0][2] >= (Real)0.0 ? fA02 : -fA02);
|
|
rkMat[1][2] = (rkMat[1][2] >= (Real)0.0 ? fA12 : -fA12);
|
|
rkMat[2][2] = (rkMat[2][2] >= (Real)0.0 ? fA22 : -fA22);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::QRIteration3 (GMatrix<Real>& rkMat)
|
|
{
|
|
GVector<Real> kW(3);
|
|
Real fRHS, fTrace, fDet;
|
|
for (int i = 0; i < m_iMaxIterations; i++)
|
|
{
|
|
fRHS = m_fEpsilon*(Math<Real>::FAbs(rkMat[0][0]) +
|
|
Math<Real>::FAbs(rkMat[1][1]));
|
|
|
|
if (Math<Real>::FAbs(rkMat[1][0]) <= fRHS)
|
|
{
|
|
// mat[0][0] is a root, solve the quadratic for the submatrix
|
|
fTrace = rkMat[1][1] + rkMat[2][2];
|
|
fDet = rkMat[1][1]*rkMat[2][2] - rkMat[1][2]*rkMat[2][1];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
m_afRoot[m_iCount++] = rkMat[0][0];
|
|
return true;
|
|
}
|
|
|
|
fRHS = m_fEpsilon*(Math<Real>::FAbs(rkMat[1][1]) +
|
|
Math<Real>::FAbs(rkMat[2][2]));
|
|
|
|
if (Math<Real>::FAbs(rkMat[2][1]) <= fRHS)
|
|
{
|
|
// mat[2][2] is a root, solve the quadratic for the submatrix
|
|
fTrace = rkMat[0][0] + rkMat[1][1];
|
|
fDet = rkMat[0][0]*rkMat[1][1] - rkMat[0][1]*rkMat[1][0];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
m_afRoot[m_iCount++] = rkMat[2][2];
|
|
return true;
|
|
}
|
|
|
|
FrancisQRStep(rkMat,kW);
|
|
}
|
|
|
|
// TO DO: In theory, cubic polynomials always have one real-valued root,
|
|
// but if the maximum iterations were exceeded, what to do? Some
|
|
// experiments show that when the polynomial nearly has a double root,
|
|
// the convergence of the algorithm is slow. Maybe a random perturbation
|
|
// to "kick" the system a bit might work?
|
|
//
|
|
// If you want to trap exceeding the maximum iterations, uncomment the
|
|
// 'assert' line of code.
|
|
//
|
|
// assert( false );
|
|
|
|
// For now, zero out the smallest subdiagonal entry to decouple the
|
|
// matrix.
|
|
if (Math<Real>::FAbs(rkMat[1][0]) <= Math<Real>::FAbs(rkMat[2][1]))
|
|
{
|
|
// mat[0][0] is a root, solve the quadratic for the submatrix
|
|
fTrace = rkMat[1][1] + rkMat[2][2];
|
|
fDet = rkMat[1][1]*rkMat[2][2] - rkMat[1][2]*rkMat[2][1];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
m_afRoot[m_iCount++] = rkMat[0][0];
|
|
}
|
|
else
|
|
{
|
|
// mat[2][2] is a root, solve the quadratic for the submatrix
|
|
fTrace = rkMat[0][0] + rkMat[1][1];
|
|
fDet = rkMat[0][0]*rkMat[1][1] - rkMat[0][1]*rkMat[1][0];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
m_afRoot[m_iCount++] = rkMat[2][2];
|
|
}
|
|
|
|
return true;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// degree 4
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindA (Real fC0, Real fC1, Real fC2, Real fC3,
|
|
Real fC4)
|
|
{
|
|
if (Math<Real>::FAbs(fC4) <= m_fEpsilon)
|
|
{
|
|
// polynomial is cubic
|
|
return FindA(fC0,fC1,fC2,fC3);
|
|
}
|
|
|
|
// make polynomial monic, x^4+c3*x^3+c2*x^2+c1*x+c0
|
|
Real fInvC4 = ((Real)1.0)/fC4;
|
|
fC0 *= fInvC4;
|
|
fC1 *= fInvC4;
|
|
fC2 *= fInvC4;
|
|
fC3 *= fInvC4;
|
|
|
|
// reduction to resolvent cubic polynomial y^3+r2*y^2+r1*y+r0 = 0
|
|
Real fR0 = -fC3*fC3*fC0 + ((Real)4.0)*fC2*fC0 - fC1*fC1;
|
|
Real fR1 = fC3*fC1 - ((Real)4.0)*fC0;
|
|
Real fR2 = -fC2;
|
|
FindA(fR0,fR1,fR2,(Real)1.0); // always produces at least one root
|
|
Real fY = m_afRoot[0];
|
|
|
|
m_iCount = 0;
|
|
Real fDiscr = ((Real)0.25)*fC3*fC3 - fC2 + fY;
|
|
if (Math<Real>::FAbs(fDiscr) <= m_fEpsilon)
|
|
{
|
|
fDiscr = (Real)0.0;
|
|
}
|
|
|
|
if (fDiscr > (Real)0.0)
|
|
{
|
|
Real fR = Math<Real>::Sqrt(fDiscr);
|
|
Real fT1 = ((Real)0.75)*fC3*fC3 - fR*fR - ((Real)2.0)*fC2;
|
|
Real fT2 = (((Real)4.0)*fC3*fC2 - ((Real)8.0)*fC1 - fC3*fC3*fC3) /
|
|
(((Real)4.0)*fR);
|
|
|
|
Real fTplus = fT1+fT2;
|
|
Real fTminus = fT1-fT2;
|
|
if (Math<Real>::FAbs(fTplus) <= m_fEpsilon)
|
|
{
|
|
fTplus = (Real)0.0;
|
|
}
|
|
if (Math<Real>::FAbs(fTminus) <= m_fEpsilon)
|
|
{
|
|
fTminus = (Real)0.0;
|
|
}
|
|
|
|
if (fTplus >= (Real)0.0)
|
|
{
|
|
Real fD = Math<Real>::Sqrt(fTplus);
|
|
m_afRoot[0] = -((Real)0.25)*fC3+((Real)0.5)*(fR+fD);
|
|
m_afRoot[1] = -((Real)0.25)*fC3+((Real)0.5)*(fR-fD);
|
|
m_iCount += 2;
|
|
}
|
|
if (fTminus >= (Real)0.0)
|
|
{
|
|
Real fE = Math<Real>::Sqrt(fTminus);
|
|
m_afRoot[m_iCount++] = -((Real)0.25)*fC3+((Real)0.5)*(fE-fR);
|
|
m_afRoot[m_iCount++] = -((Real)0.25)*fC3-((Real)0.5)*(fE+fR);
|
|
}
|
|
}
|
|
else if (fDiscr < (Real)0.0)
|
|
{
|
|
m_iCount = 0;
|
|
}
|
|
else
|
|
{
|
|
Real fT2 = fY*fY-((Real)4.0)*fC0;
|
|
if (fT2 >= -m_fEpsilon)
|
|
{
|
|
if (fT2 < (Real)0.0) // round to zero
|
|
{
|
|
fT2 = (Real)0.0;
|
|
}
|
|
fT2 = ((Real)2.0)*Math<Real>::Sqrt(fT2);
|
|
Real fT1 = ((Real)0.75)*fC3*fC3 - ((Real)2.0)*fC2;
|
|
if (fT1+fT2 >= m_fEpsilon)
|
|
{
|
|
Real fD = Math<Real>::Sqrt(fT1+fT2);
|
|
m_afRoot[0] = -((Real)0.25)*fC3+((Real)0.5)*fD;
|
|
m_afRoot[1] = -((Real)0.25)*fC3-((Real)0.5)*fD;
|
|
m_iCount += 2;
|
|
}
|
|
if (fT1-fT2 >= m_fEpsilon)
|
|
{
|
|
Real fE = Math<Real>::Sqrt(fT1-fT2);
|
|
m_afRoot[m_iCount++] = -((Real)0.25)*fC3+((Real)0.5)*fE;
|
|
m_afRoot[m_iCount++] = -((Real)0.25)*fC3-((Real)0.5)*fE;
|
|
}
|
|
}
|
|
}
|
|
|
|
return m_iCount > 0;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindE (Real fC0, Real fC1, Real fC2, Real fC3,
|
|
Real fC4, bool bDoBalancing)
|
|
{
|
|
if (Math<Real>::FAbs(fC4) <= m_fEpsilon)
|
|
{
|
|
// polynomial is cubic
|
|
return FindA(fC0,fC1,fC2,fC3);
|
|
}
|
|
|
|
// make polynomial monic, x^4+c3*x^3+c2*x^2+c1*x+c0
|
|
Real fInvC4 = ((Real)1.0)/fC4;
|
|
fC0 *= fInvC4;
|
|
fC1 *= fInvC4;
|
|
fC2 *= fInvC4;
|
|
fC3 *= fInvC4;
|
|
|
|
// construct the 4-by-4 companion matrix
|
|
GMatrix<Real> kMat(4,4); // initialized to zero
|
|
kMat[1][0] = (Real)1.0;
|
|
kMat[2][1] = (Real)1.0;
|
|
kMat[3][2] = (Real)1.0;
|
|
kMat[0][3] = -fC0;
|
|
kMat[1][3] = -fC1;
|
|
kMat[2][3] = -fC2;
|
|
kMat[3][3] = -fC3;
|
|
|
|
if (bDoBalancing)
|
|
{
|
|
BalanceCompanion4(kMat);
|
|
}
|
|
|
|
return QRIteration4(kMat);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetBound (Real fC0, Real fC1, Real fC2, Real fC3,
|
|
Real fC4)
|
|
{
|
|
if (Math<Real>::FAbs(fC4) <= m_fEpsilon)
|
|
{
|
|
// polynomial is cubic
|
|
return GetBound(fC0,fC1,fC2,fC3);
|
|
}
|
|
|
|
Real fInvC4 = ((Real)1.0)/fC4;
|
|
Real fMax = Math<Real>::FAbs(fC0)*fInvC4;
|
|
|
|
Real fTmp = Math<Real>::FAbs(fC1)*fInvC4;
|
|
if (fTmp > fMax)
|
|
{
|
|
fMax = fTmp;
|
|
}
|
|
|
|
fTmp = Math<Real>::FAbs(fC2)*fInvC4;
|
|
if (fTmp > fMax)
|
|
{
|
|
fMax = fTmp;
|
|
}
|
|
|
|
fTmp = Math<Real>::FAbs(fC3)*fInvC4;
|
|
if (fTmp > fMax)
|
|
{
|
|
fMax = fTmp;
|
|
}
|
|
|
|
return (Real)1.0 + fMax;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::IsBalancedCompanion4 (Real fA10, Real fA21,
|
|
Real fA32, Real fA03, Real fA13, Real fA23, Real fA33)
|
|
{
|
|
const Real fTolerance = (Real)0.001;
|
|
|
|
// row/col 0
|
|
Real fRowNorm = fA03;
|
|
Real fColNorm = fA10;
|
|
Real fTest = Math<Real>::FAbs(1.0f - fColNorm/fRowNorm);
|
|
if (fTest > fTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// row/col 1
|
|
fRowNorm = (fA10 >= fA13 ? fA10 : fA13);
|
|
fColNorm = fA21;
|
|
fTest = Math<Real>::FAbs(1.0f - fColNorm/fRowNorm);
|
|
if (fTest > fTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// row/col 2
|
|
fRowNorm = (fA21 >= fA23 ? fA21 : fA23);
|
|
fColNorm = fA32;
|
|
fTest = Math<Real>::FAbs(1.0f - fColNorm/fRowNorm);
|
|
if (fTest > fTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// row/col 3
|
|
fRowNorm = (fA32 >= fA33 ? fA32 : fA33);
|
|
fColNorm = (fA03 >= fA13 ? fA03 : fA13);
|
|
if (fA23 > fColNorm)
|
|
{
|
|
fColNorm = fA23;
|
|
}
|
|
if (fA33 > fColNorm)
|
|
{
|
|
fColNorm = fA33;
|
|
}
|
|
fTest = Math<Real>::FAbs(1.0f - fColNorm/fRowNorm);
|
|
return fTest <= fTolerance;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::BalanceCompanion4 (GMatrix<Real>& rkMat)
|
|
{
|
|
Real fA10 = Math<Real>::FAbs(rkMat[1][0]);
|
|
Real fA21 = Math<Real>::FAbs(rkMat[2][1]);
|
|
Real fA32 = Math<Real>::FAbs(rkMat[3][2]);
|
|
Real fA03 = Math<Real>::FAbs(rkMat[0][3]);
|
|
Real fA13 = Math<Real>::FAbs(rkMat[1][3]);
|
|
Real fA23 = Math<Real>::FAbs(rkMat[2][3]);
|
|
Real fA33 = Math<Real>::FAbs(rkMat[3][3]);
|
|
Real fRowNorm, fColNorm, fScale, fInvScale;
|
|
|
|
const int iMax = 16;
|
|
int i;
|
|
for (i = 0; i < iMax; i++)
|
|
{
|
|
// balance row/col 0
|
|
fRowNorm = fA03;
|
|
fColNorm = fA10;
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fA03 *= fScale;
|
|
fA10 = fA03;
|
|
|
|
// balance row/col 1
|
|
fRowNorm = (fA10 >= fA13 ? fA10 : fA13);
|
|
fColNorm = fA21;
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fInvScale = ((Real)1.0)/fScale;
|
|
fA10 *= fScale;
|
|
fA13 *= fScale;
|
|
fA21 *= fInvScale;
|
|
|
|
// balance row/col 2
|
|
fRowNorm = (fA21 >= fA23 ? fA21 : fA23);
|
|
fColNorm = fA32;
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fInvScale = ((Real)1.0)/fScale;
|
|
fA21 *= fScale;
|
|
fA23 *= fScale;
|
|
fA32 *= fInvScale;
|
|
|
|
// balance row/col 3
|
|
fRowNorm = (fA32 >= fA33 ? fA32 : fA33);
|
|
fColNorm = (fA03 >= fA13 ? fA03 : fA13);
|
|
if (fA23 > fColNorm)
|
|
{
|
|
fColNorm = fA23;
|
|
}
|
|
if (fA33 > fColNorm)
|
|
{
|
|
fColNorm = fA33;
|
|
}
|
|
fScale = Math<Real>::Sqrt(fColNorm/fRowNorm);
|
|
fInvScale = ((Real)1.0)/fScale;
|
|
fA32 *= fScale;
|
|
fA03 *= fInvScale;
|
|
fA13 *= fInvScale;
|
|
fA23 *= fInvScale;
|
|
|
|
if (IsBalancedCompanion4(fA10,fA21,fA32,fA03,fA13,fA23,fA33))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
assert(i < iMax);
|
|
|
|
rkMat[1][0] = (rkMat[1][0] >= (Real)0.0 ? fA10 : -fA10);
|
|
rkMat[2][1] = (rkMat[2][1] >= (Real)0.0 ? fA21 : -fA21);
|
|
rkMat[3][2] = (rkMat[3][2] >= (Real)0.0 ? fA32 : -fA32);
|
|
rkMat[0][3] = (rkMat[0][3] >= (Real)0.0 ? fA03 : -fA03);
|
|
rkMat[1][3] = (rkMat[1][3] >= (Real)0.0 ? fA13 : -fA13);
|
|
rkMat[2][3] = (rkMat[2][3] >= (Real)0.0 ? fA23 : -fA23);
|
|
rkMat[3][3] = (rkMat[3][3] >= (Real)0.0 ? fA33 : -fA33);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::QRIteration4 (GMatrix<Real>& rkMat)
|
|
{
|
|
GVector<Real> kW(4);
|
|
GMatrix<Real> kMS(3,3);
|
|
Real fRHS, fTrace, fDet, afSaveRoot[2];
|
|
int i, j, iSaveCount;
|
|
for (i = 0; i < m_iMaxIterations; i++)
|
|
{
|
|
fRHS = m_fEpsilon*(Math<Real>::FAbs(rkMat[0][0]) +
|
|
Math<Real>::FAbs(rkMat[1][1]));
|
|
|
|
if (Math<Real>::FAbs(rkMat[1][0]) <= fRHS)
|
|
{
|
|
// mat[0][0] is a root, reduce the 3-by-3 submatrix
|
|
// TO DO: Avoid the copy and pass row/column offsets to the
|
|
// FrancisQR method.
|
|
kMS[0][0] = rkMat[1][1];
|
|
kMS[0][1] = rkMat[1][2];
|
|
kMS[0][2] = rkMat[1][3];
|
|
kMS[1][0] = rkMat[2][1];
|
|
kMS[1][1] = rkMat[2][2];
|
|
kMS[1][2] = rkMat[2][3];
|
|
kMS[2][0] = rkMat[3][1];
|
|
kMS[2][1] = rkMat[3][2];
|
|
kMS[2][2] = rkMat[3][3];
|
|
QRIteration3(kMS);
|
|
m_afRoot[m_iCount++] = rkMat[0][0];
|
|
return true;
|
|
}
|
|
|
|
fRHS = m_fEpsilon*(Math<Real>::FAbs(rkMat[1][1]) +
|
|
Math<Real>::FAbs(rkMat[2][2]));
|
|
|
|
if (Math<Real>::FAbs(rkMat[2][1]) <= fRHS)
|
|
{
|
|
// The matrix is decoupled into two 2-by-2 blocks. Solve the
|
|
// quadratics for the blocks.
|
|
fTrace = rkMat[0][0] + rkMat[1][1];
|
|
fDet = rkMat[0][0]*rkMat[1][1] - rkMat[0][1]*rkMat[1][0];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
iSaveCount = m_iCount;
|
|
for (j = 0; j < iSaveCount; j++)
|
|
{
|
|
afSaveRoot[j] = m_afRoot[j];
|
|
}
|
|
|
|
fTrace = rkMat[2][2] + rkMat[3][3];
|
|
fDet = rkMat[2][2]*rkMat[3][3] - rkMat[2][3]*rkMat[3][2];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
for (j = 0; j < iSaveCount; j++)
|
|
{
|
|
m_afRoot[m_iCount++] = afSaveRoot[j];
|
|
}
|
|
return m_iCount > 0;
|
|
}
|
|
|
|
fRHS = m_fEpsilon*(Math<Real>::FAbs(rkMat[2][2]) +
|
|
Math<Real>::FAbs(rkMat[3][3]));
|
|
|
|
if (Math<Real>::FAbs(rkMat[3][2]) <= fRHS)
|
|
{
|
|
// mat[3][3] is a root, reduce the 3-by-3 submatrix
|
|
// TO DO: Avoid the copy and pass row/column offsets to the
|
|
// FrancisQR method.
|
|
kMS[0][0] = rkMat[0][0];
|
|
kMS[0][1] = rkMat[0][1];
|
|
kMS[0][2] = rkMat[0][2];
|
|
kMS[1][0] = rkMat[1][0];
|
|
kMS[1][1] = rkMat[1][1];
|
|
kMS[1][2] = rkMat[1][2];
|
|
kMS[2][0] = rkMat[2][0];
|
|
kMS[2][1] = rkMat[2][1];
|
|
kMS[2][2] = rkMat[2][2];
|
|
QRIteration3(kMS);
|
|
m_afRoot[m_iCount++] = rkMat[3][3];
|
|
return true;
|
|
}
|
|
|
|
FrancisQRStep(rkMat,kW);
|
|
}
|
|
|
|
// TO DO: What to do if the maximum iterations were exceeded? Maybe a
|
|
// random perturbation to "kick" the system a bit might work?
|
|
//
|
|
// If you want to trap exceeding the maximum iterations, uncomment the
|
|
// 'assert' line of code.
|
|
//
|
|
// assert( false );
|
|
|
|
// For now, decouple the matrix using the smallest subdiagonal entry.
|
|
i = 0;
|
|
Real fMin = Math<Real>::FAbs(rkMat[1][0]);
|
|
Real fAbs = Math<Real>::FAbs(rkMat[2][1]);
|
|
if (fAbs < fMin)
|
|
{
|
|
fMin = fAbs;
|
|
i = 1;
|
|
}
|
|
fAbs = Math<Real>::FAbs(rkMat[3][2]);
|
|
if (fAbs < fMin)
|
|
{
|
|
fMin = fAbs;
|
|
i = 2;
|
|
}
|
|
|
|
if (i == 0)
|
|
{
|
|
// mat[0][0] is a root, reduce the 3-by-3 submatrix
|
|
// TO DO: Avoid the copy and pass row/column offsets to the
|
|
// FrancisQR method.
|
|
kMS[0][0] = rkMat[1][1];
|
|
kMS[0][1] = rkMat[1][2];
|
|
kMS[0][2] = rkMat[1][3];
|
|
kMS[1][0] = rkMat[2][1];
|
|
kMS[1][1] = rkMat[2][2];
|
|
kMS[1][2] = rkMat[2][3];
|
|
kMS[2][0] = rkMat[3][1];
|
|
kMS[2][1] = rkMat[3][2];
|
|
kMS[2][2] = rkMat[3][3];
|
|
QRIteration3(kMS);
|
|
m_afRoot[m_iCount++] = rkMat[0][0];
|
|
}
|
|
else if (i == 1)
|
|
{
|
|
// The matrix is decoupled into two 2-by-2 blocks. Solve the
|
|
// quadratics for the blocks.
|
|
fTrace = rkMat[0][0] + rkMat[1][1];
|
|
fDet = rkMat[0][0]*rkMat[1][1] - rkMat[0][1]*rkMat[1][0];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
iSaveCount = m_iCount;
|
|
for (j = 0; j < iSaveCount; j++)
|
|
{
|
|
afSaveRoot[j] = m_afRoot[j];
|
|
}
|
|
|
|
fTrace = rkMat[2][2] + rkMat[3][3];
|
|
fDet = rkMat[2][2]*rkMat[3][3] - rkMat[2][3]*rkMat[3][2];
|
|
FindA(fDet,-fTrace,(Real)1.0);
|
|
for (j = 0; j < iSaveCount; j++)
|
|
{
|
|
m_afRoot[m_iCount++] = afSaveRoot[j];
|
|
}
|
|
}
|
|
else // i == 2
|
|
{
|
|
// mat[3][3] is a root, reduce the 3-by-3 submatrix
|
|
// TO DO: Avoid the copy and pass row/column offsets to the
|
|
// FrancisQR method.
|
|
kMS[0][0] = rkMat[0][0];
|
|
kMS[0][1] = rkMat[0][1];
|
|
kMS[0][2] = rkMat[0][2];
|
|
kMS[1][0] = rkMat[1][0];
|
|
kMS[1][1] = rkMat[1][1];
|
|
kMS[1][2] = rkMat[1][2];
|
|
kMS[2][0] = rkMat[2][0];
|
|
kMS[2][1] = rkMat[2][1];
|
|
kMS[2][2] = rkMat[2][2];
|
|
QRIteration3(kMS);
|
|
m_afRoot[m_iCount++] = rkMat[3][3];
|
|
}
|
|
|
|
return m_iCount > 0;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// degree N
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindB (const Polynomial1<Real>& rkPoly,
|
|
int iDigits)
|
|
{
|
|
Real fBound = GetBound(rkPoly);
|
|
return FindB(rkPoly,-fBound,fBound,iDigits);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindN (const Polynomial1<Real>&, int)
|
|
{
|
|
// TO DO: Implement this.
|
|
assert(false);
|
|
return false;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindE (const Polynomial1<Real>&, bool)
|
|
{
|
|
// TO DO: Implement this.
|
|
assert(false);
|
|
return false;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
Real PolynomialRoots<Real>::GetBound (const Polynomial1<Real>& rkPoly)
|
|
{
|
|
Polynomial1<Real> kCPoly = rkPoly;
|
|
kCPoly.Compress(m_fEpsilon);
|
|
int iDegree = kCPoly.GetDegree();
|
|
if (iDegree < 1)
|
|
{
|
|
// polynomial is constant, return invalid bound
|
|
return -(Real)1.0;
|
|
}
|
|
|
|
Real fInvCDeg = ((Real)1.0)/kCPoly[iDegree];
|
|
Real fMax = (Real)0.0;
|
|
for (int i = 0; i < iDegree; i++)
|
|
{
|
|
Real fTmp = Math<Real>::FAbs(kCPoly[i])*fInvCDeg;
|
|
if (fTmp > fMax)
|
|
{
|
|
fMax = fTmp;
|
|
}
|
|
}
|
|
|
|
return (Real)1.0 + fMax;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindB (const Polynomial1<Real>& rkPoly,
|
|
Real fXMin, Real fXMax, int iDigits)
|
|
{
|
|
// reallocate root array if necessary
|
|
if (rkPoly.GetDegree() > m_iMaxRoot)
|
|
{
|
|
m_iMaxRoot = rkPoly.GetDegree();
|
|
WM4_DELETE[] m_afRoot;
|
|
m_afRoot = WM4_NEW Real[m_iMaxRoot];
|
|
}
|
|
|
|
Real fRoot;
|
|
if (rkPoly.GetDegree() == 1)
|
|
{
|
|
if (Bisection(rkPoly,fXMin,fXMax,iDigits,fRoot))
|
|
{
|
|
m_iCount = 1;
|
|
m_afRoot[0] = fRoot;
|
|
return true;
|
|
}
|
|
m_iCount = 0;
|
|
return false;
|
|
}
|
|
|
|
// get roots of derivative polynomial
|
|
Polynomial1<Real> kDeriv = rkPoly.GetDerivative();
|
|
FindB(kDeriv,fXMin,fXMax,iDigits);
|
|
|
|
int i, iNewCount = 0;
|
|
Real* afNewRoot = WM4_NEW Real[m_iCount+1];
|
|
|
|
if ( m_iCount > 0 )
|
|
{
|
|
// find root on [xmin,root[0]]
|
|
if (Bisection(rkPoly,fXMin,m_afRoot[0],iDigits,fRoot))
|
|
{
|
|
afNewRoot[iNewCount++] = fRoot;
|
|
}
|
|
|
|
// find root on [root[i],root[i+1]] for 0 <= i <= count-2
|
|
for (i = 0; i <= m_iCount-2; i++)
|
|
{
|
|
if (Bisection(rkPoly,m_afRoot[i],m_afRoot[i+1],iDigits,fRoot))
|
|
{
|
|
afNewRoot[iNewCount++] = fRoot;
|
|
}
|
|
}
|
|
|
|
// find root on [root[count-1],xmax]
|
|
if (Bisection(rkPoly,m_afRoot[m_iCount-1],fXMax,iDigits,fRoot))
|
|
{
|
|
afNewRoot[iNewCount++] = fRoot;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// polynomial is monotone on [xmin,xmax], has at most one root
|
|
if (Bisection(rkPoly,fXMin,fXMax,iDigits,fRoot))
|
|
{
|
|
afNewRoot[iNewCount++] = fRoot;
|
|
}
|
|
}
|
|
|
|
// copy to old buffer
|
|
if (iNewCount > 0)
|
|
{
|
|
m_iCount = 1;
|
|
m_afRoot[0] = afNewRoot[0];
|
|
for (i = 1; i < iNewCount; i++)
|
|
{
|
|
Real fRootDiff = afNewRoot[i]-afNewRoot[i-1];
|
|
if (Math<Real>::FAbs(fRootDiff) > m_fEpsilon)
|
|
{
|
|
m_afRoot[m_iCount++] = afNewRoot[i];
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_iCount = 0;
|
|
}
|
|
|
|
WM4_DELETE[] afNewRoot;
|
|
return m_iCount > 0;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::FindN (const Polynomial1<Real>&, Real, Real, int)
|
|
{
|
|
// TO DO: Implement this.
|
|
assert(false);
|
|
return false;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::AllRealPartsNegative (
|
|
const Polynomial1<Real>& rkPoly)
|
|
{
|
|
// make a copy of coefficients, later calls will change the copy
|
|
int iDegree = rkPoly.GetDegree();
|
|
const Real* afPolyCoeff = (const Real*)rkPoly;
|
|
Real* afCoeff = WM4_NEW Real[iDegree+1];
|
|
size_t uiSize = (iDegree+1)*sizeof(Real);
|
|
System::Memcpy(afCoeff,uiSize,afPolyCoeff,uiSize);
|
|
|
|
// make polynomial monic
|
|
if (afCoeff[iDegree] != (Real)1.0)
|
|
{
|
|
Real fInv = ((Real)1.0)/afCoeff[iDegree];
|
|
for (int i = 0; i < iDegree; i++)
|
|
{
|
|
afCoeff[i] *= fInv;
|
|
}
|
|
afCoeff[iDegree] = (Real)1.0;
|
|
}
|
|
|
|
return AllRealPartsNegative(iDegree,afCoeff);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::AllRealPartsPositive (
|
|
const Polynomial1<Real>& rkPoly)
|
|
{
|
|
// make a copy of coefficients, later calls will change the copy
|
|
int iDegree = rkPoly.GetDegree();
|
|
const Real* afPolyCoeff = (const Real*)rkPoly;
|
|
Real* afCoeff = WM4_NEW Real[iDegree+1];
|
|
size_t uiSize = (iDegree+1)*sizeof(Real);
|
|
System::Memcpy(afCoeff,uiSize,afPolyCoeff,uiSize);
|
|
|
|
// make polynomial monic
|
|
int i;
|
|
if (afCoeff[iDegree] != (Real)1.0)
|
|
{
|
|
Real fInv = ((Real)1.0)/afCoeff[iDegree];
|
|
for (i = 0; i < iDegree; i++)
|
|
{
|
|
afCoeff[i] *= fInv;
|
|
}
|
|
afCoeff[iDegree] = (Real)1.0;
|
|
}
|
|
|
|
// reflect z -> -z
|
|
int iSign = -1;
|
|
for (i = iDegree-1; i >= 0; i--, iSign = -iSign)
|
|
{
|
|
afCoeff[i] *= iSign;
|
|
}
|
|
|
|
return AllRealPartsNegative(iDegree,afCoeff);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::AllRealPartsNegative (int iDegree, Real* afCoeff)
|
|
{
|
|
// assert: afCoeff[iDegree] = 1
|
|
|
|
if (afCoeff[iDegree-1] <= (Real)0.0)
|
|
{
|
|
return false;
|
|
}
|
|
if (iDegree == 1)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
Real* afTmpCoeff = WM4_NEW Real[iDegree];
|
|
afTmpCoeff[0] = ((Real)2.0)*afCoeff[0]*afCoeff[iDegree-1];
|
|
int i;
|
|
for (i = 1; i <= iDegree-2; i++)
|
|
{
|
|
afTmpCoeff[i] = afCoeff[iDegree-1]*afCoeff[i];
|
|
if (((iDegree-i) % 2) == 0)
|
|
{
|
|
afTmpCoeff[i] -= afCoeff[i-1];
|
|
}
|
|
afTmpCoeff[i] *= (Real)2.0;
|
|
}
|
|
afTmpCoeff[iDegree-1] =
|
|
((Real)2.0)*afCoeff[iDegree-1]*afCoeff[iDegree-1];
|
|
|
|
int iNextDegree;
|
|
for (iNextDegree = iDegree-1; iNextDegree >= 0; iNextDegree--)
|
|
{
|
|
if (afTmpCoeff[iNextDegree] != (Real)0.0)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
for (i = 0; i <= iNextDegree-1; i++)
|
|
{
|
|
afCoeff[i] = afTmpCoeff[i]/afTmpCoeff[iNextDegree];
|
|
}
|
|
WM4_DELETE[] afTmpCoeff;
|
|
|
|
return AllRealPartsNegative(iNextDegree,afCoeff);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
int PolynomialRoots<Real>::GetRootCount (const Polynomial1<Real>& rkPoly,
|
|
Real fT0, Real fT1)
|
|
{
|
|
int iDegree = rkPoly.GetDegree();
|
|
const Real* afCoeff = (const Real*)rkPoly;
|
|
|
|
if (iDegree == 0)
|
|
{
|
|
// polynomial is constant on the interval
|
|
if (afCoeff[0] != (Real)0.0)
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
return -1; // to indicate "infinitely many"
|
|
}
|
|
}
|
|
|
|
// generate the Sturm sequence
|
|
std::vector<Polynomial1<Real>*> kSturm;
|
|
Polynomial1<Real>* pkF0 = WM4_NEW Polynomial1<Real>(rkPoly);
|
|
Polynomial1<Real>* pkF1 = WM4_NEW Polynomial1<Real>(
|
|
pkF0->GetDerivative());
|
|
kSturm.push_back(pkF0);
|
|
kSturm.push_back(pkF1);
|
|
|
|
while (pkF1->GetDegree() > 0)
|
|
{
|
|
Polynomial1<Real>* pkF2 = WM4_NEW Polynomial1<Real>(-1);
|
|
Polynomial1<Real> kQuot;
|
|
pkF0->Divide(*pkF1,kQuot,*pkF2,Math<Real>::ZERO_TOLERANCE);
|
|
*pkF2 = -(*pkF2);
|
|
kSturm.push_back(pkF2);
|
|
pkF0 = pkF1;
|
|
pkF1 = pkF2;
|
|
}
|
|
|
|
int i;
|
|
Real fValue0, fValue1;
|
|
|
|
// count the sign changes at t0
|
|
int iSignChanges0 = 0;
|
|
if (fT0 == -Math<Real>::MAX_REAL)
|
|
{
|
|
pkF0 = kSturm[0];
|
|
if (pkF0->GetDegree() & 1)
|
|
{
|
|
fValue0 = -(*pkF0)[pkF0->GetDegree()];
|
|
}
|
|
else
|
|
{
|
|
fValue0 = (*pkF0)[pkF0->GetDegree()];
|
|
}
|
|
|
|
if (Math<Real>::FAbs(fValue0) < m_fEpsilon)
|
|
{
|
|
fValue0 = (Real)0.0;
|
|
}
|
|
|
|
for (i = 1; i < (int)kSturm.size(); i++)
|
|
{
|
|
pkF1 = kSturm[i];
|
|
|
|
if (pkF1->GetDegree() & 1)
|
|
{
|
|
fValue1 = -(*pkF1)[pkF1->GetDegree()];
|
|
}
|
|
else
|
|
{
|
|
fValue1 = (*pkF1)[pkF1->GetDegree()];
|
|
}
|
|
|
|
if (Math<Real>::FAbs(fValue1) < m_fEpsilon)
|
|
{
|
|
fValue1 = (Real)0.0;
|
|
}
|
|
|
|
if (fValue0*fValue1 < (Real)0.0 || fValue0 == (Real)0.0)
|
|
{
|
|
iSignChanges0++;
|
|
}
|
|
|
|
fValue0 = fValue1;
|
|
pkF0 = pkF1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pkF0 = kSturm[0];
|
|
fValue0 = (*pkF0)(fT0);
|
|
if (Math<Real>::FAbs(fValue0) < m_fEpsilon)
|
|
{
|
|
fValue0 = (Real)0.0;
|
|
}
|
|
|
|
for (i = 1; i < (int)kSturm.size(); i++)
|
|
{
|
|
pkF1 = kSturm[i];
|
|
fValue1 = (*pkF1)(fT0);
|
|
if (Math<Real>::FAbs(fValue1) < m_fEpsilon)
|
|
{
|
|
fValue1 = (Real)0.0;
|
|
}
|
|
|
|
if (fValue0*fValue1 < (Real)0.0 || fValue0 == (Real)0.0)
|
|
{
|
|
iSignChanges0++;
|
|
}
|
|
|
|
fValue0 = fValue1;
|
|
pkF0 = pkF1;
|
|
}
|
|
}
|
|
|
|
// count the sign changes at t1
|
|
int iSignChanges1 = 0;
|
|
if (fT1 == Math<Real>::MAX_REAL)
|
|
{
|
|
pkF0 = kSturm[0];
|
|
fValue0 = (*pkF0)[pkF0->GetDegree()];
|
|
if (Math<Real>::FAbs(fValue0) < m_fEpsilon)
|
|
{
|
|
fValue0 = (Real)0.0;
|
|
}
|
|
|
|
for (i = 1; i < (int)kSturm.size(); i++)
|
|
{
|
|
pkF1 = kSturm[i];
|
|
fValue1 = (*pkF1)[pkF1->GetDegree()];
|
|
if (Math<Real>::FAbs(fValue1) < m_fEpsilon)
|
|
{
|
|
fValue1 = (Real)0.0;
|
|
}
|
|
|
|
if (fValue0*fValue1 < (Real)0.0 || fValue0 == (Real)0.0)
|
|
{
|
|
iSignChanges1++;
|
|
}
|
|
|
|
fValue0 = fValue1;
|
|
pkF0 = pkF1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pkF0 = kSturm[0];
|
|
fValue0 = (*pkF0)(fT1);
|
|
if (Math<Real>::FAbs(fValue0) < m_fEpsilon)
|
|
{
|
|
fValue0 = (Real)0.0;
|
|
}
|
|
|
|
for (i = 1; i < (int)kSturm.size(); i++)
|
|
{
|
|
pkF1 = kSturm[i];
|
|
fValue1 = (*pkF1)(fT1);
|
|
if (Math<Real>::FAbs(fValue1) < m_fEpsilon)
|
|
{
|
|
fValue1 = (Real)0.0;
|
|
}
|
|
|
|
if (fValue0*fValue1 < (Real)0.0 || fValue0 == (Real)0.0 )
|
|
{
|
|
iSignChanges1++;
|
|
}
|
|
|
|
fValue0 = fValue1;
|
|
pkF0 = pkF1;
|
|
}
|
|
}
|
|
|
|
// clean up
|
|
for (i = 0; i < (int)kSturm.size(); i++)
|
|
{
|
|
WM4_DELETE kSturm[i];
|
|
}
|
|
|
|
if (iSignChanges0 >= iSignChanges1)
|
|
{
|
|
return iSignChanges0 - iSignChanges1;
|
|
}
|
|
|
|
// theoretically we should not get here
|
|
assert(false);
|
|
return 0;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
bool PolynomialRoots<Real>::Bisection (const Polynomial1<Real>& rkPoly,
|
|
Real fXMin, Real fXMax, int iDigits, Real& rfRoot)
|
|
{
|
|
Real fP0 = rkPoly(fXMin);
|
|
if (Math<Real>::FAbs(fP0) <= Math<Real>::ZERO_TOLERANCE)
|
|
{
|
|
rfRoot = fXMin;
|
|
return true;
|
|
}
|
|
Real fP1 = rkPoly(fXMax);
|
|
if (Math<Real>::FAbs(fP1) <= Math<Real>::ZERO_TOLERANCE)
|
|
{
|
|
rfRoot = fXMax;
|
|
return true;
|
|
}
|
|
if (fP0*fP1 > (Real)0.0)
|
|
return false;
|
|
|
|
// determine number of iterations to get 'digits' accuracy.
|
|
Real fTmp0 = Math<Real>::Log(fXMax-fXMin);
|
|
Real fTmp1 = ((Real)iDigits)*Math<Real>::Log((Real)10.0);
|
|
Real fArg = (fTmp0 + fTmp1)/Math<Real>::Log((Real)2.0);
|
|
int iMaxIter = (int)(fArg + (Real)0.5);
|
|
|
|
for (int i = 0; i < iMaxIter; i++)
|
|
{
|
|
rfRoot = ((Real)0.5)*(fXMin + fXMax);
|
|
Real fP = rkPoly(rfRoot);
|
|
Real fProduct = fP*fP0;
|
|
if (fProduct < (Real)0.0)
|
|
{
|
|
fXMax = rfRoot;
|
|
fP1 = fP;
|
|
}
|
|
else if (fProduct > (Real)0.0)
|
|
{
|
|
fXMin = rfRoot;
|
|
fP0 = fP;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// FindE support
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::GetHouseholderVector (int iSize,
|
|
const Vector3<Real>& rkU, Vector3<Real>& rkV)
|
|
{
|
|
// Householder vector V:
|
|
// Given a vector U, compute a vector V such that V[0] = 1 and
|
|
// (I-2*V*V^T/|V|^2)*U is zero in all but the first component. The
|
|
// matrix P = I-2*V*V^T/|V|^2 is a Householder transformation, a
|
|
// reflection matrix.
|
|
|
|
Real fLength = rkU[0]*rkU[0];
|
|
int i;
|
|
for (i = 1; i < iSize; i++)
|
|
{
|
|
fLength += rkU[i]*rkU[i];
|
|
}
|
|
fLength = Math<Real>::Sqrt(fLength);
|
|
|
|
if (fLength > m_fEpsilon)
|
|
{
|
|
Real fInv = ((Real)1.0)/(rkU[0]+Math<Real>::Sign(rkU[0])*fLength);
|
|
rkV[0] = (Real)1.0;
|
|
for (i = 1; i < iSize; i++)
|
|
{
|
|
rkV[i] = fInv*rkU[i];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// U is the zero vector, any vector will do
|
|
rkV[0] = (Real)1.0;
|
|
for (i = 1; i < iSize; i++)
|
|
{
|
|
rkV[i] = (Real)0.0;
|
|
}
|
|
}
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::PremultiplyHouseholder (GMatrix<Real>& rkMat,
|
|
GVector<Real>& rkW, int iRMin, int iRMax, int iCMin, int iCMax,
|
|
int iVSize, const Vector3<Real>& rkV)
|
|
{
|
|
// Householder premultiplication:
|
|
// Given a matrix A and an m-by-1 vector V with V[0] = 1, let S be the
|
|
// submatrix of A with m rows rmin <= r <= m+rmin-1 and columns
|
|
// cmin <= c <= cmax. Overwrite S with P*S where P = I-2*V*V^T/|V|^2.
|
|
|
|
int iSubRows = iRMax - iRMin + 1, iSubCols = iCMax - iCMin + 1;
|
|
int iRow, iCol;
|
|
|
|
Real fSqrLen = rkV[0]*rkV[0];
|
|
for (int i = 1; i < iVSize; i++)
|
|
{
|
|
fSqrLen += rkV[i]*rkV[i];
|
|
}
|
|
|
|
Real fBeta = -((Real)2.0)/fSqrLen;
|
|
for (iCol = 0; iCol < iSubCols; iCol++)
|
|
{
|
|
rkW[iCol] = (Real)0.0;
|
|
for (iRow = 0; iRow < iSubRows; iRow++)
|
|
{
|
|
rkW[iCol] += rkV[iRow]*rkMat[iRMin+iRow][iCMin+iCol];
|
|
}
|
|
rkW[iCol] *= fBeta;
|
|
}
|
|
|
|
for (iRow = 0; iRow < iSubRows; iRow++)
|
|
{
|
|
for (iCol = 0; iCol < iSubCols; iCol++)
|
|
{
|
|
rkMat[iRMin+iRow][iCMin+iCol] += rkV[iRow]*rkW[iCol];
|
|
}
|
|
}
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::PostmultiplyHouseholder (GMatrix<Real>& rkMat,
|
|
GVector<Real>& rkW, int iRMin, int iRMax, int iCMin, int iCMax,
|
|
int iVSize, const Vector3<Real>& rkV)
|
|
{
|
|
// Householder postmultiplication:
|
|
// Given a matrix A and an n-by-1 vector V with V[0] = 1, let S be the
|
|
// submatrix of A with n columns cmin <= c <= n+cmin-1 and rows
|
|
// rmin <= r <= rmax. Overwrite S with S*P where P = I-2*V*V^T/|V|^2.
|
|
|
|
int iSubRows = iRMax - iRMin + 1, iSubCols = iCMax - iCMin + 1;
|
|
int iRow, iCol;
|
|
|
|
Real fSqrLen = rkV[0]*rkV[0];
|
|
for (int i = 1; i < iVSize; i++)
|
|
{
|
|
fSqrLen += rkV[i]*rkV[i];
|
|
}
|
|
|
|
Real fBeta = -((Real)2.0)/fSqrLen;
|
|
for (iRow = 0; iRow < iSubRows; iRow++)
|
|
{
|
|
rkW[iRow] = (Real)0.0;
|
|
for (iCol = 0; iCol < iSubCols; iCol++)
|
|
{
|
|
rkW[iRow] += rkMat[iRMin+iRow][iCMin+iCol]*rkV[iCol];
|
|
}
|
|
rkW[iRow] *= fBeta;
|
|
}
|
|
|
|
for (iRow = 0; iRow < iSubRows; iRow++)
|
|
{
|
|
for (iCol = 0; iCol < iSubCols; iCol++)
|
|
{
|
|
rkMat[iRMin+iRow][iCMin+iCol] += rkW[iRow]*rkV[iCol];
|
|
}
|
|
}
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
template <class Real>
|
|
void PolynomialRoots<Real>::FrancisQRStep (GMatrix<Real>& rkH,
|
|
GVector<Real>& rkW)
|
|
{
|
|
// Given an n-by-n unreduced upper Hessenberg matrix H whose trailing
|
|
// 2-by-2 principal submatrix has eigenvalues a1 and a2, overwrite H
|
|
// with Z^T*H*Z where Z = P(1)*...*P(n-2) is a product of Householder
|
|
// matrices and Z^T*(H-a1*I)*(H-a2*I) is upper triangular.
|
|
|
|
// assert: H is unreduced upper Hessenberg and n >= 3
|
|
|
|
// compute first column of (H-a1*I)*(H-a2*I)
|
|
int iN = rkH.GetRows();
|
|
Real fTrace = rkH[iN-2][iN-2] + rkH[iN-1][iN-1];
|
|
Real fDet = rkH[iN-2][iN-2]*rkH[iN-1][iN-1] -
|
|
rkH[iN-2][iN-1]*rkH[iN-1][iN-2];
|
|
Vector3<Real> kU;
|
|
kU[0] = rkH[0][0]*rkH[1][1]+rkH[0][1]*rkH[1][0]-fTrace*rkH[0][0]+fDet;
|
|
kU[1] = rkH[1][0]*(rkH[0][0]+rkH[1][1]-fTrace);
|
|
kU[2] = rkH[1][0]*rkH[2][1];
|
|
|
|
// overwrite H with P(0)*H*P(0)^T
|
|
Vector3<Real> kV;
|
|
GetHouseholderVector(3,kU,kV);
|
|
PremultiplyHouseholder(rkH,rkW,0,2,0,iN-1,3,kV);
|
|
PostmultiplyHouseholder(rkH,rkW,0,iN-1,0,2,3,kV);
|
|
|
|
for (int i = 1; i <= iN-3; i++)
|
|
{
|
|
// overwrite H with P(i)*H*P(i)^T
|
|
kU[0] = rkH[i ][i-1];
|
|
kU[1] = rkH[i+1][i-1];
|
|
kU[2] = rkH[i+2][i-1];
|
|
GetHouseholderVector(3,kU,kV);
|
|
|
|
// The column range does not need to be 0 to iN-1 because of the
|
|
// pattern of zeros that occur in matrix H.
|
|
PremultiplyHouseholder(rkH,rkW,i,i+2,i-1,iN-1,3,kV);
|
|
|
|
// The row range does not need to be 0 to iN-1 because of the pattern
|
|
// of zeros that occur in matrix H.
|
|
int iRMax = i+3;
|
|
if (iRMax >= iN)
|
|
{
|
|
iRMax = iN-1;
|
|
}
|
|
PostmultiplyHouseholder(rkH,rkW,0,iRMax,i,i+2,3,kV);
|
|
}
|
|
|
|
// overwrite H with P(n-2)*H*P(n-2)^T
|
|
kU[0] = rkH[iN-2][iN-3];
|
|
kU[1] = rkH[iN-1][iN-3];
|
|
GetHouseholderVector(2,kU,kV);
|
|
|
|
// The column range does not need to be 0 to iN-1 because of the pattern
|
|
// of zeros that occur in matrix H.
|
|
PremultiplyHouseholder(rkH,rkW,iN-2,iN-1,iN-3,iN-1,2,kV);
|
|
|
|
PostmultiplyHouseholder(rkH,rkW,0,iN-1,iN-2,iN-1,2,kV);
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
// explicit instantiation
|
|
//----------------------------------------------------------------------------
|
|
template WM4_FOUNDATION_ITEM
|
|
class PolynomialRoots<float>;
|
|
|
|
template WM4_FOUNDATION_ITEM
|
|
class PolynomialRoots<double>;
|
|
//----------------------------------------------------------------------------
|
|
}
|