[occt.git] / src / AppParCurves / AppParCurves.cxx

// Copyright (c) 1995-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.

#define No_Standard_RangeError
#define No_Standard_OutOfRange

#include <AppParCurves.ixx>
#include <BSplCLib.hxx>
#include <TColStd_Array1OfReal.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec2d.hxx>


void AppParCurves::BernsteinMatrix(const Standard_Integer NbPoles, 
				   const math_Vector& U,
				   math_Matrix& A) {

  Standard_Integer i, j, id;
  Standard_Real u0, u1, y0, y1, xs;
  Standard_Integer first = U.Lower(), last = U.Upper();
  math_Vector B(1, NbPoles-1);


  for (i = first; i <= last; i++) {
    B(1) = 1;
    u0 = U(i);
    u1 = 1.-u0;
    
    for (id = 2; id <= NbPoles-1; id++) {
      y0 = B(1);
      y1 = u0*y0;
      B(1) = y0-y1;
      for (j = 2; j <= id-1; j++) {
	xs = y1;
	y0 = B(j);
	y1 = u0*y0;
	B(j) = y0-y1+xs;
      }
      B(id) = y1;
    }
    A(i, 1) = u1*B(1);
    A(i, NbPoles) = u0*B(NbPoles-1);
    for (j = 2; j <= NbPoles-1; j++) {
      A(i, j) = u1*B(j)+u0*B(j-1);
    }
  }
}


void AppParCurves::Bernstein(const Standard_Integer NbPoles, 
			     const math_Vector& U,
			     math_Matrix& A,
			     math_Matrix& DA) {
  
  Standard_Integer i, j, id, Ndeg = NbPoles-1;
  Standard_Real u0, u1, y0, y1, xs, bj, bj1;;
  Standard_Integer first = U.Lower(), last = U.Upper();
  math_Vector B(1, NbPoles-1);

  
  for (i = first; i <= last; i++) {
    B(1) = 1;
    u0 = U(i);
    u1 = 1.-u0;
    
    for (id = 2; id <= NbPoles-1; id++) {
      y0 = B(1);
      y1 = u0*y0;
      B(1) = y0-y1;
      for (j = 2; j <= id-1; j++) {
	xs = y1;
	y0 = B(j);
	y1 = u0*y0;
	B(j) = y0-y1+xs;
      }
      B(id) = y1;
    }
    DA(i, 1) = -Ndeg*B(1);
    DA(i, NbPoles) = Ndeg*B(NbPoles-1);
    A(i, 1) = u1*B(1);
    A(i, NbPoles) = u0*B(NbPoles-1);
    for (j = 2; j <= NbPoles-1; j++) {
      bj = B(j);  bj1 = B(j-1);
      DA(i,j) = Ndeg*(bj1-bj);
      A(i, j) = u1*bj+u0*bj1;
    }
  }
}


void AppParCurves::SecondDerivativeBernstein(const Standard_Real U,
					     math_Vector&        DDA) {
//  Standard_Real U1 = 1-U, Y0, Y1, Xs;
  Standard_Real Y0, Y1, Xs;
  Standard_Integer NbPoles = DDA.Length();
  Standard_Integer id, j, N2, N3, N4, deg = NbPoles-1;
  N2 = deg-1; N3 = deg-2, N4 = deg*(deg-1);
  math_Vector B(1, deg-1);
  B(1) = 1.;
  
  // Cas particulier si degre = 1:
  if (deg == 1) {
    DDA(1) = 0.0;
    DDA(2) = 0.0;
  }
  else if (deg == 2) {
    DDA(1) = 2.0;
    DDA(2) = -4.0;
    DDA(3) = 2.0;
  }
  else {

    for (id = 2; id <= deg-1; id++) {
      Y0 = B(1);
      Y1 = U*Y0;
      B(1) = Y0-Y1;
      for (j = 2; j <= id-1; j++) {
	Xs = Y1;
	Y0 = B(j);
	Y1 = U*Y0;
	B(j) = Y0 - Y1 +Xs;
      }
      B(id) = Y1;
    }
    
    DDA(1) = N4*B(1);
    DDA(2) = N4*(-2*B(1) + B(2));
    DDA(deg) = N4*(B(deg-2) - 2*B(deg-1));
    DDA(deg+1) = N4*B(deg-1);
    
    for(j = 2; j <= deg-2; j++) {
      DDA(j+1) = N4*(B(j-1)-2*B(j)+B(j+1));
    }
  }
}


void AppParCurves::SplineFunction(const Standard_Integer nbpoles,
				  const Standard_Integer deg,
				  const math_Vector&     Parameters,
				  const math_Vector&     flatknots,
				  math_Matrix&           A,
				  math_Matrix&           DA,
				  math_IntegerVector&    index)
{
//  Standard_Real U, NewU, co, diff, t1, t2;
  Standard_Real U, NewU;
//  gp_Pnt2d Pt, P0;
//  gp_Vec2d V1;
//  Standard_Integer i, j, k, iter, in, ik, deg1 = deg+1;
  Standard_Integer i, j, deg1 = deg+1;
//  Standard_Integer oldkindex, kindex, theindex, ttindex;
  Standard_Integer oldkindex, kindex, theindex;
  math_Vector locpoles(1 , deg1);
  math_Vector locdpoles(1 , deg1);
  Standard_Integer firstp = Parameters.Lower(), lastp = Parameters.Upper();

  TColStd_Array1OfReal Aflatknots(flatknots.Lower(), flatknots.Upper());
  for (i = flatknots.Lower(); i <= flatknots.Upper(); i++) {
    Aflatknots(i) = flatknots(i);
  }


  oldkindex = 1;
  
  Standard_Integer pp, qq;
  Standard_Real Saved, Inverse, LocalInverse, locqq, locdqq, val;

  for (i = firstp; i <= lastp; i++) {
    U = Parameters(i);
    NewU = U;
    kindex = oldkindex;
    BSplCLib::LocateParameter(deg, Aflatknots, U, Standard_False, 
			      deg1, nbpoles+1, kindex, NewU);
    
    oldkindex = kindex;
    
    // On stocke les index:
    index(i) = kindex - deg-1;
    
    locpoles(1) = 1.0;
    
    for (qq = 2; qq <= deg; qq++) {
      locpoles(qq) = 0.0;
      for (pp = 1; pp <= qq-1; pp++) {
	Inverse = 1.0 / (flatknots(kindex + pp)  - flatknots(kindex - qq + pp + 1)) ; 
        Saved = (U - flatknots(kindex - qq + pp + 1)) * Inverse * locpoles(pp);
        locpoles(pp) *= (flatknots(kindex + pp) - U) * Inverse ;
        locpoles(pp) += locpoles(qq) ;
        locpoles(qq) = Saved ;
      }
    }

    qq = deg+1;
    for (pp = 1; pp <= deg; pp++) {
      locdpoles(pp)= locpoles(pp);
    }

    locqq = 0.0;
    locdqq = 0.0;
    for (pp = 1; pp <= deg; pp++) {
      Inverse =  1.0 / (flatknots(kindex + pp)  - flatknots(kindex - qq + pp + 1)); 
      Saved = (U - flatknots(kindex - qq + pp + 1)) * Inverse * locpoles(pp);
      locpoles(pp) *= (flatknots(kindex + pp) - U) * Inverse;
      locpoles(pp) += locqq;
      locqq = Saved ;
      LocalInverse = (Standard_Real) (deg) * Inverse;
      Saved = LocalInverse * locdpoles(pp);
      locdpoles(pp) *= - LocalInverse ;
      locdpoles(pp) +=   locdqq;
      locdqq = Saved ;
    }

    locpoles(qq) = locqq;
    locdpoles(qq) = locdqq;

    for (j = 1; j <= deg1; j++) {
      val = locpoles(j);
      theindex = j+oldkindex-deg1;
      A(i, theindex) = val;
      DA(i, theindex) = locdpoles(j);
    }

    for (j = 1; j < oldkindex-deg; j++)
      A(i, j) = DA(i, j) = 0.0;
    for (j = oldkindex+1; j <= nbpoles; j++)
      A(i, j) = DA(i, j) = 0.0;
    
  }

}
Commit	Line	Data
b311480e	1	// Copyright (c) 1995-1999 Matra Datavision
973c2be1	2	// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e	3	//
973c2be1	4	// This file is part of Open CASCADE Technology software library.
b311480e	5	//
973c2be1	6	// This library is free software; you can redistribute it and / or modify it
	7	// under the terms of the GNU Lesser General Public version 2.1 as published
	8	// by the Free Software Foundation, with special exception defined in the file
	9	// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
	10	// distribution for complete text of the license and disclaimer of any warranty.
b311480e	11	//
973c2be1	12	// Alternatively, this file may be used under the terms of Open CASCADE
973c2be1	13	// commercial license or contractual agreement.
7fd59977	14
	15	#define No_Standard_RangeError
	16	#define No_Standard_OutOfRange
	17
	18	#include <AppParCurves.ixx>
	19	#include <BSplCLib.hxx>
	20	#include <TColStd_Array1OfReal.hxx>
	21	#include <gp_Pnt2d.hxx>
	22	#include <gp_Vec2d.hxx>
	23
	24
	25	void AppParCurves::BernsteinMatrix(const Standard_Integer NbPoles,
	26	const math_Vector& U,
	27	math_Matrix& A) {
	28
	29	Standard_Integer i, j, id;
	30	Standard_Real u0, u1, y0, y1, xs;
	31	Standard_Integer first = U.Lower(), last = U.Upper();
	32	math_Vector B(1, NbPoles-1);
	33
	34
	35	for (i = first; i <= last; i++) {
	36	B(1) = 1;
	37	u0 = U(i);
	38	u1 = 1.-u0;
	39
	40	for (id = 2; id <= NbPoles-1; id++) {
	41	y0 = B(1);
	42	y1 = u0*y0;
	43	B(1) = y0-y1;
	44	for (j = 2; j <= id-1; j++) {
	45	xs = y1;
	46	y0 = B(j);
	47	y1 = u0*y0;
	48	B(j) = y0-y1+xs;
	49	}
	50	B(id) = y1;
	51	}
	52	A(i, 1) = u1*B(1);
	53	A(i, NbPoles) = u0*B(NbPoles-1);
	54	for (j = 2; j <= NbPoles-1; j++) {
	55	A(i, j) = u1B(j)+u0B(j-1);
	56	}
	57	}
	58	}
	59
	60
	61	void AppParCurves::Bernstein(const Standard_Integer NbPoles,
	62	const math_Vector& U,
	63	math_Matrix& A,
	64	math_Matrix& DA) {
	65
	66	Standard_Integer i, j, id, Ndeg = NbPoles-1;
	67	Standard_Real u0, u1, y0, y1, xs, bj, bj1;;
	68	Standard_Integer first = U.Lower(), last = U.Upper();
	69	math_Vector B(1, NbPoles-1);
	70
	71
	72	for (i = first; i <= last; i++) {
	73	B(1) = 1;
	74	u0 = U(i);
	75	u1 = 1.-u0;
	76
	77	for (id = 2; id <= NbPoles-1; id++) {
78	y0 = B(1);
79	y1 = u0*y0;
80	B(1) = y0-y1;
81	for (j = 2; j <= id-1; j++) {
82	xs = y1;
83	y0 = B(j);
84	y1 = u0*y0;
85	B(j) = y0-y1+xs;
86	}
87	B(id) = y1;
88	}
89	DA(i, 1) = -Ndeg*B(1);
90	DA(i, NbPoles) = Ndeg*B(NbPoles-1);
91	A(i, 1) = u1*B(1);
92	A(i, NbPoles) = u0*B(NbPoles-1);
93	for (j = 2; j <= NbPoles-1; j++) {
94	bj = B(j); bj1 = B(j-1);
95	DA(i,j) = Ndeg*(bj1-bj);
96	A(i, j) = u1bj+u0bj1;
97	}
98	}
99	}
100
101
102	void AppParCurves::SecondDerivativeBernstein(const Standard_Real U,
103	math_Vector& DDA) {
104	// Standard_Real U1 = 1-U, Y0, Y1, Xs;
105	Standard_Real Y0, Y1, Xs;
106	Standard_Integer NbPoles = DDA.Length();
107	Standard_Integer id, j, N2, N3, N4, deg = NbPoles-1;
108	N2 = deg-1; N3 = deg-2, N4 = deg*(deg-1);
109	math_Vector B(1, deg-1);
110	B(1) = 1.;
111
112	// Cas particulier si degre = 1:
113	if (deg == 1) {
114	DDA(1) = 0.0;
115	DDA(2) = 0.0;
116	}
117	else if (deg == 2) {
118	DDA(1) = 2.0;
119	DDA(2) = -4.0;
120	DDA(3) = 2.0;
121	}
122	else {
123
124	for (id = 2; id <= deg-1; id++) {
125	Y0 = B(1);
126	Y1 = U*Y0;
127	B(1) = Y0-Y1;
128	for (j = 2; j <= id-1; j++) {
129	Xs = Y1;
130	Y0 = B(j);
131	Y1 = U*Y0;
132	B(j) = Y0 - Y1 +Xs;
133	}
134	B(id) = Y1;
135	}
136
137	DDA(1) = N4*B(1);
138	DDA(2) = N4(-2B(1) + B(2));
139	DDA(deg) = N4(B(deg-2) - 2B(deg-1));
140	DDA(deg+1) = N4*B(deg-1);
141
142	for(j = 2; j <= deg-2; j++) {
143	DDA(j+1) = N4(B(j-1)-2B(j)+B(j+1));
144	}
145	}
146	}
147
148
149
150	void AppParCurves::SplineFunction(const Standard_Integer nbpoles,
151	const Standard_Integer deg,
152	const math_Vector& Parameters,
153	const math_Vector& flatknots,
154	math_Matrix& A,
155	math_Matrix& DA,
156	math_IntegerVector& index)
157	{
158	// Standard_Real U, NewU, co, diff, t1, t2;
159	Standard_Real U, NewU;
160	// gp_Pnt2d Pt, P0;
161	// gp_Vec2d V1;
162	// Standard_Integer i, j, k, iter, in, ik, deg1 = deg+1;
163	Standard_Integer i, j, deg1 = deg+1;
164	// Standard_Integer oldkindex, kindex, theindex, ttindex;
165	Standard_Integer oldkindex, kindex, theindex;
166	math_Vector locpoles(1 , deg1);
167	math_Vector locdpoles(1 , deg1);
168	Standard_Integer firstp = Parameters.Lower(), lastp = Parameters.Upper();
169
170	TColStd_Array1OfReal Aflatknots(flatknots.Lower(), flatknots.Upper());
171	for (i = flatknots.Lower(); i <= flatknots.Upper(); i++) {
172	Aflatknots(i) = flatknots(i);
173	}
174
175
176	oldkindex = 1;
177
178	Standard_Integer pp, qq;
179	Standard_Real Saved, Inverse, LocalInverse, locqq, locdqq, val;
180
181	for (i = firstp; i <= lastp; i++) {
182	U = Parameters(i);
183	NewU = U;
184	kindex = oldkindex;
185	BSplCLib::LocateParameter(deg, Aflatknots, U, Standard_False,
186	deg1, nbpoles+1, kindex, NewU);
187
188	oldkindex = kindex;
189
190	// On stocke les index:
191	index(i) = kindex - deg-1;
192
193	locpoles(1) = 1.0;
194
195	for (qq = 2; qq <= deg; qq++) {
196	locpoles(qq) = 0.0;
197	for (pp = 1; pp <= qq-1; pp++) {
198	Inverse = 1.0 / (flatknots(kindex + pp) - flatknots(kindex - qq + pp + 1)) ;
199	Saved = (U - flatknots(kindex - qq + pp + 1)) * Inverse * locpoles(pp);
200	locpoles(pp) = (flatknots(kindex + pp) - U) Inverse ;
201	locpoles(pp) += locpoles(qq) ;
202	locpoles(qq) = Saved ;
203	}
204	}
205
206	qq = deg+1;
207	for (pp = 1; pp <= deg; pp++) {
208	locdpoles(pp)= locpoles(pp);
209	}
210
211	locqq = 0.0;
212	locdqq = 0.0;
213	for (pp = 1; pp <= deg; pp++) {
214	Inverse = 1.0 / (flatknots(kindex + pp) - flatknots(kindex - qq + pp + 1));
215	Saved = (U - flatknots(kindex - qq + pp + 1)) * Inverse * locpoles(pp);
216	locpoles(pp) = (flatknots(kindex + pp) - U) Inverse;
217	locpoles(pp) += locqq;
218	locqq = Saved ;
219	LocalInverse = (Standard_Real) (deg) * Inverse;
220	Saved = LocalInverse * locdpoles(pp);
221	locdpoles(pp) *= - LocalInverse ;
222	locdpoles(pp) += locdqq;
223	locdqq = Saved ;
224	}
225
226	locpoles(qq) = locqq;
227	locdpoles(qq) = locdqq;
228
229	for (j = 1; j <= deg1; j++) {
230	val = locpoles(j);
231	theindex = j+oldkindex-deg1;
232	A(i, theindex) = val;
233	DA(i, theindex) = locdpoles(j);
234	}
235
236	for (j = 1; j < oldkindex-deg; j++)
237	A(i, j) = DA(i, j) = 0.0;
238	for (j = oldkindex+1; j <= nbpoles; j++)
239	A(i, j) = DA(i, j) = 0.0;
240
241	}
242
243	}