--- /dev/null
+// File: AppCont_LeastSquare.gxx
+// Created: Tue Mar 14 14:09:22 1995
+// Author: Modelistation
+// <model@phobox>
+
+
+#ifndef DEB
+#define No_Standard_OutOfRange
+#define No_Standard_RangeError
+#endif
+
+
+
+#include <math.hxx>
+#include <math_Vector.hxx>
+#include <math_Matrix.hxx>
+#include <TColgp_Array1OfPnt.hxx>
+#include <TColgp_Array1OfPnt2d.hxx>
+#include <gp_Pnt2d.hxx>
+#include <gp_Pnt.hxx>
+#include <gp_Vec.hxx>
+#include <gp_Vec2d.hxx>
+#include <TColgp_Array1OfVec.hxx>
+#include <TColgp_Array1OfVec2d.hxx>
+#include <AppParCurves_MultiPoint.hxx>
+#include <AppCont_ContMatrices.hxx>
+#include <PLib.hxx>
+
+
+
+
+//=======================================================================
+//function : AppCont_LeastSquare
+//purpose :
+//=======================================================================
+
+AppCont_LeastSquare::AppCont_LeastSquare
+ (const MultiLine& SSP,
+ const Standard_Real U0,
+ const Standard_Real U1,
+ const AppParCurves_Constraint FirstCons,
+ const AppParCurves_Constraint LastCons,
+ const Standard_Integer Deg,
+ const Standard_Integer NbPoints):
+ SCU(Deg+1),
+ Points(1, NbPoints, 1, NbBColumns(SSP)),
+ Poles(1, Deg+1, 1, NbBColumns(SSP), 0.0),
+ myParam(1, NbPoints),
+ VB(1, Deg+1, 1, NbPoints)
+
+{
+ Done = Standard_False;
+ Degre = Deg;
+ math_Matrix InvM(1, Deg+1, 1, Deg+1);
+ Standard_Integer i, j, k, c, i2;
+ Standard_Integer classe = Deg+1, cl1 = Deg;
+ Standard_Real U, dU, Coeff, Coeff2;
+ Standard_Real IBij, IBPij;
+
+ Standard_Integer FirstP = 1, LastP = NbPoints;
+ Standard_Integer nbcol = NbBColumns(SSP);
+ math_Matrix B(1, classe, 1, nbcol, 0.0);
+ Standard_Integer bdeb = 1, bfin = classe;
+ AppParCurves_Constraint myFirstC = FirstCons, myLastC = LastCons;
+ nbP = LineTool::NbP3d(SSP);
+ nbP2d = LineTool::NbP2d(SSP);
+ Standard_Integer mynbP = nbP, mynbP2d = nbP2d;
+ if (nbP == 0) mynbP = 1;
+ if (nbP2d == 0) mynbP2d = 1;
+
+ Standard_Integer i2plus1, i2plus2;
+ Nbdiscret = NbPoints;
+ TColgp_Array1OfPnt TabP(1, mynbP);
+ TColgp_Array1OfVec TabV(1, mynbP);
+ TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
+ TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
+
+ Standard_Boolean Ok;
+ if (myFirstC == AppParCurves_TangencyPoint) {
+ if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U0, TabV, TabV2d);
+ else if (nbP != 0) Ok=LineTool::D1(SSP, U0, TabV);
+ else Ok=LineTool::D1(SSP, U0, TabV2d);
+ if (!Ok) myFirstC = AppParCurves_PassPoint;
+ }
+
+ if (myLastC == AppParCurves_TangencyPoint) {
+ if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U1, TabV, TabV2d);
+ else if (nbP != 0) Ok=LineTool::D1(SSP, U1, TabV);
+ else Ok=LineTool::D1(SSP, U1, TabV2d);
+ if (!Ok) myLastC = AppParCurves_PassPoint;
+ }
+
+ math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
+ math::GaussPoints(NbPoints, GaussP);
+ math::GaussWeights(NbPoints, GaussW);
+
+ math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
+
+ dU = 0.5*(U1-U0);
+
+ // calcul et mise en ordre des parametres et des poids:
+ for (i = FirstP; i <= LastP; i++) {
+ U = 0.5*(U1+U0) + dU*GaussP(i);
+ if (i <= (NbPoints+1)/2) {
+ myParam(LastP-i+1) = U;
+ VBParam(LastP-i+1) = 0.5*(1 + GaussP(i));
+ TheWeights(LastP-i+1) = 0.5*GaussW(i);
+ }
+ else {
+ VBParam(i-(NbPoints+1)/2) = 0.5*(1 + GaussP(i));
+ myParam(i-(NbPoints+1)/2) = U;
+ TheWeights(i-(NbPoints+1)/2) = 0.5*GaussW(i);
+ }
+ }
+
+
+ for (i = FirstP; i <= LastP; i++) {
+ U = myParam(i);
+ if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U, TabP, TabP2d);
+ else if (nbP != 0) LineTool::Value(SSP, U, TabP);
+ else LineTool::Value(SSP, U, TabP2d);
+
+ i2 = 1;
+ for (j = 1; j <= nbP; j++) {
+ (TabP(j)).Coord(Points(i, i2), Points(i, i2+1), Points(i, i2+2));
+ i2 += 3;
+ }
+ for (j = 1; j <= nbP2d; j++) {
+ (TabP2d(j)).Coord(Points(i, i2), Points(i, i2+1));
+ i2 += 2;
+ }
+ }
+
+ // Calcul du VB ( Valeur des fonctions de Bernstein):
+
+// for (i = 1; i <= classe; i++) {
+// for (j = 1; j <= NbPoints; j++) {
+// VB(i,j)=PLib::Binomial(cl1,i-1)*Pow((1-VBParam(j)),classe-i)*
+// Pow(VBParam(j),i-1);
+// }
+// }
+
+ VBernstein(classe, NbPoints, VB);
+
+ // Traitement du second membre:
+
+ Standard_Real *tmppoints, *tmpbis;
+ tmppoints = new Standard_Real[nbcol];
+
+ for (c = 1; c <= classe; c++) {
+ tmpbis = tmppoints;
+ for (k = 1; k <= nbcol; k++, tmpbis++) {
+ *tmpbis = 0.0;
+ }
+ for (i = 1; i <= NbPoints; i++) {
+ Coeff = TheWeights(i)*VB(c, i);
+ tmpbis = tmppoints;
+ for (j = 1; j <= nbcol; j++, tmpbis++) {
+ *tmpbis += Points(i, j)*Coeff;
+ //B(c, j) += Points(i, j)*Coeff;
+ }
+ }
+ tmpbis = tmppoints;
+ for (k = 1; k <= nbcol; k++, tmpbis++) {
+ B(c, k) += *tmpbis;
+ }
+ }
+
+ delete [] tmppoints;
+
+ if (myFirstC == AppParCurves_NoConstraint &&
+ myLastC == AppParCurves_NoConstraint) {
+
+ math_Matrix InvM(1, classe, 1, classe);
+ InvMMatrix(classe, InvM);
+ // Calcul direct des poles:
+
+ for (i = 1; i <= classe; i++) {
+ for (j = 1; j <= classe; j++) {
+ IBij = InvM(i, j);
+ for (k = 1; k <= nbcol; k++) {
+ Poles(i, k) += IBij * B(j, k);
+ }
+ }
+ }
+ }
+
+
+ else {
+ math_Matrix M(1, classe, 1, classe);
+ MMatrix(classe, M);
+
+ if (myFirstC == AppParCurves_PassPoint ||
+ myFirstC == AppParCurves_TangencyPoint) {
+
+ if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U0, TabP, TabP2d);
+ else if (nbP != 0) LineTool::Value(SSP, U0, TabP);
+ else LineTool::Value(SSP, U0, TabP2d);
+ i2 =1;
+ for (k = 1; k<= nbP; k++) {
+ (TabP(k)).Coord(Poles(1, i2), Poles(1, i2+1), Poles(1, i2+2));
+ i2 += 3;
+ }
+ for (k = 1; k<= nbP2d; k++) {
+ (TabP2d(k)).Coord(Poles(1, i2), Poles(1, i2+1));
+ i2 += 2;
+ }
+
+ }
+
+ if (myLastC == AppParCurves_PassPoint ||
+ myLastC == AppParCurves_TangencyPoint) {
+
+ i2 = 1;
+ if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U1, TabP, TabP2d);
+ else if (nbP != 0) LineTool::Value(SSP, U1, TabP);
+ else LineTool::Value(SSP, U1, TabP2d);
+ for (k = 1; k<= nbP; k++) {
+ (TabP(k)).Coord(Poles(classe,i2),
+ Poles(classe,i2+1),
+ Poles(classe,i2+2));
+ i2 += 3;
+ }
+ for (k = 1; k<= nbP2d; k++) {
+ (TabP2d(k)).Coord(Poles(classe, i2), Poles(classe, i2+1));
+ i2 += 2;
+ }
+ }
+
+
+
+ if (myFirstC == AppParCurves_PassPoint) {
+ bdeb = 2;
+ // mise a jour du second membre:
+ for (i = 1; i <= classe; i++) {
+ Coeff = M(i, 1);
+ for (k = 1; k <= nbcol; k++) {
+ B(i, k) -= Poles(1, k)*Coeff;
+ }
+ }
+ }
+
+
+ if (myLastC == AppParCurves_PassPoint) {
+ bfin = cl1;
+ for (i = 1; i <= classe; i++) {
+ Coeff = M(i, classe);
+ for (k = 1; k <= nbcol; k++) {
+ B(i, k) -= Poles(classe, k)*Coeff;
+ }
+ }
+ }
+
+
+ if (myFirstC == AppParCurves_TangencyPoint) {
+ // On fixe le second pole::
+ bdeb = 3;
+ if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U0, TabV, TabV2d);
+ else if (nbP != 0) LineTool::D1(SSP, U0, TabV);
+ else LineTool::D1(SSP, U0, TabV2d);
+ i2 = 1;
+ Coeff = (U1-U0)/Degre;
+ for (k = 1; k<= nbP; k++) {
+ i2plus1 = i2+1; i2plus2 = i2+2;
+ Poles(2, i2) = Poles(1, i2) + TabV(k).X()*Coeff;
+ Poles(2, i2plus1) = Poles(1, i2plus1) + TabV(k).Y()*Coeff;
+ Poles(2, i2plus2) = Poles(1, i2plus2) + TabV(k).Z()*Coeff;
+ i2 += 3;
+ }
+ for (k = 1; k<= nbP2d; k++) {
+ i2plus1 = i2+1;
+ Poles(2, i2) = Poles(1, i2) + TabV2d(k).X()*Coeff;
+ Poles(2, i2plus1) = Poles(1, i2plus1) + TabV2d(k).Y()*Coeff;
+ i2 += 2;
+ }
+
+
+ for (i = 1; i <= classe; i++) {
+ Coeff = M(i, 1); Coeff2 = M(i, 2);
+ for (k = 1; k <= nbcol; k++) {
+ B(i, k) -= Poles(1, k)*Coeff+Poles(2, k)*Coeff2;
+ }
+ }
+ }
+
+ if (myLastC == AppParCurves_TangencyPoint) {
+ bfin = classe-2;
+
+ if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U1, TabV, TabV2d);
+ else if (nbP != 0) LineTool::D1(SSP, U1, TabV);
+ else LineTool::D1(SSP, U1, TabV2d);
+ i2 = 1;
+ Coeff = (U1-U0)/Degre;
+ for (k = 1; k<= nbP; k++) {
+ i2plus1 = i2+1; i2plus2 = i2+2;
+ Poles(cl1,i2) = Poles(classe, i2) - TabV(k).X()*Coeff;
+ Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV(k).Y()*Coeff;
+ Poles(cl1,i2plus2) = Poles(classe, i2plus2) - TabV(k).Z()*Coeff;
+ i2 += 3;
+ }
+ for (k = 1; k<= nbP2d; k++) {
+ i2plus1 = i2+1;
+ Poles(cl1,i2) = Poles(classe, i2) - TabV2d(k).X()*Coeff;
+ Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV2d(k).Y()*Coeff;
+ i2 += 2;
+ }
+
+ for (i = 1; i <= classe; i++) {
+ Coeff = M(i, classe); Coeff2 = M(i, cl1);
+ for (k = 1; k <= nbcol; k++) {
+ B(i, k) -= Poles(classe, k)*Coeff + Poles(cl1, k)*Coeff2;
+ }
+ }
+ }
+
+ if (bdeb <= bfin) {
+ math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);
+
+ for (i = bdeb; i <= bfin; i++) {
+ for (j = 1; j <= classe; j++) {
+ Coeff = M(i, j);
+ for (k = 1; k <= nbcol; k++) {
+ B2(i, k) += B(j, k)*Coeff;
+ }
+ }
+ }
+
+
+ // Resolution:
+ // ===========
+ math_Matrix IBP(bdeb, bfin, bdeb, bfin);
+
+ // dans IBPMatrix at IBTMatrix ne sont stockees que les resultats pour
+ // une classe inferieure ou egale a 26 (pour l instant du moins.)
+
+ if (bdeb == 2 && bfin == classe-1 && classe <= 26) {
+ IBPMatrix(classe, IBP);
+ }
+ else if (bdeb == 3 && bfin == classe-2 && classe <= 26) {
+ IBTMatrix(classe, IBP);
+ }
+ else {
+ math_Matrix MP(1, classe, bdeb, bfin);
+
+ for (i = 1; i <= classe; i++) {
+ for (j = bdeb; j <= bfin; j++) {
+ MP(i, j) = M(i, j);
+ }
+ }
+ math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
+ IBP1 = MP.Transposed()*MP;
+ IBP = IBP1.Inverse();
+ }
+
+
+ Done = Standard_True;
+ for (i = bdeb; i <= bfin; i++) {
+ for (j = bdeb; j <= bfin; j++) {
+ IBPij = IBP(i, j);;
+ for (k = 1; k<= nbcol; k++) {
+ Poles(i, k) += IBPij * B2(j, k);
+ }
+ }
+ }
+ }
+ }
+}
+
+
+
+
+//=======================================================================
+//function : NbBColumns
+//purpose :
+//=======================================================================
+
+Standard_Integer AppCont_LeastSquare::NbBColumns(
+ const MultiLine& SSP) const
+{
+ Standard_Integer BCol;
+ BCol = (LineTool::NbP3d(SSP))*3 +
+ (LineTool::NbP2d(SSP))*2;
+ return BCol;
+}
+
+
+//=======================================================================
+//function : Value
+//purpose :
+//=======================================================================
+
+const AppParCurves_MultiCurve& AppCont_LeastSquare::Value()
+{
+
+ Standard_Integer i, j, j2;
+ gp_Pnt Pt;
+ gp_Pnt2d Pt2d;
+ Standard_Integer ideb = 1, ifin = Degre+1;
+
+ // On met le resultat dans les curves correspondantes
+ for (i = ideb; i <= ifin; i++) {
+ j2 = 1;
+ AppParCurves_MultiPoint MPole(nbP, nbP2d);
+ for (j = 1; j <= nbP; j++) {
+ Pt.SetCoord(Poles(i, j2), Poles(i, j2+1), Poles(i,j2+2));
+ MPole.SetPoint(j, Pt);
+ j2 += 3;
+ }
+ for (j = nbP+1;j <= nbP+nbP2d; j++) {
+ Pt2d.SetCoord(Poles(i, j2), Poles(i, j2+1));
+ MPole.SetPoint2d(j, Pt2d);
+ j2 += 2;
+ }
+ SCU.SetValue(i, MPole);
+ }
+ return SCU;
+}
+
+
+
+//=======================================================================
+//function : Error
+//purpose :
+//=======================================================================
+
+void AppCont_LeastSquare::Error(Standard_Real& F,
+ Standard_Real& MaxE3d,
+ Standard_Real& MaxE2d) const
+{
+ Standard_Integer i, j, k, c, i2, classe = Degre+1;
+// Standard_Real Coeff, val = 0.0, err3d = 0.0, err2d =0.0;
+ Standard_Real Coeff, err3d = 0.0, err2d =0.0;
+ Standard_Integer ncol = Points.UpperCol()-Points.LowerCol()+1;
+
+ math_Matrix MyPoints(1, Nbdiscret, 1, ncol);
+ MyPoints = Points;
+
+ MaxE3d = MaxE2d = F = 0.0;
+
+ Standard_Real *tmppoles, *tmpbis;
+ tmppoles = new Standard_Real[ncol];
+
+ for (c = 1; c <= classe; c++) {
+ tmpbis = tmppoles;
+ for (k = 1; k <= ncol; k++, tmpbis++) {
+ *tmpbis = Poles(c, k);
+ }
+ for (i = 1; i <= Nbdiscret; i++) {
+ Coeff = VB(c, i);
+ tmpbis = tmppoles;
+ for (j = 1; j <= ncol; j++, tmpbis++) {
+ MyPoints(i, j) -= (*tmpbis)*Coeff; // Poles(c, j)*Coeff;
+ }
+ }
+ }
+ delete [] tmppoles;
+
+ Standard_Real e1, e2, e3;
+ for (i = 1; i <= Nbdiscret; i++) {
+ i2 = 1;
+ for (j = 1; j<= nbP; j++) {
+ e1 = MyPoints(i, i2);
+ e2 = MyPoints(i, i2+1);
+ e3 = MyPoints(i, i2+2);
+ err3d = e1*e1+e2*e2+e3*e3;
+ MaxE3d = Max(MaxE3d, err3d);
+ F += err3d;
+ i2 += 3;
+ }
+ for (j = 1; j<= nbP2d; j++) {
+ e1 = MyPoints(i, i2);
+ e2 = MyPoints(i, i2+1);
+ err2d = e1*e1+e2*e2;
+ MaxE2d = Max(MaxE2d, err2d);
+ F += err2d;
+ i2 += 2;
+ }
+ }
+
+ MaxE3d = Sqrt(MaxE3d);
+ MaxE2d = Sqrt(MaxE2d);
+
+}
+
+
+//=======================================================================
+//function : IsDone
+//purpose :
+//=======================================================================
+
+Standard_Boolean AppCont_LeastSquare::IsDone() const
+{
+ return Done;
+}
projects / occt.git / blobdiff