[occt.git] / src / AppParCurves / AppParCurves_Variational_8.gxx

// Created on: 1999-02-15
// Created by: Igor FEOKTISTOV
// Copyright (c) 1999-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and / or modify it
// under the terms of the GNU Lesser General Public version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <PLib_Base.hxx>
#include <PLib_JacobiPolynomial.hxx>
#include <PLib_HermitJacobi.hxx>
#include <TColStd_Array1OfReal.hxx>
#include <math_Vector.hxx>

void AppParCurves_Variational::AssemblingConstraints(const Handle(FEmTool_Curve)& Curve,
						     const TColStd_Array1OfReal& Parameters,
						     const Standard_Real CBLONG,
						     FEmTool_Assembly& A ) const
{

  Standard_Integer MxDeg = Curve->Base()->WorkDegree(),
                   NbElm = Curve->NbElements(),
                   NbDim = Curve->Dimension();

  TColStd_Array1OfReal G0(0, MxDeg), G1(0, MxDeg), G2(0, MxDeg);
  math_Vector V0((Standard_Real*)&G0(0), 0, MxDeg), 
              V1((Standard_Real*)&G1(0), 0, MxDeg), 
              V2((Standard_Real*)&G2(0), 0, MxDeg);

  Standard_Integer IndexOfConstraint, Ng3d, Ng2d, NBeg2d, NPass, NgPC1, 
                   NTang3d, NTang2d,  
                   Point, TypOfConstr, 
                   p0 = Parameters.Lower() - myFirstPoint,
                   curel = 1, el, i, ipnt, ityp, j, k, pnt, curdim,
                   jt, Ntheta = 6 * myNbP3d + 2 * myNbP2d;
  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  
//  Ng3d = 3 * NbConstr + 2 * myNbTangPoints + 5 * myNbCurvPoints;
//  Ng2d = 2 * NbConstr + 1 * myNbTangPoints + 3 * myNbCurvPoints;
  Ng3d = 3 * NbConstr + 3 * myNbTangPoints + 5 * myNbCurvPoints;
  Ng2d = 2 * NbConstr + 2 * myNbTangPoints + 3 * myNbCurvPoints;
  NBeg2d = Ng3d * myNbP3d;
//  NgPC1 = NbConstr + myNbCurvPoints;
  NgPC1 = NbConstr + myNbTangPoints + myNbCurvPoints;
  NPass = 0; 
  NTang3d = 3 * NgPC1;
  NTang2d = 2 * NgPC1;

  TColStd_Array1OfReal& Intervals = Curve->Knots();

  Standard_Real t, R1, R2;

  Handle(PLib_Base) myBase = Curve->Base();
  Handle(PLib_HermitJacobi) myHermitJacobi = (*((Handle(PLib_HermitJacobi)*)&myBase));
  Standard_Integer Order = myHermitJacobi->NivConstr() + 1;

  Standard_Real UFirst, ULast, coeff, c0, mfact, mfact1;

  A.NullifyConstraint();

  ipnt = -1;
  ityp = 0;
  for(i = 1; i <= NbConstr; i++) {

    ipnt += 2; ityp += 2;

    Point = myTypConstraints->Value(ipnt);
    TypOfConstr = myTypConstraints->Value(ityp);

    t = Parameters(p0 + Point);

    for(el = curel; el <= NbElm; ) {
      if( t <= Intervals(++el) ) {
	curel = el - 1;
	break;
      }
    }
    

    UFirst = Intervals(curel); ULast = Intervals(curel + 1);
    coeff = (ULast - UFirst)/2.; c0 = (ULast + UFirst)/2.;

    t = (t - c0) / coeff;
    
    if(TypOfConstr == 0) {
      myBase->D0(t, G0);
      for(k = 1; k < Order; k++) {
	mfact = Pow(coeff, k);
	G0(k) *= mfact;
	G0(k + Order) *= mfact;
      }
    }
    else if(TypOfConstr == 1) {
      myBase->D1(t, G0, G1);
      for(k = 1; k < Order; k++) {
	mfact = Pow(coeff, k);
	G0(k) *= mfact;
	G0(k + Order) *= mfact;
	G1(k) *= mfact;
	G1(k + Order) *= mfact;
      }
      mfact = 1./coeff;
      for(k = 0; k <= MxDeg; k++) {
	G1(k) *= mfact;
      }
    }
    else {
      myBase->D2(t, G0, G1, G2);
      for(k = 1; k < Order; k++) {
	mfact = Pow(coeff, k);
	G0(k) *= mfact;
	G0(k + Order) *= mfact;
	G1(k) *= mfact;
	G1(k + Order) *= mfact;
	G2(k) *= mfact;
	G2(k + Order) *= mfact;
      }
      mfact = 1. / coeff;
      mfact1 = mfact / coeff;
      for(k = 0; k <= MxDeg; k++) {
	G1(k) *= mfact;
	G2(k) *= mfact1;
      }
    }

    NPass++;

    j = NbDim * (Point - myFirstPoint);
    Standard_Integer n0 = NPass;
    curdim = 0;
    for(pnt = 1; pnt <= myNbP3d; pnt++) {
      IndexOfConstraint = n0;
      for(k = 1; k <= 3; k++) {
	curdim++;
	A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
	IndexOfConstraint += NgPC1;
      }
      j +=3;
      n0 += Ng3d;
    }

    n0 = NPass + NBeg2d;
    for(pnt = 1; pnt <= myNbP2d; pnt++) {
      IndexOfConstraint = n0;
      for(k = 1; k <= 2; k++) {
	curdim++;
	A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
	IndexOfConstraint += NgPC1;
      }
      j +=2;
      n0 += Ng2d;
    }

/*    if(TypOfConstr == 1) {

      IndexOfConstraint = NTang3d + 1;
      jt = Ntheta * (i - 1);
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	for(k = 1; k <= 3; k++) {
	  A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) *  V1, 0.);
	  A.AddConstraint(IndexOfConstraint + 1, curel, k, myTtheta->Value(jt + 3 + k) *  V1, 0.);
	}
	IndexOfConstraint += Ng3d;
	jt += 6;
      }

      IndexOfConstraint = NBeg2d + NTang2d + 1;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	for(k = 1; k <= 2; k++) {
	  A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) * V1, 0.);
	}
	IndexOfConstraint += Ng2d;
	jt += 2;
      }

      NTang3d += 2;
      NTang2d += 1;
    } */
    if(TypOfConstr == 1) {

      NPass++;
      n0 = NPass;
      j = 2 * NbDim * (i - 1);
      curdim = 0;
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 3; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng3d;
	j += 6;
      }

      n0 = NPass + NBeg2d;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 2; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng2d;
	j += 4;
      }
    }
    if(TypOfConstr == 2) {

      NPass++;
      n0 = NPass;
      j = 2 * NbDim * (i - 1);
      curdim = 0;
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 3; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng3d;
	j += 6;
      }

      n0 = NPass + NBeg2d;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	IndexOfConstraint = n0;
	for(k = 1; k <= 2; k++) {
	  curdim++;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
	  IndexOfConstraint += NgPC1;
	}
	n0 += Ng2d;
	j += 4;
      }

      j = 2 * NbDim * (i - 1) + 3;
      jt = Ntheta * (i - 1);
      IndexOfConstraint = NTang3d + 1;
      curdim = 0;
      for(pnt = 1; pnt <= myNbP3d; pnt++) {
	R1 = 0.; R2 = 0.;
	for(k = 1; k <= 3; k++) { 
	  R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
	  R2 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + 3 + k);
	}
	R1 *= CBLONG * CBLONG;
	R2 *= CBLONG * CBLONG;
	for(k = 1; k <= 3; k++) {
	  curdim++;
	  if(k > 1) R1 = R2 = 0.;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
	  A.AddConstraint(IndexOfConstraint + 1, curel, curdim, myTfthet->Value(jt + 3 + k) * V2, R2);
	}
	IndexOfConstraint += Ng3d;
	j += 6;
	jt += 6;
      }

      j--;
      IndexOfConstraint = NBeg2d + NTang2d + 1;
      for(pnt = 1; pnt <= myNbP2d; pnt++) {
	R1 = 0.;
	for(k = 1; k <= 2; k++) { 
	  R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
	}
	R1 *= CBLONG * CBLONG;
	for(k = 1; k <= 2; k++) {
	  curdim++;
	  if(k > 1) R1 = 0.;
	  A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
	}
	IndexOfConstraint += Ng2d;
	j += 4;
	jt += 2;
      }

      NTang3d += 2;
      NTang2d += 1;
    }

  }

}
Commit	Line	Data
b311480e	1	// Created on: 1999-02-15
	2	// Created by: Igor FEOKTISTOV
	3	// Copyright (c) 1999-1999 Matra Datavision
973c2be1	4	// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e	5	//
973c2be1	6	// This file is part of Open CASCADE Technology software library.
b311480e	7	//
973c2be1	8	// This library is free software; you can redistribute it and / or modify it
	9	// under the terms of the GNU Lesser General Public version 2.1 as published
	10	// by the Free Software Foundation, with special exception defined in the file
	11	// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
	12	// distribution for complete text of the license and disclaimer of any warranty.
b311480e	13	//
973c2be1	14	// Alternatively, this file may be used under the terms of Open CASCADE
973c2be1	15	// commercial license or contractual agreement.
7fd59977	16
	17	#include <PLib_Base.hxx>
	18	#include <PLib_JacobiPolynomial.hxx>
	19	#include <PLib_HermitJacobi.hxx>
	20	#include <TColStd_Array1OfReal.hxx>
	21	#include <math_Vector.hxx>
	22
	23	void AppParCurves_Variational::AssemblingConstraints(const Handle(FEmTool_Curve)& Curve,
	24	const TColStd_Array1OfReal& Parameters,
	25	const Standard_Real CBLONG,
	26	FEmTool_Assembly& A ) const
	27	{
	28
	29	Standard_Integer MxDeg = Curve->Base()->WorkDegree(),
	30	NbElm = Curve->NbElements(),
	31	NbDim = Curve->Dimension();
	32
	33	TColStd_Array1OfReal G0(0, MxDeg), G1(0, MxDeg), G2(0, MxDeg);
	34	math_Vector V0((Standard_Real*)&G0(0), 0, MxDeg),
	35	V1((Standard_Real*)&G1(0), 0, MxDeg),
	36	V2((Standard_Real*)&G2(0), 0, MxDeg);
	37
	38	Standard_Integer IndexOfConstraint, Ng3d, Ng2d, NBeg2d, NPass, NgPC1,
	39	NTang3d, NTang2d,
	40	Point, TypOfConstr,
	41	p0 = Parameters.Lower() - myFirstPoint,
	42	curel = 1, el, i, ipnt, ityp, j, k, pnt, curdim,
	43	jt, Ntheta = 6 * myNbP3d + 2 * myNbP2d;
	44	Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
	45
	46	// Ng3d = 3 * NbConstr + 2 * myNbTangPoints + 5 * myNbCurvPoints;
	47	// Ng2d = 2 * NbConstr + 1 * myNbTangPoints + 3 * myNbCurvPoints;
	48	Ng3d = 3 * NbConstr + 3 * myNbTangPoints + 5 * myNbCurvPoints;
	49	Ng2d = 2 * NbConstr + 2 * myNbTangPoints + 3 * myNbCurvPoints;
	50	NBeg2d = Ng3d * myNbP3d;
	51	// NgPC1 = NbConstr + myNbCurvPoints;
	52	NgPC1 = NbConstr + myNbTangPoints + myNbCurvPoints;
	53	NPass = 0;
	54	NTang3d = 3 * NgPC1;
	55	NTang2d = 2 * NgPC1;
	56
	57	TColStd_Array1OfReal& Intervals = Curve->Knots();
	58
	59	Standard_Real t, R1, R2;
	60
	61	Handle(PLib_Base) myBase = Curve->Base();
	62	Handle(PLib_HermitJacobi) myHermitJacobi = (((Handle(PLib_HermitJacobi))&myBase));
	63	Standard_Integer Order = myHermitJacobi->NivConstr() + 1;
	64
	65	Standard_Real UFirst, ULast, coeff, c0, mfact, mfact1;
	66
	67	A.NullifyConstraint();
	68
	69	ipnt = -1;
	70	ityp = 0;
	71	for(i = 1; i <= NbConstr; i++) {
	72
	73	ipnt += 2; ityp += 2;
	74
	75	Point = myTypConstraints->Value(ipnt);
	76	TypOfConstr = myTypConstraints->Value(ityp);
	77
	78	t = Parameters(p0 + Point);
	79
80	for(el = curel; el <= NbElm; ) {
81	if( t <= Intervals(++el) ) {
82	curel = el - 1;
83	break;
84	}
85	}
86
87
88	UFirst = Intervals(curel); ULast = Intervals(curel + 1);
89	coeff = (ULast - UFirst)/2.; c0 = (ULast + UFirst)/2.;
90
91	t = (t - c0) / coeff;
92
93	if(TypOfConstr == 0) {
94	myBase->D0(t, G0);
95	for(k = 1; k < Order; k++) {
96	mfact = Pow(coeff, k);
97	G0(k) *= mfact;
98	G0(k + Order) *= mfact;
99	}
100	}
101	else if(TypOfConstr == 1) {
102	myBase->D1(t, G0, G1);
103	for(k = 1; k < Order; k++) {
104	mfact = Pow(coeff, k);
105	G0(k) *= mfact;
106	G0(k + Order) *= mfact;
107	G1(k) *= mfact;
108	G1(k + Order) *= mfact;
109	}
110	mfact = 1./coeff;
111	for(k = 0; k <= MxDeg; k++) {
112	G1(k) *= mfact;
113	}
114	}
115	else {
116	myBase->D2(t, G0, G1, G2);
117	for(k = 1; k < Order; k++) {
118	mfact = Pow(coeff, k);
119	G0(k) *= mfact;
120	G0(k + Order) *= mfact;
121	G1(k) *= mfact;
122	G1(k + Order) *= mfact;
123	G2(k) *= mfact;
124	G2(k + Order) *= mfact;
125	}
126	mfact = 1. / coeff;
127	mfact1 = mfact / coeff;
128	for(k = 0; k <= MxDeg; k++) {
129	G1(k) *= mfact;
130	G2(k) *= mfact1;
131	}
132	}
133
134	NPass++;
135
136	j = NbDim * (Point - myFirstPoint);
137	Standard_Integer n0 = NPass;
138	curdim = 0;
139	for(pnt = 1; pnt <= myNbP3d; pnt++) {
140	IndexOfConstraint = n0;
141	for(k = 1; k <= 3; k++) {
142	curdim++;
143	A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
144	IndexOfConstraint += NgPC1;
145	}
146	j +=3;
147	n0 += Ng3d;
148	}
149
150	n0 = NPass + NBeg2d;
151	for(pnt = 1; pnt <= myNbP2d; pnt++) {
152	IndexOfConstraint = n0;
153	for(k = 1; k <= 2; k++) {
154	curdim++;
155	A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
156	IndexOfConstraint += NgPC1;
157	}
158	j +=2;
159	n0 += Ng2d;
160	}
161
162	/* if(TypOfConstr == 1) {
163
164	IndexOfConstraint = NTang3d + 1;
165	jt = Ntheta * (i - 1);
166	for(pnt = 1; pnt <= myNbP3d; pnt++) {
167	for(k = 1; k <= 3; k++) {
168	A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) * V1, 0.);
169	A.AddConstraint(IndexOfConstraint + 1, curel, k, myTtheta->Value(jt + 3 + k) * V1, 0.);
170	}
171	IndexOfConstraint += Ng3d;
172	jt += 6;
173	}
174
175	IndexOfConstraint = NBeg2d + NTang2d + 1;
176	for(pnt = 1; pnt <= myNbP2d; pnt++) {
177	for(k = 1; k <= 2; k++) {
178	A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) * V1, 0.);
179	}
180	IndexOfConstraint += Ng2d;
181	jt += 2;
182	}
183
184	NTang3d += 2;
185	NTang2d += 1;
186	} */
187	if(TypOfConstr == 1) {
188
189	NPass++;
190	n0 = NPass;
191	j = 2 * NbDim * (i - 1);
192	curdim = 0;
193	for(pnt = 1; pnt <= myNbP3d; pnt++) {
194	IndexOfConstraint = n0;
195	for(k = 1; k <= 3; k++) {
196	curdim++;
197	A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
198	IndexOfConstraint += NgPC1;
199	}
200	n0 += Ng3d;
201	j += 6;
202	}
203
204	n0 = NPass + NBeg2d;
205	for(pnt = 1; pnt <= myNbP2d; pnt++) {
206	IndexOfConstraint = n0;
207	for(k = 1; k <= 2; k++) {
208	curdim++;
209	A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
210	IndexOfConstraint += NgPC1;
211	}
212	n0 += Ng2d;
213	j += 4;
214	}
215	}
216	if(TypOfConstr == 2) {
217
218	NPass++;
219	n0 = NPass;
220	j = 2 * NbDim * (i - 1);
221	curdim = 0;
222	for(pnt = 1; pnt <= myNbP3d; pnt++) {
223	IndexOfConstraint = n0;
224	for(k = 1; k <= 3; k++) {
225	curdim++;
226	A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
227	IndexOfConstraint += NgPC1;
228	}
229	n0 += Ng3d;
230	j += 6;
231	}
232
233	n0 = NPass + NBeg2d;
234	for(pnt = 1; pnt <= myNbP2d; pnt++) {
235	IndexOfConstraint = n0;
236	for(k = 1; k <= 2; k++) {
237	curdim++;
238	A.AddConstraint(IndexOfConstraint, curel, curdim, V1, CBLONG * myTabConstraints->Value(j + k));
239	IndexOfConstraint += NgPC1;
240	}
241	n0 += Ng2d;
242	j += 4;
243	}
244
245	j = 2 * NbDim * (i - 1) + 3;
246	jt = Ntheta * (i - 1);
247	IndexOfConstraint = NTang3d + 1;
248	curdim = 0;
249	for(pnt = 1; pnt <= myNbP3d; pnt++) {
250	R1 = 0.; R2 = 0.;
251	for(k = 1; k <= 3; k++) {
252	R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
253	R2 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + 3 + k);
254	}
255	R1 = CBLONG CBLONG;
256	R2 = CBLONG CBLONG;
257	for(k = 1; k <= 3; k++) {
258	curdim++;
259	if(k > 1) R1 = R2 = 0.;
260	A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
261	A.AddConstraint(IndexOfConstraint + 1, curel, curdim, myTfthet->Value(jt + 3 + k) * V2, R2);
262	}
263	IndexOfConstraint += Ng3d;
264	j += 6;
265	jt += 6;
266	}
267
268	j--;
269	IndexOfConstraint = NBeg2d + NTang2d + 1;
270	for(pnt = 1; pnt <= myNbP2d; pnt++) {
271	R1 = 0.;
272	for(k = 1; k <= 2; k++) {
273	R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
274	}
275	R1 = CBLONG CBLONG;
276	for(k = 1; k <= 2; k++) {
277	curdim++;
278	if(k > 1) R1 = 0.;
279	A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
280	}
281	IndexOfConstraint += Ng2d;
282	j += 4;
283	jt += 2;
284	}
285
286	NTang3d += 2;
287	NTang2d += 1;
288	}
289
290	}
291
292	}