[occt.git] / src / AppCont / AppCont_LeastSquare.gxx

// Created on: 1995-03-14
// Created by: Modelistation
// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-2012 OPEN CASCADE SAS
//
// The content of this file is subject to the Open CASCADE Technology Public
// License Version 6.5 (the "License"). You may not use the content of this file
// except in compliance with the License. Please obtain a copy of the License
// at http://www.opencascade.org and read it completely before using this file.
//
// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
//
// The Original Code and all software distributed under the License is
// distributed on an "AS IS" basis, without warranty of any kind, and the
// Initial Developer hereby disclaims all such warranties, including without
// limitation, any warranties of merchantability, fitness for a particular
// purpose or non-infringement. Please see the License for the specific terms
// and conditions governing the rights and limitations under the License.


#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;
}
Commit	Line	Data
b311480e	1	// Created on: 1995-03-14
	2	// Created by: Modelistation
	3	// Copyright (c) 1995-1999 Matra Datavision
	4	// Copyright (c) 1999-2012 OPEN CASCADE SAS
	5	//
	6	// The content of this file is subject to the Open CASCADE Technology Public
	7	// License Version 6.5 (the "License"). You may not use the content of this file
	8	// except in compliance with the License. Please obtain a copy of the License
	9	// at http://www.opencascade.org and read it completely before using this file.
	10	//
	11	// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
	12	// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
	13	//
	14	// The Original Code and all software distributed under the License is
	15	// distributed on an "AS IS" basis, without warranty of any kind, and the
	16	// Initial Developer hereby disclaims all such warranties, including without
	17	// limitation, any warranties of merchantability, fitness for a particular
	18	// purpose or non-infringement. Please see the License for the specific terms
	19	// and conditions governing the rights and limitations under the License.
	20
7fd59977	21
	22
	23	#ifndef DEB
	24	#define No_Standard_OutOfRange
	25	#define No_Standard_RangeError
	26	#endif
	27
	28
	29
	30	#include <math.hxx>
	31	#include <math_Vector.hxx>
	32	#include <math_Matrix.hxx>
	33	#include <TColgp_Array1OfPnt.hxx>
	34	#include <TColgp_Array1OfPnt2d.hxx>
	35	#include <gp_Pnt2d.hxx>
	36	#include <gp_Pnt.hxx>
	37	#include <gp_Vec.hxx>
	38	#include <gp_Vec2d.hxx>
	39	#include <TColgp_Array1OfVec.hxx>
	40	#include <TColgp_Array1OfVec2d.hxx>
	41	#include <AppParCurves_MultiPoint.hxx>
	42	#include <AppCont_ContMatrices.hxx>
	43	#include <PLib.hxx>
	44
	45
	46
	47
	48	//=======================================================================
	49	//function : AppCont_LeastSquare
	50	//purpose :
	51	//=======================================================================
	52
	53	AppCont_LeastSquare::AppCont_LeastSquare
	54	(const MultiLine& SSP,
	55	const Standard_Real U0,
	56	const Standard_Real U1,
	57	const AppParCurves_Constraint FirstCons,
	58	const AppParCurves_Constraint LastCons,
	59	const Standard_Integer Deg,
	60	const Standard_Integer NbPoints):
	61	SCU(Deg+1),
	62	Points(1, NbPoints, 1, NbBColumns(SSP)),
	63	Poles(1, Deg+1, 1, NbBColumns(SSP), 0.0),
	64	myParam(1, NbPoints),
	65	VB(1, Deg+1, 1, NbPoints)
	66
	67	{
	68	Done = Standard_False;
	69	Degre = Deg;
	70	math_Matrix InvM(1, Deg+1, 1, Deg+1);
	71	Standard_Integer i, j, k, c, i2;
	72	Standard_Integer classe = Deg+1, cl1 = Deg;
	73	Standard_Real U, dU, Coeff, Coeff2;
	74	Standard_Real IBij, IBPij;
	75
	76	Standard_Integer FirstP = 1, LastP = NbPoints;
	77	Standard_Integer nbcol = NbBColumns(SSP);
	78	math_Matrix B(1, classe, 1, nbcol, 0.0);
	79	Standard_Integer bdeb = 1, bfin = classe;
	80	AppParCurves_Constraint myFirstC = FirstCons, myLastC = LastCons;
	81	nbP = LineTool::NbP3d(SSP);
	82	nbP2d = LineTool::NbP2d(SSP);
	83	Standard_Integer mynbP = nbP, mynbP2d = nbP2d;
	84	if (nbP == 0) mynbP = 1;
85	if (nbP2d == 0) mynbP2d = 1;
86
87	Standard_Integer i2plus1, i2plus2;
88	Nbdiscret = NbPoints;
89	TColgp_Array1OfPnt TabP(1, mynbP);
90	TColgp_Array1OfVec TabV(1, mynbP);
91	TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
92	TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
93
94	Standard_Boolean Ok;
95	if (myFirstC == AppParCurves_TangencyPoint) {
96	if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U0, TabV, TabV2d);
97	else if (nbP != 0) Ok=LineTool::D1(SSP, U0, TabV);
98	else Ok=LineTool::D1(SSP, U0, TabV2d);
99	if (!Ok) myFirstC = AppParCurves_PassPoint;
100	}
101
102	if (myLastC == AppParCurves_TangencyPoint) {
103	if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U1, TabV, TabV2d);
104	else if (nbP != 0) Ok=LineTool::D1(SSP, U1, TabV);
105	else Ok=LineTool::D1(SSP, U1, TabV2d);
106	if (!Ok) myLastC = AppParCurves_PassPoint;
107	}
108
109	math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
110	math::GaussPoints(NbPoints, GaussP);
111	math::GaussWeights(NbPoints, GaussW);
112
113	math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
114
115	dU = 0.5*(U1-U0);
116
117	// calcul et mise en ordre des parametres et des poids:
118	for (i = FirstP; i <= LastP; i++) {
119	U = 0.5(U1+U0) + dUGaussP(i);
120	if (i <= (NbPoints+1)/2) {
121	myParam(LastP-i+1) = U;
122	VBParam(LastP-i+1) = 0.5*(1 + GaussP(i));
123	TheWeights(LastP-i+1) = 0.5*GaussW(i);
124	}
125	else {
126	VBParam(i-(NbPoints+1)/2) = 0.5*(1 + GaussP(i));
127	myParam(i-(NbPoints+1)/2) = U;
128	TheWeights(i-(NbPoints+1)/2) = 0.5*GaussW(i);
129	}
130	}
131
132
133	for (i = FirstP; i <= LastP; i++) {
134	U = myParam(i);
135	if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U, TabP, TabP2d);
136	else if (nbP != 0) LineTool::Value(SSP, U, TabP);
137	else LineTool::Value(SSP, U, TabP2d);
138
139	i2 = 1;
140	for (j = 1; j <= nbP; j++) {
141	(TabP(j)).Coord(Points(i, i2), Points(i, i2+1), Points(i, i2+2));
142	i2 += 3;
143	}
144	for (j = 1; j <= nbP2d; j++) {
145	(TabP2d(j)).Coord(Points(i, i2), Points(i, i2+1));
146	i2 += 2;
147	}
148	}
149
150	// Calcul du VB ( Valeur des fonctions de Bernstein):
151
152	// for (i = 1; i <= classe; i++) {
153	// for (j = 1; j <= NbPoints; j++) {
154	// VB(i,j)=PLib::Binomial(cl1,i-1)Pow((1-VBParam(j)),classe-i)
155	// Pow(VBParam(j),i-1);
156	// }
157	// }
158
159	VBernstein(classe, NbPoints, VB);
160
161	// Traitement du second membre:
162
163	Standard_Real tmppoints, tmpbis;
164	tmppoints = new Standard_Real[nbcol];
165
166	for (c = 1; c <= classe; c++) {
167	tmpbis = tmppoints;
168	for (k = 1; k <= nbcol; k++, tmpbis++) {
169	*tmpbis = 0.0;
170	}
171	for (i = 1; i <= NbPoints; i++) {
172	Coeff = TheWeights(i)*VB(c, i);
173	tmpbis = tmppoints;
174	for (j = 1; j <= nbcol; j++, tmpbis++) {
175	tmpbis += Points(i, j)Coeff;
176	//B(c, j) += Points(i, j)*Coeff;
177	}
178	}
179	tmpbis = tmppoints;
180	for (k = 1; k <= nbcol; k++, tmpbis++) {
181	B(c, k) += *tmpbis;
182	}
183	}
184
185	delete [] tmppoints;
186
187	if (myFirstC == AppParCurves_NoConstraint &&
188	myLastC == AppParCurves_NoConstraint) {
189
190	math_Matrix InvM(1, classe, 1, classe);
191	InvMMatrix(classe, InvM);
192	// Calcul direct des poles:
193
194	for (i = 1; i <= classe; i++) {
195	for (j = 1; j <= classe; j++) {
196	IBij = InvM(i, j);
197	for (k = 1; k <= nbcol; k++) {
198	Poles(i, k) += IBij * B(j, k);
199	}
200	}
201	}
202	}
203
204
205	else {
206	math_Matrix M(1, classe, 1, classe);
207	MMatrix(classe, M);
208
209	if (myFirstC == AppParCurves_PassPoint \|\|
210	myFirstC == AppParCurves_TangencyPoint) {
211
212	if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U0, TabP, TabP2d);
213	else if (nbP != 0) LineTool::Value(SSP, U0, TabP);
214	else LineTool::Value(SSP, U0, TabP2d);
215	i2 =1;
216	for (k = 1; k<= nbP; k++) {
217	(TabP(k)).Coord(Poles(1, i2), Poles(1, i2+1), Poles(1, i2+2));
218	i2 += 3;
219	}
220	for (k = 1; k<= nbP2d; k++) {
221	(TabP2d(k)).Coord(Poles(1, i2), Poles(1, i2+1));
222	i2 += 2;
223	}
224
225	}
226
227	if (myLastC == AppParCurves_PassPoint \|\|
228	myLastC == AppParCurves_TangencyPoint) {
229
230	i2 = 1;
231	if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U1, TabP, TabP2d);
232	else if (nbP != 0) LineTool::Value(SSP, U1, TabP);
233	else LineTool::Value(SSP, U1, TabP2d);
234	for (k = 1; k<= nbP; k++) {
235	(TabP(k)).Coord(Poles(classe,i2),
236	Poles(classe,i2+1),
237	Poles(classe,i2+2));
238	i2 += 3;
239	}
240	for (k = 1; k<= nbP2d; k++) {
241	(TabP2d(k)).Coord(Poles(classe, i2), Poles(classe, i2+1));
242	i2 += 2;
243	}
244	}
245
246
247
248	if (myFirstC == AppParCurves_PassPoint) {
249	bdeb = 2;
250	// mise a jour du second membre:
251	for (i = 1; i <= classe; i++) {
252	Coeff = M(i, 1);
253	for (k = 1; k <= nbcol; k++) {
254	B(i, k) -= Poles(1, k)*Coeff;
255	}
256	}
257	}
258
259
260	if (myLastC == AppParCurves_PassPoint) {
261	bfin = cl1;
262	for (i = 1; i <= classe; i++) {
263	Coeff = M(i, classe);
264	for (k = 1; k <= nbcol; k++) {
265	B(i, k) -= Poles(classe, k)*Coeff;
266	}
267	}
268	}
269
270
271	if (myFirstC == AppParCurves_TangencyPoint) {
272	// On fixe le second pole::
273	bdeb = 3;
274	if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U0, TabV, TabV2d);
275	else if (nbP != 0) LineTool::D1(SSP, U0, TabV);
276	else LineTool::D1(SSP, U0, TabV2d);
277	i2 = 1;
278	Coeff = (U1-U0)/Degre;
279	for (k = 1; k<= nbP; k++) {
280	i2plus1 = i2+1; i2plus2 = i2+2;
281	Poles(2, i2) = Poles(1, i2) + TabV(k).X()*Coeff;
282	Poles(2, i2plus1) = Poles(1, i2plus1) + TabV(k).Y()*Coeff;
283	Poles(2, i2plus2) = Poles(1, i2plus2) + TabV(k).Z()*Coeff;
284	i2 += 3;
285	}
286	for (k = 1; k<= nbP2d; k++) {
287	i2plus1 = i2+1;
288	Poles(2, i2) = Poles(1, i2) + TabV2d(k).X()*Coeff;
289	Poles(2, i2plus1) = Poles(1, i2plus1) + TabV2d(k).Y()*Coeff;
290	i2 += 2;
291	}
292
293
294	for (i = 1; i <= classe; i++) {
295	Coeff = M(i, 1); Coeff2 = M(i, 2);
296	for (k = 1; k <= nbcol; k++) {
297	B(i, k) -= Poles(1, k)Coeff+Poles(2, k)Coeff2;
298	}
299	}
300	}
301
302	if (myLastC == AppParCurves_TangencyPoint) {
303	bfin = classe-2;
304
305	if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U1, TabV, TabV2d);
306	else if (nbP != 0) LineTool::D1(SSP, U1, TabV);
307	else LineTool::D1(SSP, U1, TabV2d);
308	i2 = 1;
309	Coeff = (U1-U0)/Degre;
310	for (k = 1; k<= nbP; k++) {
311	i2plus1 = i2+1; i2plus2 = i2+2;
312	Poles(cl1,i2) = Poles(classe, i2) - TabV(k).X()*Coeff;
313	Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV(k).Y()*Coeff;
314	Poles(cl1,i2plus2) = Poles(classe, i2plus2) - TabV(k).Z()*Coeff;
315	i2 += 3;
316	}
317	for (k = 1; k<= nbP2d; k++) {
318	i2plus1 = i2+1;
319	Poles(cl1,i2) = Poles(classe, i2) - TabV2d(k).X()*Coeff;
320	Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV2d(k).Y()*Coeff;
321	i2 += 2;
322	}
323
324	for (i = 1; i <= classe; i++) {
325	Coeff = M(i, classe); Coeff2 = M(i, cl1);
326	for (k = 1; k <= nbcol; k++) {
327	B(i, k) -= Poles(classe, k)Coeff + Poles(cl1, k)Coeff2;
328	}
329	}
330	}
331
332	if (bdeb <= bfin) {
333	math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);
334
335	for (i = bdeb; i <= bfin; i++) {
336	for (j = 1; j <= classe; j++) {
337	Coeff = M(i, j);
338	for (k = 1; k <= nbcol; k++) {
339	B2(i, k) += B(j, k)*Coeff;
340	}
341	}
342	}
343
344
345	// Resolution:
346	// ===========
347	math_Matrix IBP(bdeb, bfin, bdeb, bfin);
348
349	// dans IBPMatrix at IBTMatrix ne sont stockees que les resultats pour
350	// une classe inferieure ou egale a 26 (pour l instant du moins.)
351
352	if (bdeb == 2 && bfin == classe-1 && classe <= 26) {
353	IBPMatrix(classe, IBP);
354	}
355	else if (bdeb == 3 && bfin == classe-2 && classe <= 26) {
356	IBTMatrix(classe, IBP);
357	}
358	else {
359	math_Matrix MP(1, classe, bdeb, bfin);
360
361	for (i = 1; i <= classe; i++) {
362	for (j = bdeb; j <= bfin; j++) {
363	MP(i, j) = M(i, j);
364	}
365	}
366	math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
367	IBP1 = MP.Transposed()*MP;
368	IBP = IBP1.Inverse();
369	}
370
371
372	Done = Standard_True;
373	for (i = bdeb; i <= bfin; i++) {
374	for (j = bdeb; j <= bfin; j++) {
375	IBPij = IBP(i, j);;
376	for (k = 1; k<= nbcol; k++) {
377	Poles(i, k) += IBPij * B2(j, k);
378	}
379	}
380	}
381	}
382	}
383	}
384
385
386
387
388	//=======================================================================
389	//function : NbBColumns
390	//purpose :
391	//=======================================================================
392
393	Standard_Integer AppCont_LeastSquare::NbBColumns(
394	const MultiLine& SSP) const
395	{
396	Standard_Integer BCol;
397	BCol = (LineTool::NbP3d(SSP))*3 +
398	(LineTool::NbP2d(SSP))*2;
399	return BCol;
400	}
401
402
403	//=======================================================================
404	//function : Value
405	//purpose :
406	//=======================================================================
407
408	const AppParCurves_MultiCurve& AppCont_LeastSquare::Value()
409	{
410
411	Standard_Integer i, j, j2;
412	gp_Pnt Pt;
413	gp_Pnt2d Pt2d;
414	Standard_Integer ideb = 1, ifin = Degre+1;
415
416	// On met le resultat dans les curves correspondantes
417	for (i = ideb; i <= ifin; i++) {
418	j2 = 1;
419	AppParCurves_MultiPoint MPole(nbP, nbP2d);
420	for (j = 1; j <= nbP; j++) {
421	Pt.SetCoord(Poles(i, j2), Poles(i, j2+1), Poles(i,j2+2));
422	MPole.SetPoint(j, Pt);
423	j2 += 3;
424	}
425	for (j = nbP+1;j <= nbP+nbP2d; j++) {
426	Pt2d.SetCoord(Poles(i, j2), Poles(i, j2+1));
427	MPole.SetPoint2d(j, Pt2d);
428	j2 += 2;
429	}
430	SCU.SetValue(i, MPole);
431	}
432	return SCU;
433	}
434
435
436
437	//=======================================================================
438	//function : Error
439	//purpose :
440	//=======================================================================
441
442	void AppCont_LeastSquare::Error(Standard_Real& F,
443	Standard_Real& MaxE3d,
444	Standard_Real& MaxE2d) const
445	{
446	Standard_Integer i, j, k, c, i2, classe = Degre+1;
447	// Standard_Real Coeff, val = 0.0, err3d = 0.0, err2d =0.0;
448	Standard_Real Coeff, err3d = 0.0, err2d =0.0;
449	Standard_Integer ncol = Points.UpperCol()-Points.LowerCol()+1;
450
451	math_Matrix MyPoints(1, Nbdiscret, 1, ncol);
452	MyPoints = Points;
453
454	MaxE3d = MaxE2d = F = 0.0;
455
456	Standard_Real tmppoles, tmpbis;
457	tmppoles = new Standard_Real[ncol];
458
459	for (c = 1; c <= classe; c++) {
460	tmpbis = tmppoles;
461	for (k = 1; k <= ncol; k++, tmpbis++) {
462	*tmpbis = Poles(c, k);
463	}
464	for (i = 1; i <= Nbdiscret; i++) {
465	Coeff = VB(c, i);
466	tmpbis = tmppoles;
467	for (j = 1; j <= ncol; j++, tmpbis++) {
468	MyPoints(i, j) -= (tmpbis)Coeff; // Poles(c, j)*Coeff;
469	}
470	}
471	}
472	delete [] tmppoles;
473
474	Standard_Real e1, e2, e3;
475	for (i = 1; i <= Nbdiscret; i++) {
476	i2 = 1;
477	for (j = 1; j<= nbP; j++) {
478	e1 = MyPoints(i, i2);
479	e2 = MyPoints(i, i2+1);
480	e3 = MyPoints(i, i2+2);
481	err3d = e1e1+e2e2+e3*e3;
482	MaxE3d = Max(MaxE3d, err3d);
483	F += err3d;
484	i2 += 3;
485	}
486	for (j = 1; j<= nbP2d; j++) {
487	e1 = MyPoints(i, i2);
488	e2 = MyPoints(i, i2+1);
489	err2d = e1e1+e2e2;
490	MaxE2d = Max(MaxE2d, err2d);
491	F += err2d;
492	i2 += 2;
493	}
494	}
495
496	MaxE3d = Sqrt(MaxE3d);
497	MaxE2d = Sqrt(MaxE2d);
498
499	}
500
501
502	//=======================================================================
503	//function : IsDone
504	//purpose :
505	//=======================================================================
506
507	Standard_Boolean AppCont_LeastSquare::IsDone() const
508	{
509	return Done;
510	}