--- /dev/null
+// File: FairCurve_DistributionOfEnergy.cxx
+// Created: Mon Jan 22 15:11:20 1996
+// Author: Philippe MANGIN
+
+#ifndef DEB
+#define No_Standard_RangeError
+#define No_Standard_OutOfRange
+#endif
+
+#include <FairCurve_Energy.ixx>
+
+#include <math_IntegerVector.hxx>
+#include <gp_Vec2d.hxx>
+
+//=======================================================================
+FairCurve_Energy::FairCurve_Energy(const Handle(TColgp_HArray1OfPnt2d)& Poles,
+ const Standard_Integer ContrOrder1,
+ const Standard_Integer ContrOrder2,
+ const Standard_Boolean WithAuxValue,
+ const Standard_Real Angle1,
+ const Standard_Real Angle2,
+ const Standard_Integer Degree,
+ const Standard_Real Curvature1,
+ const Standard_Real Curvature2 )
+//=======================================================================
+ : MyPoles (Poles),
+ MyContrOrder1(ContrOrder1),
+ MyContrOrder2(ContrOrder2),
+ MyWithAuxValue(WithAuxValue),
+ MyNbVar(2*(Poles->Length()-2) - ContrOrder2 - ContrOrder1 + WithAuxValue),
+ MyNbValues(2*Poles->Length() + WithAuxValue),
+ MyLinearForm(0, 1),
+ MyQuadForm(0, 1),
+ MyGradient( 0, MyNbValues),
+ MyHessian( 0, MyNbValues + MyNbValues*(MyNbValues+1)/2 )
+{
+ // on attend des angles dans le repere (Ox,Oy)
+ gp_XY L0 (Cos(Angle1), Sin(Angle1)), L1 (-Cos(Angle2), Sin(Angle2));
+ MyLinearForm.SetValue(0, L0);
+ MyLinearForm.SetValue(1, L1);
+ gp_XY Q0(-Sin(Angle1), Cos(Angle1)), Q1 (Sin(Angle2), Cos(Angle2));
+ MyQuadForm.SetValue(0, ((double)Degree) / (Degree-1) * Curvature1 * Q0);
+ MyQuadForm.SetValue(1, ((double)Degree) / (Degree-1) * Curvature2 * Q1);
+}
+
+//=======================================================================
+Standard_Boolean FairCurve_Energy::Value(const math_Vector& X,
+ Standard_Real& E)
+//=======================================================================
+{
+ Standard_Boolean IsDone;
+ math_Vector Energie(0,0);
+ ComputePoles(X);
+ IsDone = Compute(0, Energie);
+ E = Energie(0);
+ return IsDone;
+}
+
+//=======================================================================
+Standard_Boolean FairCurve_Energy::Gradient(const math_Vector& X,
+ math_Vector& G)
+//=======================================================================
+{
+ Standard_Boolean IsDone;
+ Standard_Real E;
+
+ IsDone = Values(X, E, G);
+ return IsDone;
+}
+
+//=======================================================================
+void FairCurve_Energy::Gradient1(const math_Vector& Vect,
+ math_Vector& Grad)
+//=======================================================================
+{
+ Standard_Integer ii,
+ DebG = Grad.Lower(), FinG = Grad.Upper();
+ Standard_Integer Vdeb = 3,
+ Vfin = 2*MyPoles->Length()-2;
+
+// .... par calcul
+ if (MyContrOrder1 >= 1) {
+ gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
+ Grad(DebG) = MyLinearForm(0) * DPole;
+ Vdeb += 2;
+ DebG += 1;
+ }
+ if(MyContrOrder1 == 2) {
+ Standard_Real Lambda0 = MyPoles->Value(MyPoles->Lower())
+ .Distance( MyPoles->Value(MyPoles->Lower()+1) );
+ gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
+ Grad(DebG-1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * DPole;
+ Grad(DebG) = MyLinearForm(0) * DPole;
+ Vdeb += 2;
+ DebG += 1;
+ }
+ if (MyWithAuxValue) {
+ Grad(FinG) = Vect( 2*MyPoles->Length()+1 );
+ FinG -= 1;
+ }
+ if (MyContrOrder2 >= 1) {
+ gp_XY DPole (Vect(Vfin-1), Vect(Vfin));
+ Grad(FinG) = MyLinearForm(1) * DPole;
+ FinG -= 1;
+ }
+ if(MyContrOrder2 == 2) {
+ Standard_Real Lambda1 = MyPoles->Value(MyPoles->Upper())
+ .Distance(MyPoles->Value(MyPoles->Upper()-1) );
+ gp_XY DPole (Vect(Vfin-3), Vect(Vfin-2));
+ Grad(FinG) = Grad(FinG+1) + (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * DPole;
+ Grad(FinG+1) = MyLinearForm(1) * DPole;
+ FinG -= 1;
+ }
+// ... par recopie
+ for (ii=DebG; ii<=FinG; ii++) {
+ Grad(ii) = Vect(Vdeb);
+ Vdeb += 1;
+ }
+}
+
+//=======================================================================
+Standard_Boolean FairCurve_Energy::Values(const math_Vector& X,
+ Standard_Real& E,
+ math_Vector& G)
+//=======================================================================
+{
+ Standard_Boolean IsDone;
+
+ ComputePoles(X);
+ IsDone = Compute(1, MyGradient);
+ if (IsDone) {
+ E = MyGradient(0);
+ Gradient1(MyGradient, G);
+ }
+ return IsDone;
+}
+
+//=======================================================================
+Standard_Boolean FairCurve_Energy::Values(const math_Vector& X,
+ Standard_Real& E,
+ math_Vector& G,
+ math_Matrix& H)
+//=======================================================================
+{
+ Standard_Boolean IsDone;
+
+ ComputePoles(X);
+ IsDone = Compute(2, MyHessian);
+ if (IsDone) {
+ E = MyHessian(0);
+ Gradient1(MyHessian, G);
+ Hessian1 (MyHessian, H);
+ }
+ return IsDone;
+}
+
+
+//=======================================================================
+void FairCurve_Energy::Hessian1(const math_Vector& Vect,
+ math_Matrix& H)
+//=======================================================================
+{
+
+ Standard_Integer ii, jj, kk, Vk;
+ Standard_Integer Vdeb = 3 + 2*MyContrOrder1,
+ Vfin = 2*MyPoles->Length() - 2*(MyContrOrder2+1),
+ Vup = 2*MyPoles->Length()+MyWithAuxValue;
+ Standard_Integer DebH = 1+MyContrOrder1,
+ FinH = MyNbVar - MyWithAuxValue - MyContrOrder2 ;
+ Standard_Real Cos0 = pow(MyLinearForm(0).X(),2),
+ Sin0 = pow(MyLinearForm(0).Y(),2),
+ CosSin0 = 2 * MyLinearForm(0).X() * MyLinearForm(0).Y(),
+ Cos1 = pow(MyLinearForm(1).X(),2),
+ Sin1 = pow(MyLinearForm(1).Y(),2),
+ CosSin1 = 2 * MyLinearForm(1).X() * MyLinearForm(1).Y() ;
+ Standard_Real Lambda0=0, Lambda1=0;
+
+ if (MyContrOrder1 >= 1) {
+ Lambda0 = MyPoles->Value(MyPoles->Lower())
+ .Distance( MyPoles->Value(MyPoles->Lower()+1) );}
+
+ if (MyContrOrder2 >= 1) {
+ Lambda1 = MyPoles->Value(MyPoles->Upper())
+ .Distance(MyPoles->Value(MyPoles->Upper()-1) );}
+
+
+ if (MyContrOrder1 >= 1) {
+
+// calcul de la colonne lambda gauche --------------------------------
+
+ jj = Vdeb-2*MyContrOrder1;
+ kk=Indice(jj, jj); // X2X2
+ ii=Indice(jj+1, jj); // X2Y2
+ H(1, 1) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);
+
+ if (MyContrOrder1 >= 2) {
+ gp_XY Laux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0));
+ jj = Vdeb-2*(MyContrOrder1-1);
+ kk=Indice(jj, jj-2); // X1X2
+ ii=Indice(jj+1, jj-2); //X1Y2
+ gp_XY Aux(Vect(kk+2), Vect(ii+3));
+
+ H (1, 1) += 2 * (
+ ( MyQuadForm(0).X() * Vect(5) + MyQuadForm(0).Y() * Vect(6) )
+ + ( Laux.X() * ( MyLinearForm(0).X()*Vect(kk) + MyLinearForm(0).Y()*Vect(kk+1))
+ + Laux.Y() * ( MyLinearForm(0).X()*Vect(ii) + MyLinearForm(0).Y()*Vect(ii+1)) )
+ + Laux.X() * Laux.Y() * Vect(ii+2) )
+ + ( Pow(Laux.X(),2) * Vect(kk+2) + Pow(Laux.Y(),2) * Vect(ii+3));
+
+ H(2,1) = (Cos0 * Vect(kk) + CosSin0*(Vect(ii)+Vect(kk+1))/2 + Sin0 * Vect(ii+1))
+ + Laux * MyLinearForm(0).Multiplied(Aux)
+ + (Laux.X()*MyLinearForm(0).Y() + Laux.Y()*MyLinearForm(0).X()) * Vect(ii+2);
+ }
+
+
+ if (MyWithAuxValue) {
+ kk =Indice(Vup, Vdeb-2*MyContrOrder1);
+ H(MyNbVar, 1) = MyLinearForm(0).X() * Vect(kk)
+ + MyLinearForm(0).Y() * Vect(kk+1);
+ }
+
+ if (MyContrOrder2 >= 1) {
+ H(MyNbVar-MyWithAuxValue, 1) = 0; // correct si il y a moins 3 noeuds
+ if (MyContrOrder2 == 2) {H(MyNbVar-MyWithAuxValue-1, 1) = 0;}
+ }
+
+
+ Vk = Vdeb;
+ kk = Indice(Vk, Vdeb-2*MyContrOrder1);
+ for (ii=DebH; ii<=FinH; ii++) {
+ H(ii, 1) = MyLinearForm(0).X() * Vect(kk)
+ + MyLinearForm(0).Y() * Vect(kk+1);
+ kk += Vk;
+ Vk++;
+ }
+ }
+
+// calcul de la ligne mu gauche -----------------------
+ if (MyContrOrder1 >= 2) {
+ jj = Vdeb-2*(MyContrOrder1-1);
+ kk=Indice(jj, jj); // X3X3
+ ii=Indice(jj+1, jj); // X3Y3
+ H(2, 2) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);
+
+ if (MyWithAuxValue) {
+ kk =Indice(Vup, Vdeb-2*(MyContrOrder1-1));
+ gp_XY Pole (Vect(kk), Vect(kk+1));
+ H(MyNbVar, 1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * Pole;
+ H(MyNbVar, 2) = MyLinearForm(0).X() * Vect(kk)
+ + MyLinearForm(0).Y() * Vect(kk+1);
+ }
+
+ if (MyContrOrder2 >= 1) {
+ H(MyNbVar-MyWithAuxValue, 2) = 0; // correct si il y a moins 3 noeuds
+ if (MyContrOrder2 == 2) {H(MyNbVar-MyWithAuxValue-1, 2) = 0;}
+ }
+ Vk = Vdeb;
+
+ Standard_Real Xaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).X(),
+ Yaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).Y();
+
+ kk = Indice(Vk, Vdeb-2*MyContrOrder1+2);
+ for (ii=DebH; ii<=FinH; ii++) {
+ H(ii, 2) = MyLinearForm(0).X() * Vect(kk)
+ + MyLinearForm(0).Y() * Vect(kk+1);
+ H(ii, 1) += Xaux * Vect(kk) + Yaux*Vect(kk+1);
+ kk += Vk;
+ Vk++;
+ }
+ }
+
+// calcul de la ligne lambda droite -----------------------
+ if (MyContrOrder2 >= 1) {
+
+ jj = FinH + 1;
+ Vk = Vfin + 2*MyContrOrder2 - 1;
+ kk = Indice(Vk, Vdeb);
+ for (ii=DebH; ii<=FinH; ii++) {
+ H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
+ + MyLinearForm(1).Y() * Vect(kk+Vk);
+ kk++;
+ }
+
+ kk = Indice(Vk, Vk);
+ H(jj, jj) = Cos1 * Vect(kk) + CosSin1 * Vect(kk+Vk) + Sin1 * Vect(kk+Vk+1);
+
+ if (MyContrOrder2 >= 2) {
+ // H(jj,jj) +=
+ gp_XY Laux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1));
+ jj = Vfin + 2*MyContrOrder2 - 3;
+ kk=Indice(jj+2, jj); // Xn-1Xn-2
+ ii=Indice(jj+3, jj); //Yn-1Xn-2
+ Standard_Integer ll = Indice(jj, jj);
+
+ H (FinH+1, FinH+1) += 2 * (
+ ( MyQuadForm(1).X() * Vect(jj) + MyQuadForm(1).Y() * Vect(jj+1) )
+ + ( Laux.X() * ( MyLinearForm(1).X()*Vect(kk) + MyLinearForm(1).Y()*Vect(ii))
+ + Laux.Y() * ( MyLinearForm(1).X()*Vect(kk+1) + MyLinearForm(1).Y()*Vect(ii+1)) )
+ + Laux.X() * Laux.Y() * Vect(ll+jj) )
+ + ( Pow(Laux.X(),2) * Vect(ll) + Pow(Laux.Y(),2) * Vect(ll+jj+1));
+
+ H(FinH+2, FinH+1) = Cos1 * Vect(kk) + CosSin1*(Vect(ii)+Vect(kk+1))/2 + Sin1 * Vect(ii+1);
+ gp_XY Aux(Vect(ll), Vect(ll+jj+1));
+ H(FinH+2, FinH+1) += Laux * MyLinearForm(1).Multiplied(Aux)
+ + (Laux.X()*MyLinearForm(1).Y() + Laux.Y()*MyLinearForm(1).X())
+ * Vect(ll+jj);
+// H(FinH+2, FinH+1) = 0; // faute de mieux ... Bug dans l'expression precedente
+ }
+ }
+
+// calcule de la ligne mu droite -----------------------
+ if (MyContrOrder2 >= 2) {
+ jj = FinH + 2;
+ Vk = Vfin + 2*MyContrOrder2 - 3;
+ kk = Indice(Vk, Vdeb);
+
+ Standard_Real Xaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).X(),
+ Yaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).Y();
+
+ for (ii=DebH; ii<=FinH; ii++) {
+ H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
+ + MyLinearForm(1).Y() * Vect(kk+Vk);
+ // update de la ligne Lambda droite
+ H(jj-1,ii) += Xaux*Vect(kk) + Yaux*Vect(kk+Vk);
+ kk++;
+ }
+ kk = Indice(Vk, Vk);
+ ii = Indice(Vk+1, Vk);
+ H(jj,jj) = Cos1*Vect(kk) + CosSin1*Vect(ii) + Sin1*Vect(ii+1);
+ }
+
+// calcule de la ligne Variable Auxiliaire -----------------------
+ if (MyWithAuxValue) {
+
+ kk = Indice(Vup, Vdeb);
+ for (ii=DebH; ii<=FinH; ii++) {
+ H(MyNbVar, ii) = Vect(kk);
+ kk++;
+ }
+
+ if (MyContrOrder2 >= 1) {
+ kk = Indice(Vup, Vfin+2*MyContrOrder2-1);
+ H(MyNbVar, FinH+1) =
+ MyLinearForm(1).X() * Vect(kk) + MyLinearForm(1).Y() * Vect(kk+1);
+ }
+ if (MyContrOrder2 >= 2) {
+ kk = Indice(Vup, Vfin+2*MyContrOrder2-3);
+ gp_XY Pole( Vect(kk), Vect(kk+1));
+ H(MyNbVar, FinH+1) += (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * Pole;
+ H(MyNbVar, FinH+2) = MyLinearForm(1) * Pole;
+ }
+ kk = Indice(Vup, Vup);
+ H(H.UpperRow(), H.UpperRow()) = Vect(kk);
+ }
+
+// recopie du bloc interne ---------------------------------------------
+
+ kk = Indice(Vdeb, Vdeb);
+ for (ii = DebH; ii <=FinH; ii++) {
+ for (jj = DebH; jj<=ii; jj++) {
+ H(ii,jj) = Vect(kk);
+ kk++;
+ }
+ kk += Vdeb-1;
+ }
+// symetrie
+ for (ii = H.LowerRow(); ii <= H.UpperRow(); ii++)
+ for (jj = ii+1; jj <= H.UpperRow(); jj++) H(ii,jj) = H(jj,ii);
+}
+
+//=======================================================================
+Standard_Boolean FairCurve_Energy::Variable(math_Vector& X) const
+//=======================================================================
+{
+ Standard_Integer ii,
+ IndexDeb1 = MyPoles->Lower()+1,
+ IndexDeb2 = X.Lower(),
+ IndexFin1 = MyPoles->Upper()-1,
+ IndexFin2 = X.Upper() - MyWithAuxValue; // on decremente de 1 si le glissement
+ // est libre car la derniere valeur de X lui est reserve.
+
+
+// calculs des variables de contraintes
+ if (MyContrOrder1 >= 1) {
+ X(IndexDeb2) = MyPoles->Value(MyPoles->Lower())
+ .Distance( MyPoles->Value(MyPoles->Lower()+1) );
+ IndexDeb1 += 1;
+ IndexDeb2 += 1;
+ }
+ if (MyContrOrder1 == 2) {
+ gp_Vec2d b1b2( MyPoles->Value(MyPoles->Lower()+1),
+ MyPoles->Value(MyPoles->Lower()+2) );
+ X(IndexDeb2) = b1b2.XY() * MyLinearForm(0);
+ IndexDeb1 += 1;
+ IndexDeb2 += 1;
+ }
+ if (MyContrOrder2 == 2) {
+ gp_Vec2d bn2bn1( MyPoles->Value(MyPoles->Upper()-2),
+ MyPoles->Value(MyPoles->Upper()-1));
+ X(IndexFin2) = - bn2bn1.XY() * MyLinearForm(1);
+ IndexFin1 -= 1;
+ IndexFin2 -= 1;
+ }
+ if (MyContrOrder2 >= 1) {
+ X(IndexFin2) = MyPoles->Value(MyPoles->Upper())
+ .Distance(MyPoles->Value(MyPoles->Upper()-1) );
+ IndexFin1 -= 1;
+ }
+
+// La recopie des variables auxiliaires n'est pas realise dans la classe abstraite
+
+// recopie des poles vers les variables
+ for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
+ X(IndexDeb2) = MyPoles->Value(ii).X();
+ X(IndexDeb2+1) = MyPoles->Value(ii).Y();
+ IndexDeb2 +=2;
+ }
+ return Standard_True;
+}
+
+//=======================================================================
+void FairCurve_Energy::ComputePoles(const math_Vector& X)
+//=======================================================================
+{
+ Standard_Integer ii,
+ IndexDeb1 = MyPoles->Lower()+1,
+ IndexDeb2 = X.Lower(),
+ IndexFin1 = MyPoles->Upper()-1,
+ IndexFin2 = X.Upper() - MyWithAuxValue; // on decremente de 1 si le glissement
+ // est libre car la derniere valeur de X lui est reserve.
+// calculs des poles contraints
+// for (ii=MyPoles->Lower();ii<=MyPoles->Upper();ii++) {
+// cout << ii << " X = " << MyPoles->Value(ii).X() <<
+// " Y = " << MyPoles->Value(ii).Y() << endl;}
+
+ if (MyContrOrder1 >= 1) {
+ IndexDeb1 += 1;
+ IndexDeb2 += 1;
+ ComputePolesG1( 0, X(1), MyPoles->Value(MyPoles->Lower()),
+ MyPoles->ChangeValue(MyPoles->Lower()+1) );
+ }
+ if (MyContrOrder1 == 2) {
+ IndexDeb1 += 1;
+ IndexDeb2 += 1;
+ ComputePolesG2( 0, X(1), X(2), MyPoles->Value(MyPoles->Lower()),
+ MyPoles->ChangeValue(MyPoles->Lower()+2) );
+ }
+ if (MyContrOrder2 == 2) {
+ IndexFin1 -= 1;
+ IndexFin2 -= 1;
+ ComputePolesG2( 1, X(IndexFin2), X(IndexFin2+1),
+ MyPoles->Value(MyPoles->Upper()),
+ MyPoles->ChangeValue(MyPoles->Upper()-2) );
+ }
+ if (MyContrOrder2 >= 1) {
+ IndexFin1 -= 1;
+ ComputePolesG1( 1, X(IndexFin2), MyPoles->Value(MyPoles->Upper()),
+ MyPoles->ChangeValue(MyPoles->Upper()-1) );
+ }
+
+// if (MyWithAuxValue) { MyLengthSliding = X(X.Upper()); }
+// recopie des autres
+ for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
+ MyPoles -> ChangeValue(ii).SetX( X(IndexDeb2) );
+ MyPoles -> ChangeValue(ii).SetY( X(IndexDeb2+1) );
+ IndexDeb2 += 2;
+ }
+}
+
+//=======================================================================
+void FairCurve_Energy::ComputePolesG1(const Standard_Integer Side,
+ const Standard_Real Lambda,
+ const gp_Pnt2d& P1,
+ gp_Pnt2d& P2) const
+//=======================================================================
+{ P2.SetXY ( P1.XY() + MyLinearForm(Side) * Lambda ); }
+
+//=======================================================================
+void FairCurve_Energy::ComputePolesG2(const Standard_Integer Side,
+ const Standard_Real Lambda,
+ const Standard_Real Rho,
+ const gp_Pnt2d& P1,
+ gp_Pnt2d& P2) const
+//=======================================================================
+{ P2.SetXY ( P1.XY()
+ + MyLinearForm(Side) * (Lambda + Rho )
+ + MyQuadForm(Side) * (Lambda * Lambda) ) ; }
+
+
+
+
projects / occt.git / blobdiff