mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-06 21:57:47 +00:00
GEEKING
This commit is contained in:
watrabi
@@ -0,0 +1,373 @@
|
||||
#include "../sg.h"
|
||||
static sgFloat min_function ( lpD_POINT p1, lpD_POINT p2 );
|
||||
static sgFloat min_derivativ( lpD_POINT p1, lpD_POINT p2, lpD_POINT d );
|
||||
static sgFloat sqrt_derivates( sgFloat *G, short n );
|
||||
/**********************************************************
|
||||
* function - function to be minimazied
|
||||
* derivativs - derivativ of function to be minimazied
|
||||
* C1, C2 - function's parameters
|
||||
* X - beginning point, return - point of local min
|
||||
* n - number of parameters
|
||||
* number_of_iter - acceptable number of iterations
|
||||
* for this reliz number of parameter <=5
|
||||
*/
|
||||
sgFloat Fletchjer_Paur( functionFP function, derivativs derivativs,
|
||||
void *C1, void *C2, sgFloat *X, short n,
|
||||
short number_of_iteration ){
|
||||
short i, j, iter;
|
||||
sgFloat H[5][5]; // matrix must be n x n !!!!!!!!!!!!!!!!!!
|
||||
sgFloat P[5], Q[5], U[5], V[5], Y[5], G[5], D[5], M[5];
|
||||
sgFloat z, w, r, kk, wk, dk, Fp, Fq, Fr, step, Gp, Gq, Gr, min1=-1;
|
||||
sgFloat G3, eps_dd, eps_nn;
|
||||
|
||||
eps_dd=eps_d*eps_d;
|
||||
eps_nn=eps_n*eps_n;
|
||||
//bilding of begining matrix H - it's unit matrix
|
||||
for( i=0; i<n; i++ ) for( j=0; j<n; j++ ) H[i][j] = ( i==j ) ? (1.) : (0.);
|
||||
|
||||
for( iter=0; iter <= number_of_iteration; iter++ ){
|
||||
for( i=0; i<n; i++ ) Y[i] = P[i] = X[i];
|
||||
|
||||
//calculating of new quantity of function into point X
|
||||
Fp = (*function)( C1, C2, X );
|
||||
if( fabs(Fp) < eps_dd ) return (0);
|
||||
|
||||
//calculating of new paticular derivates of function
|
||||
/*G3 =*/ (*derivativs)( C1, C2, X, G );
|
||||
|
||||
//calculating of beginning direction {d} = -[H]{g};
|
||||
for( i=0; i<n; i++ ){
|
||||
U[i] = G[i];
|
||||
for( D[i]=0., j=0; j<n; j++ ) D[i] -= H[i][j]*G[j];
|
||||
}
|
||||
//linear founding of min on function f( x + step*d )
|
||||
//------------------> calculating of point P
|
||||
while(1){
|
||||
//calculating of gradient for point P
|
||||
for( Gp = 0., i=0; i<n; i++ ) Gp += G[i]*D[i];
|
||||
|
||||
//founding of min along direction and choising of step from formula
|
||||
// min{1, -2(Fp - Fm)/Gp}
|
||||
// where Fp = F(P),
|
||||
// Fm - approximation of real quantity of min, in my case Fm = 0
|
||||
if( fabs(Gp) < eps_dd ) { step = 1.; break; }
|
||||
if( ( step = fabs( 2*Fp/Gp ) ) > 1. ) step = 1.;
|
||||
if( Gp < -eps_dd ) break;
|
||||
|
||||
// calculating of new point P
|
||||
for( i=0; i<n; i++ ) P[i] = X[i] = P[i] - step*D[i];
|
||||
|
||||
//calculating of new quantity of function into point P
|
||||
Fp = (*function)( C1, C2, P );
|
||||
if( fabs(Fp) < eps_dd ) return (0);
|
||||
|
||||
//calculating of new paticular derivates of function
|
||||
/*G3 =*/ (*derivativs)( C1, C2, P, G );
|
||||
}
|
||||
|
||||
//------------------> calculating of point Q
|
||||
while(1){
|
||||
//calculating of next point Q x(i+1) = x(i) + step*d(i)
|
||||
for( i=0; i<n; i++ ) Q[i] = X[i] = P[i] + step*D[i];
|
||||
|
||||
//calculating of new quantity of function into point Q
|
||||
Fq = (*function)( C1, C2, Q );
|
||||
if( fabs( Fq ) < eps_dd ) return (0);
|
||||
|
||||
//calculating of new paticular derivates of function
|
||||
/*G3 =*/ (*derivativs)( C1, C2, Q, G );
|
||||
|
||||
//calculating of gradient for point Q
|
||||
for( Gq = 0., i=0; i<n; i++ ) Gq += G[i]*D[i];
|
||||
// if( Gq > 0. || Fq > Fp ) break;
|
||||
if( Gq > eps_dd || Fq > Fp ) break;
|
||||
step *= 2; //increase step to "expand" min
|
||||
}
|
||||
|
||||
while(1){
|
||||
//min lay on [p,q]
|
||||
z = 3*( Fp - Fq )/step + Gp + Gq;
|
||||
// if( ( w = z*z - Gp*Gq ) < 0. ) w = 0.;
|
||||
if( ( w = z*z - Gp*Gq ) < -eps_dd ) w = 0.;
|
||||
w = sqrt( w );
|
||||
|
||||
//approximation of min
|
||||
r = step*( 1. - ( Gq + w - z )/( Gq - Gp + 2*w ) );
|
||||
|
||||
//calculating of new point
|
||||
for( i=0; i<n; i++ ) X[i] = P[i] + r*D[i];
|
||||
|
||||
//calculating of new quantity of function into point X
|
||||
Fr = (*function)( C1, C2, X );
|
||||
if( fabs(Fr) < eps_dd ) return (0);
|
||||
|
||||
//calculating of new paticular derivates of function
|
||||
G3 = (*derivativs)( C1, C2, X, G );
|
||||
|
||||
//calculating of gradient for point X
|
||||
for( Gr=0., i=0; i<n; i++ ) Gr += G[i]*D[i];
|
||||
|
||||
if( fabs( Gr ) < eps_dd ) break;
|
||||
if( ( Fr < Fp || fabs( Fp - Fr )<= eps_dd ) &&
|
||||
( Fr < Fq || fabs( Fq - Fr )<= eps_dd ) ) break;
|
||||
|
||||
// if( Fr <= Fp && Fr <= Fq ) break;
|
||||
|
||||
// if( Gr > 0. ){
|
||||
if( Gr > eps_dd ){ // choise span [p,r]
|
||||
step = r;
|
||||
for( i=0; i<n; i++ ) Q[i] = X[i];
|
||||
//??????
|
||||
Fq = Fr; Gq = Gr;
|
||||
break;
|
||||
}
|
||||
|
||||
//taking [r,q]
|
||||
step -= r;
|
||||
for( i=0; i<n; i++ ) P[i] = X[i];
|
||||
Fp = Fr; Gp = Gr;
|
||||
}
|
||||
|
||||
//changing of matrix H
|
||||
// H(i+1) = H(i) + A(i) + B(i)
|
||||
// A(i) = Vi*ViT / ( ViT*Ui)
|
||||
// B(i) = - Hi Ui UiT Hi / (UiT Hi Ui)
|
||||
|
||||
for( i=0; i<n ; i++ ){
|
||||
U[i] = G[i] - U[i];
|
||||
V[i] = X[i] - Y[i];
|
||||
}
|
||||
|
||||
for( kk=0, wk=0, dk=0, i=0; i<n; i++ ){
|
||||
for( M[i]=0, j=0; j<n; j++ ) M[i] += H[i][j]*U[j];
|
||||
kk += M[i]*U[i];
|
||||
wk += V[i]*U[i];
|
||||
dk += V[i]*V[i];
|
||||
}
|
||||
|
||||
if( kk != 0 && wk != 0 ) {
|
||||
kk=1./kk; wk=1./wk;
|
||||
for( i=0; i<n; i++ )
|
||||
for( j=0; j<n; j++ )
|
||||
H[i][j] = H[i][j] - M[i]*M[j]*kk + V[i]*V[j]*wk;
|
||||
}
|
||||
//control
|
||||
// if( sqrt(dk) < eps_dd || G3 < eps_dd ) break;
|
||||
if( dk < eps_nn || G3 < eps_d ) break;
|
||||
} // end of iteration loop
|
||||
min1 = ( Fr > Fp ) ? (Fp) : (Fr);
|
||||
min1 = ( min1 > Fq ) ? (Fq) : (min1);
|
||||
|
||||
return( sqrt(min1) );
|
||||
}
|
||||
|
||||
//---------------------------------------------------------->
|
||||
//================================SPLINE========================================
|
||||
/***********************************************************
|
||||
* functions for NURBScurve-to-point distance minimization
|
||||
*/
|
||||
sgFloat function_0( void *sply_dat, void *point, sgFloat X[] ){
|
||||
D_POINT p1, *p;
|
||||
|
||||
//point on NURBS curve
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &p1, 0 );
|
||||
|
||||
//initial point
|
||||
p=(D_POINT*)point;
|
||||
|
||||
//function to be minimized
|
||||
return( min_function( &p1, p ) );
|
||||
}
|
||||
|
||||
sgFloat derivativs_0( void *sply_dat, void *point, sgFloat X[], sgFloat G[] ){
|
||||
D_POINT p1, d1, *p;
|
||||
|
||||
//point and derivative on NURBS curve
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &p1, 0 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &d1, 1 );
|
||||
|
||||
p=(D_POINT*)point;
|
||||
|
||||
G[0] = min_derivativ( &p1, p, &d1 );
|
||||
return( sqrt_derivates( G, 1 ) );
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* functions for NURBScurve-to-line distance minimization
|
||||
*/
|
||||
sgFloat function_1( void *sply_dat, void *line, sgFloat X[] ){
|
||||
D_POINT p1, p2, *p;
|
||||
|
||||
//point on NURBS curve
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &p1, 0 );
|
||||
|
||||
//parametrical line is r=p1+(p2-p1)*lambda;
|
||||
p=(D_POINT*)line;
|
||||
|
||||
//point on line
|
||||
p2.x = p[0].x + ( p[1].x - p[0].x )*X[1];
|
||||
p2.y = p[0].y + ( p[1].y - p[0].y )*X[1];
|
||||
p2.z = p[0].z + ( p[1].z - p[0].z )*X[1];
|
||||
|
||||
//function to be minimized
|
||||
return( min_function( &p1, &p2 ) );
|
||||
}
|
||||
|
||||
sgFloat derivativs_1( void *sply_dat, void *line, sgFloat X[], sgFloat G[] ){
|
||||
D_POINT p1, p2, d1, d2, *p;
|
||||
|
||||
//point and derivativ on NURBS curve
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &p1, 0 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &d1, 1 );
|
||||
|
||||
//parametrical line is r=p+(p1-p)*lambda;
|
||||
p=(D_POINT*)line;
|
||||
|
||||
//point and dirivativ on line
|
||||
d2.x = p[1].x - p[0].x;
|
||||
d2.y = p[1].y - p[0].y;
|
||||
d2.z = p[1].z - p[0].z;
|
||||
|
||||
p2.x = p[0].x + d2.x*X[1];
|
||||
p2.y = p[0].y + d2.y*X[1];
|
||||
p2.z = p[0].z + d2.z*X[1];
|
||||
|
||||
G[0] = min_derivativ( &p1, &p2, &d1 );
|
||||
G[1] = -1.*min_derivativ( &p1, &p2, &d2 );
|
||||
return( sqrt_derivates( G, 2 ) );
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* functions for NURBScurve-to-plane distance minimization
|
||||
*/
|
||||
sgFloat function_2( void *sply_dat, void *plane, sgFloat X[] ){
|
||||
D_POINT p1, p2, *p;
|
||||
|
||||
//point on NURBS curve
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &p1, 0 );
|
||||
|
||||
//parametrical plane is r=p+(p1-p)*lambda+(p2-p)*nu;
|
||||
p=(D_POINT*)plane;
|
||||
|
||||
//point on plane
|
||||
p2.x = p[0].x + ( p[1].x - p[0].x )*X[1] + ( p[2].x - p[0].x )*X[2];
|
||||
p2.y = p[0].y + ( p[1].x - p[0].x )*X[1] + ( p[2].x - p[0].x )*X[2];
|
||||
p2.z = p[0].z + ( p[1].x - p[0].x )*X[1] + ( p[2].x - p[0].x )*X[2];
|
||||
|
||||
//function to be minimized
|
||||
return( min_function( &p1, &p2 ) );
|
||||
}
|
||||
|
||||
sgFloat derivativs_2( void *sply_dat, void *plane, sgFloat X[], sgFloat G[] ){
|
||||
D_POINT p1, p2, d1, d2, d3, *p;
|
||||
|
||||
//point and derivativ on NURBS curve
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &p1, 0 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat, X[0], &d1, 1 );
|
||||
|
||||
//parametrical plane is r=p+(p1-p)*lambda+(p2-p)*nu;
|
||||
p=(D_POINT*)plane;
|
||||
|
||||
//derivativ by first plane parameter
|
||||
d2.x = p[1].x - p[0].x;
|
||||
d2.y = p[1].y - p[0].y;
|
||||
d2.z = p[1].z - p[0].z;
|
||||
|
||||
//derivativ by second plane parameter
|
||||
d3.x = p[2].x - p[0].x;
|
||||
d3.y = p[2].y - p[0].y;
|
||||
d3.z = p[2].z - p[0].z;
|
||||
|
||||
//point on plane
|
||||
p2.x = p[0].x + d2.x*X[1] + d3.x*X[2];
|
||||
p2.y = p[0].y + d2.y*X[1] + d3.y*X[2];
|
||||
p2.z = p[0].z + d2.z*X[1] + d3.z*X[2];
|
||||
|
||||
G[0] = min_derivativ( &p1, &p2, &d1 );
|
||||
G[1] = -1.*min_derivativ( &p1, &p2, &d2 );
|
||||
G[2] = -1.*min_derivativ( &p1, &p2, &d3 );
|
||||
return( sqrt_derivates( G, 3 ) );
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* functions for NURBScurve-to-NURBScurve distance minimization
|
||||
*/
|
||||
sgFloat function_3( void *sply_dat1, void *sply_dat2, sgFloat X[] ){
|
||||
D_POINT p1, p2;
|
||||
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat1, X[0], &p1, 0 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat2, X[1], &p2, 0 );
|
||||
return( min_function( &p1, &p2 ) );
|
||||
}
|
||||
|
||||
sgFloat derivativs_3( void *sply_dat1, void *sply_dat2, sgFloat X[], sgFloat G[] ){
|
||||
D_POINT p1, p2, d1, d2;
|
||||
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat1, X[0], &p1, 0 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat1, X[0], &d1, 1 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat2, X[1], &p2, 0 );
|
||||
get_point_on_sply( (lpSPLY_DAT)sply_dat2, X[1], &d2, 1 );
|
||||
|
||||
G[0] = min_derivativ( &p1, &p2, &d1 );
|
||||
G[1] = -1.*min_derivativ( &p1, &p2, &d2 );
|
||||
return( sqrt_derivates( G, 2 ) );
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* function to be minimized
|
||||
*/
|
||||
static sgFloat min_function( lpD_POINT p1, lpD_POINT p2 ){
|
||||
return(( p1->x - p2->x )*( p1->x - p2->x )+
|
||||
( p1->y - p2->y )*( p1->y - p2->y )+
|
||||
( p1->z - p2->z )*( p1->z - p2->z ) );
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* derivatives
|
||||
*/
|
||||
static sgFloat min_derivativ( lpD_POINT p1, lpD_POINT p2, lpD_POINT d ){
|
||||
return( 2*( p1->x - p2->x )*d->x +
|
||||
2*( p1->y - p2->y )*d->y +
|
||||
2*( p1->z - p2->z )*d->z );
|
||||
}
|
||||
|
||||
static sgFloat sqrt_derivates( sgFloat *G, short n ){
|
||||
short i;
|
||||
sgFloat t=0;
|
||||
for( i=0; i<n; i++ ) t += G[i]*G[i];
|
||||
return ( sqrt(t) );
|
||||
}
|
||||
|
||||
//================================SURFACE=======================================
|
||||
/***********************************************************
|
||||
* functions for NURBSsurface-to-point distance minimization
|
||||
*/
|
||||
sgFloat function_0_0( void *srf_dat, void *point, sgFloat X[] ){
|
||||
D_POINT p1, *p;
|
||||
|
||||
//point on NURBS surface
|
||||
get_point_on_surface( (lpSURF_DAT)srf_dat, X[0], X[1], &p1 );
|
||||
|
||||
//initial point
|
||||
p=(D_POINT*)point;
|
||||
|
||||
//function to be minimized
|
||||
return( min_function( &p1, p ) );
|
||||
}
|
||||
|
||||
sgFloat derivativs_0_0( void *srf_dat, void *point, sgFloat X[], sgFloat G[] ){
|
||||
D_POINT p1, d1, d2, *p;
|
||||
|
||||
//point and derivative on NURBS surface
|
||||
get_point_on_surface( (lpSURF_DAT)srf_dat, X[0], X[1], &p1 );
|
||||
|
||||
get_deriv_on_surface( (lpSURF_DAT)srf_dat, X[0], X[1], &d1, 0 );
|
||||
get_deriv_on_surface( (lpSURF_DAT)srf_dat, X[0], X[1], &d2, 0 );
|
||||
|
||||
p=(D_POINT*)point;
|
||||
|
||||
G[0] = min_derivativ( &p1, p, &d1 );
|
||||
G[1] = min_derivativ( &p1, p, &d2 );
|
||||
|
||||
return( sqrt_derivates( G, 2 ) );
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
#include "../sg.h"
|
||||
|
||||
/**********************************************************
|
||||
* Free memory for matrix m x n
|
||||
*/
|
||||
void free_matrix( sgFloat **L, short n ){
|
||||
while( n > 0 ) SGFree( L[--n] );
|
||||
SGFree(L);
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Expand memory for matrix m x n
|
||||
* m - number of rows
|
||||
* n - number of colomns
|
||||
* return =**L - if memory expand; or = NULL
|
||||
*/
|
||||
sgFloat ** mem_matrix( short m, short n ){
|
||||
short i;
|
||||
sgFloat **L;
|
||||
|
||||
if( (L = (sgFloat**)SGMalloc( sizeof(sgFloat*) * m) ) == NULL) goto err;
|
||||
for( i = 0; i < m; i++ ) {
|
||||
if( (L[i] = (sgFloat*)SGMalloc(sizeof(sgFloat) * n) ) == NULL ){
|
||||
free_matrix( L, i );
|
||||
goto err;
|
||||
}
|
||||
}
|
||||
return L;
|
||||
err:
|
||||
nurbs_handler_err(SPL_NO_MEMORY);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
//-------------------------------------->>>>>>>>>>>>
|
||||
/**********************************************************
|
||||
* Free memory for matrix m x n
|
||||
*/
|
||||
void free_matrix_point( lpD_POINT *L, short n ){
|
||||
while( n > 0 ) SGFree( L[--n] );
|
||||
SGFree(L);
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Expand memory for matrix m x n
|
||||
* m - number of rows
|
||||
* n - number of colomns
|
||||
* return =**L - if memory expand; or = NULL
|
||||
*/
|
||||
lpD_POINT * mem_matrix_point( short m, short n ){
|
||||
short i;
|
||||
lpD_POINT *L;
|
||||
|
||||
if( (L = (D_POINT**)SGMalloc( sizeof(lpD_POINT) * m) ) == NULL) goto err;
|
||||
for( i = 0; i < m; i++ ) {
|
||||
if( (L[i] = (D_POINT*)SGMalloc(sizeof(D_POINT) * n) ) == NULL ){
|
||||
free_matrix_point( L, i );
|
||||
goto err;
|
||||
}
|
||||
}
|
||||
return L;
|
||||
err:
|
||||
nurbs_handler_err(SPL_NO_MEMORY);
|
||||
return NULL;
|
||||
}
|
||||
@@ -0,0 +1,425 @@
|
||||
#include "../sg.h"
|
||||
//spline must be APPR!!!!
|
||||
|
||||
//----------------------------------------------------------------->>>>>>>>>>>>>
|
||||
static void create_NURBS_line(lpSPLY_DAT sply, sgFloat *v1, sgFloat *v2);
|
||||
static BOOL create_NURBS_arc( lpSPLY_DAT sply_dat, sgFloat *center, sgFloat *Ort_X,
|
||||
sgFloat *Ort_Y, sgFloat r, sgFloat a_begin, sgFloat fi);
|
||||
static void create_NURBS_circle(lpSPLY_DAT sply, sgFloat *center, sgFloat *ort1,
|
||||
sgFloat *ort2, sgFloat r1, sgFloat r2);
|
||||
|
||||
static OSCAN_COD nrb_cre_scan(hOBJ hobj,lpSCAN_CONTROL lpsc);
|
||||
|
||||
static OSCAN_COD nrb_line (lpOBJ obj,lpSCAN_CONTROL lpsc);
|
||||
static OSCAN_COD nrb_circle (lpOBJ obj,lpSCAN_CONTROL lpsc);
|
||||
static OSCAN_COD nrb_arc (lpOBJ obj,lpSCAN_CONTROL lpsc);
|
||||
static OSCAN_COD nrb_spline (lpOBJ obj,lpSCAN_CONTROL lpsc);
|
||||
|
||||
typedef struct {
|
||||
SPLY_DAT sply_dat;
|
||||
BOOL first;
|
||||
} NRB_DAT; //
|
||||
typedef NRB_DAT * lpNRB_DAT;
|
||||
|
||||
static OSCAN_COD (**nrb_typeg)(lpOBJ obj,lpSCAN_CONTROL lpsc);
|
||||
/*******************************************************************************
|
||||
* ( OPATH, OLINE, OCIRCLE, OARC, OSPLINE)
|
||||
* NURBS
|
||||
*/
|
||||
BOOL transform_to_NURBS(hOBJ hobj, hOBJ *hobj_new){
|
||||
SCAN_CONTROL sc;
|
||||
BOOL rt=FALSE;
|
||||
OSTATUS status;
|
||||
// sgFloat coeff;
|
||||
lpOBJ obj, obj1;
|
||||
lpGEO_SPLINE gspline;
|
||||
NRB_DAT nrb_dat;
|
||||
|
||||
nrb_typeg = (OSCAN_COD(**)(lpOBJ obj,lpSCAN_CONTROL lpsc))GetMethodArray(OMT_TRANSTONURBS);
|
||||
|
||||
nrb_typeg[OLINE] = nrb_line; // OLINE
|
||||
nrb_typeg[OCIRCLE]= nrb_circle; // OCIRCLE
|
||||
nrb_typeg[OARC] = nrb_arc; // OARC
|
||||
nrb_typeg[OSPLINE]= nrb_spline; // OSPLINE
|
||||
|
||||
//----------------->>>>
|
||||
//create new hobj
|
||||
if((*hobj_new = o_alloc(OSPLINE)) == NULL) return FALSE;
|
||||
obj=(lpOBJ)(*hobj_new);
|
||||
|
||||
obj1=(lpOBJ)hobj;
|
||||
copy_obj_nongeo_par( obj1, obj);
|
||||
// set_obj_nongeo_par(obj);
|
||||
if (!get_status_path(hobj, &status)) return FALSE;
|
||||
|
||||
gspline = (lpGEO_SPLINE)(obj->geo_data);
|
||||
//----------------->>>>
|
||||
//initializate spline structure
|
||||
if( !init_sply_dat( SPL_NEW, 2, SPL_APPR, &nrb_dat.sply_dat )) return FALSE;
|
||||
|
||||
nrb_dat.first=TRUE;
|
||||
|
||||
init_scan(&sc);
|
||||
sc.user_geo_scan = nrb_cre_scan;
|
||||
reinterpret_cast<lpNRB_DAT &>(sc.data) = &nrb_dat;
|
||||
if(o_scan(hobj,&sc) == OSFALSE) goto err;
|
||||
|
||||
//reparametrization for all ribs
|
||||
if( nrb_dat.sply_dat.U[0]!=0. || nrb_dat.sply_dat.U[nrb_dat.sply_dat.numU-1]!=1.)
|
||||
if( !Reparametrization( nrb_dat.sply_dat.nump, nrb_dat.sply_dat.numU,
|
||||
0, 1, nrb_dat.sply_dat.degree, nrb_dat.sply_dat.U,
|
||||
nrb_dat.sply_dat.P, nrb_dat.sply_dat.u )) goto err;
|
||||
|
||||
if( nrb_dat.sply_dat.nump > 2 ) calculate_P(&nrb_dat.sply_dat, 0);
|
||||
|
||||
if( status & ST_CLOSE ) nrb_dat.sply_dat.sdtype = (SPLY_TYPE_IN)(nrb_dat.sply_dat.sdtype | SPL_CLOSE);
|
||||
//convert spline to geo spline
|
||||
if( !create_geo_sply(&nrb_dat.sply_dat, gspline)) goto err;
|
||||
|
||||
rt=TRUE;
|
||||
err:
|
||||
free_sply_dat(&nrb_dat.sply_dat);
|
||||
return rt;
|
||||
}
|
||||
|
||||
static OSCAN_COD nrb_cre_scan(hOBJ hobj,lpSCAN_CONTROL lpsc){
|
||||
lpOBJ obj;
|
||||
BOOL cod;
|
||||
|
||||
obj = (lpOBJ)hobj;
|
||||
cod = nrb_typeg[lpsc->type](obj,lpsc);
|
||||
return (OSCAN_COD)cod;
|
||||
}
|
||||
//----------------------------------------------------------------->>>>>>>>>>>>>
|
||||
/*******************************************************************************
|
||||
* Create new object, it's a line as NURBS
|
||||
*/
|
||||
#pragma argsused
|
||||
static OSCAN_COD nrb_line(lpOBJ obj, lpSCAN_CONTROL lpsc){
|
||||
lpNRB_DAT data = (lpNRB_DAT)(lpsc->data);
|
||||
lpGEO_LINE gline;
|
||||
SPLY_DAT sply_dat, sply_tmp;
|
||||
|
||||
/*typedef struct {
|
||||
D_POINT v1;
|
||||
D_POINT v2;
|
||||
}GEO_LINE;
|
||||
*/
|
||||
gline = (lpGEO_LINE)obj->geo_data;
|
||||
|
||||
if(data->first){
|
||||
create_NURBS_line(&data->sply_dat, (sgFloat*)&gline->v1, (sgFloat*)&gline->v2 );
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline( &data->sply_dat ) ) return OSFALSE;
|
||||
data->first=FALSE;
|
||||
}else{
|
||||
//initializate spline structure
|
||||
if( !init_sply_dat( SPL_NEW, 2, SPL_APPR, &sply_dat )) return (OSCAN_COD)FALSE;
|
||||
create_NURBS_line(&sply_dat, (sgFloat*)&gline->v1, (sgFloat*)&gline->v2 );
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline( &sply_dat ) ) goto err;
|
||||
if( !Two_Spline_Union_Dat(&sply_dat, &data->sply_dat, &sply_tmp)) goto err;
|
||||
static int ccc = 0;
|
||||
ccc++;
|
||||
free_sply_dat(&data->sply_dat);
|
||||
data->sply_dat=sply_tmp;
|
||||
free_sply_dat(&sply_dat);
|
||||
}
|
||||
return OSTRUE;
|
||||
err:
|
||||
free_sply_dat(&sply_dat);
|
||||
free_sply_dat(&sply_tmp);
|
||||
return OSFALSE;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Create line as NURBS
|
||||
*/
|
||||
static void create_NURBS_line(lpSPLY_DAT sply, sgFloat* v1, sgFloat* v2){
|
||||
short i;
|
||||
|
||||
// put knots into spline
|
||||
for( i=0; i<3; i++) {
|
||||
sply->U[i]=0.;
|
||||
sply->U[i+3]=1.;
|
||||
// put weigth into spline
|
||||
sply->P[i].vertex[3]=1.;
|
||||
}
|
||||
// put control points into spline
|
||||
for( i=0; i<3; i++ ){
|
||||
sply->P[0].vertex[i]=v1[i];
|
||||
sply->P[2].vertex[i]=v2[i];
|
||||
sply->P[1].vertex[i]=(sply->P[0].vertex[i]+sply->P[2].vertex[i])/2.;
|
||||
}
|
||||
|
||||
sply->numU=6;
|
||||
sply->nump=sply->numk=3;
|
||||
parameter_free_APPR( sply->nump-1, 2, sply->P, sply->u, NULL );
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------->>>>>>>>>>>>>
|
||||
/*******************************************************************************
|
||||
* Create new object, it's an arc as NURBS
|
||||
*/
|
||||
#pragma argsused
|
||||
static OSCAN_COD nrb_arc(lpOBJ obj, lpSCAN_CONTROL lpsc){
|
||||
lpNRB_DAT data = (lpNRB_DAT)(lpsc->data);
|
||||
lpGEO_ARC garc;
|
||||
MATR matr;
|
||||
D_POINT Ort_X, Ort_Y;
|
||||
SPLY_DAT sply_dat, sply_tmp;
|
||||
/*
|
||||
typedef struct {
|
||||
sgFloat r;
|
||||
D_POINT n; //
|
||||
D_POINT vc;
|
||||
D_POINT vb;
|
||||
D_POINT ve;
|
||||
sgFloat ab;
|
||||
sgFloat angle;
|
||||
}GEO_ARC;
|
||||
*/
|
||||
garc = (lpGEO_ARC)obj->geo_data;
|
||||
|
||||
//detect points on orts
|
||||
// first ort
|
||||
o_hcunit(matr);
|
||||
if( !o_rotate_xyz( matr, &garc->vc, &garc->n, -garc->ab ))return OSFALSE;
|
||||
o_hcncrd( matr, &garc->vb, &Ort_X);
|
||||
|
||||
dpoint_sub( &Ort_X, &garc->vc, &Ort_X );
|
||||
if( !dnormal_vector ( &Ort_X )) return OSFALSE;
|
||||
|
||||
// second ort
|
||||
o_hcunit(matr);
|
||||
if( !o_rotate_xyz( matr, &garc->vc, &garc->n, -garc->ab+M_PI/2. ))return OSFALSE;
|
||||
o_hcncrd( matr, &garc->vb, &Ort_Y);
|
||||
|
||||
dpoint_sub( &Ort_Y, &garc->vc, &Ort_Y );
|
||||
if( !dnormal_vector ( &Ort_Y )) return OSFALSE;
|
||||
|
||||
if(data->first){
|
||||
if( !create_NURBS_arc(&data->sply_dat, (sgFloat*)&garc->vc, (sgFloat*)&Ort_X,
|
||||
(sgFloat*)&Ort_Y, garc->r, garc->ab, garc->angle)) return OSFALSE;
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline( &data->sply_dat ) ) return OSFALSE;
|
||||
data->first=FALSE;
|
||||
}else{
|
||||
//initializate spline structure
|
||||
if( !init_sply_dat( SPL_NEW, 2, SPL_APPR, &sply_dat )) return (OSCAN_COD)FALSE;
|
||||
if( !create_NURBS_arc(&sply_dat, (sgFloat*)&garc->vc, (sgFloat*)&Ort_X,
|
||||
(sgFloat*)&Ort_Y, garc->r, garc->ab, garc->angle)) goto err0;
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline( &sply_dat ) ) goto err0;
|
||||
|
||||
if( !Two_Spline_Union_Dat(&sply_dat, &data->sply_dat, &sply_tmp)) goto err;
|
||||
free_sply_dat(&data->sply_dat);
|
||||
data->sply_dat=sply_tmp;
|
||||
free_sply_dat(&sply_dat);
|
||||
}
|
||||
return OSTRUE;
|
||||
err:
|
||||
free_sply_dat(&sply_tmp);
|
||||
err0:
|
||||
free_sply_dat(&sply_dat);
|
||||
return OSFALSE;
|
||||
}
|
||||
|
||||
static BOOL create_NURBS_arc( lpSPLY_DAT sply_dat, sgFloat* center, sgFloat* Ort_X,
|
||||
sgFloat* Ort_Y, sgFloat r, sgFloat a_begin, sgFloat fi){
|
||||
short i, ii, j, index, narc, n_p;
|
||||
sgFloat d_fi, w1, c, s, c1, s1, angle, f;
|
||||
DA_POINT P0, P1, P2, T0, T2;
|
||||
GEO_LINE gline1, gline2;
|
||||
|
||||
//detect number of part the arc consist of
|
||||
f=fabs(fi);
|
||||
if( ( f < M_PI/2. ) || ( fabs(f-M_PI/2.) < eps_n ) ) narc=1;
|
||||
else {
|
||||
if( ( f < M_PI ) || ( fabs(f-M_PI ) < eps_n ) ) narc=2;
|
||||
else {
|
||||
if( ( f < 3*M_PI/2. ) || ( fabs(f-3*M_PI/2.) < eps_n ) ) narc=3;
|
||||
else if( ( f < 2*M_PI ) || ( fabs(f-2*M_PI ) < eps_n ) ) narc=4;
|
||||
}
|
||||
}
|
||||
// create control points and put them into spline
|
||||
d_fi=fi/narc;
|
||||
|
||||
j=2*narc+1;
|
||||
sply_dat->nump=sply_dat->numk=j;
|
||||
w1=cos(d_fi/2.);
|
||||
|
||||
c=cos(a_begin); c1=r*c;
|
||||
s=sin(a_begin); s1=r*s;
|
||||
for( i=0; i<3; i++) P0[i]=center[i] + c1*Ort_X[i] + s1*Ort_Y[i];
|
||||
Create_4D_From_3D( &sply_dat->P[0], (sgFloat *)&P0, 1.);
|
||||
for( i=0; i<3; i++) T0[i] = P0[i] - s*Ort_X[i] + c*Ort_Y[i];
|
||||
|
||||
//create line on points
|
||||
memcpy( &gline1.v1, P0, sizeof(D_POINT));
|
||||
memcpy( &gline1.v2, T0, sizeof(D_POINT));
|
||||
|
||||
angle=a_begin;
|
||||
for(ii=1, index=0; ii<=narc; ii++, index+=2 ){
|
||||
angle += d_fi;
|
||||
if( angle > 2*M_PI || fabs( angle - 2*M_PI )< eps_n ) angle -=2*M_PI;
|
||||
|
||||
c=cos(angle); c1=r*c;
|
||||
s=sin(angle); s1=r*s;
|
||||
for( i=0; i<3; i++) P2[i] = center[i] + c1*Ort_X[i] + s1*Ort_Y[i];
|
||||
Create_4D_From_3D( &sply_dat->P[index+2], (sgFloat *)&P2, 1.);
|
||||
for( i=0; i<3; i++) T2[i] = P2[i] - s*Ort_X[i] + c*Ort_Y[i];
|
||||
|
||||
//create line on points
|
||||
memcpy( &gline2.v1, P2, sizeof(D_POINT));
|
||||
memcpy( &gline2.v2, T2, sizeof(D_POINT));
|
||||
|
||||
//detect intersection of the two lines
|
||||
if( !intersect_3d_ll(&gline1, 0, &gline2, 0, (lpD_POINT)&P1, &n_p) ) return FALSE;
|
||||
if( n_p==0 ) return FALSE;
|
||||
Create_4D_From_3D( &sply_dat->P[index+1], (sgFloat *)&P1, w1);
|
||||
|
||||
// P0=P2;
|
||||
// T0=T2;
|
||||
memcpy( P0, P2, sizeof(DA_POINT));
|
||||
memcpy( T0, T2, sizeof(DA_POINT));
|
||||
memcpy( &gline1.v1, P0, sizeof(D_POINT));
|
||||
memcpy( &gline1.v2, T0, sizeof(D_POINT));
|
||||
}
|
||||
|
||||
//create knots and put them into spline
|
||||
//nuber of knots = number_of_control_points+degree+1
|
||||
sply_dat->numU=j+2+1;
|
||||
for(i=0;i<3; i++){ sply_dat->U[i]=0.; sply_dat->U[i+j]=1.;}
|
||||
switch(narc){
|
||||
case 1:
|
||||
break;
|
||||
case 2:
|
||||
sply_dat->U[3] = sply_dat->U[4] = 0.5;
|
||||
break;
|
||||
case 3:
|
||||
sply_dat->U[3] = sply_dat->U[4] = 1./3.;
|
||||
sply_dat->U[5] = sply_dat->U[6] = 2./3.;
|
||||
break;
|
||||
case 4:
|
||||
sply_dat->U[3] = sply_dat->U[4] = 0.25;
|
||||
sply_dat->U[5] = sply_dat->U[6] = 0.5;
|
||||
sply_dat->U[7] = sply_dat->U[8] = 0.75;
|
||||
break;
|
||||
}
|
||||
parameter_free_APPR( sply_dat->nump-1, 2, sply_dat->P, sply_dat->u, NULL );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------->>>>>>>>>>>>>
|
||||
/*******************************************************************************
|
||||
* Create new object, it's a circle as NURBS
|
||||
*/
|
||||
#pragma argsused
|
||||
static OSCAN_COD nrb_circle(lpOBJ obj, lpSCAN_CONTROL lpsc){
|
||||
lpNRB_DAT data = (lpNRB_DAT)(lpsc->data);
|
||||
lpGEO_CIRCLE gcirc;
|
||||
D_POINT Ort_X, Ort_Y;
|
||||
sgFloat r1, r2;
|
||||
|
||||
gcirc = (lpGEO_CIRCLE)obj->geo_data;
|
||||
|
||||
//detect two points on circle
|
||||
init_ecs_arc((lpGEO_ARC)gcirc, ECS_CIRCLE);
|
||||
// first ort
|
||||
get_point_on_arc(0., &Ort_X);
|
||||
dpoint_sub( &Ort_X, &gcirc->vc, &Ort_X );
|
||||
if( !dnormal_vector ( &Ort_X )) return OSFALSE;
|
||||
//first radii
|
||||
r1=gcirc->r;
|
||||
|
||||
// second ort
|
||||
get_point_on_arc(1./4., &Ort_Y);
|
||||
dpoint_sub( &Ort_Y, &gcirc->vc, &Ort_Y );
|
||||
if( !dnormal_vector ( &Ort_Y )) return OSFALSE;
|
||||
// second radii
|
||||
r2=r1;
|
||||
|
||||
if(data->first){
|
||||
create_NURBS_circle( &data->sply_dat, (sgFloat*)&gcirc->vc, (sgFloat*)&Ort_X,
|
||||
(sgFloat*)&Ort_Y, r1, r2);
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline( &data->sply_dat ) ) return OSFALSE;
|
||||
data->first=FALSE;
|
||||
}
|
||||
return OSTRUE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* create nurbs curve for circle/ellipse
|
||||
*/
|
||||
static void create_NURBS_circle( lpSPLY_DAT sply, sgFloat *center, sgFloat *ort1,
|
||||
sgFloat * ort2, sgFloat r1, sgFloat r2){
|
||||
short i;
|
||||
|
||||
// put knots into spline
|
||||
for( i=0; i<3; i++ ) { sply->U[i]=0.; sply->U[i+7]=1.; }
|
||||
sply->U[3]=0.25;
|
||||
sply->U[4]=sply->U[5]=0.5;
|
||||
sply->U[6]=0.75;
|
||||
sply->numU=10;
|
||||
|
||||
// put weigth into spline
|
||||
for( i=0; i<3; i++ ) sply->P[3*i].vertex[3]=1.;
|
||||
sply->P[1].vertex[3]=sply->P[2].vertex[3]=sply->P[4].vertex[3]=sply->P[5].vertex[3]=0.5;
|
||||
|
||||
// put control points into spline
|
||||
for( i=0; i<3; i++ ) sply->P[0].vertex[i] = center[i]+r1*ort1[i];
|
||||
memcpy( &sply->P[6], &sply->P[0], sizeof(W_NODE) );
|
||||
|
||||
for( i=0; i<3; i++ ){
|
||||
sply->P[1].vertex[i] = (sply->P[0].vertex[i]+r2*ort2[i])/2.;
|
||||
sply->P[2].vertex[i] = (center[i]+r2*ort2[i]-r1*ort1[i])/2.;
|
||||
sply->P[3].vertex[i] = center[i]-r1*ort1[i];
|
||||
}
|
||||
for( i=0; i<3; i++ ){
|
||||
sply->P[4].vertex[i]= (sply->P[3].vertex[i]-r2*ort2[i])/2.;
|
||||
sply->P[5].vertex[i]= (center[i]-r2*ort2[i]+r1*ort1[i])/2.;
|
||||
}
|
||||
|
||||
sply->nump=sply->numk=7;
|
||||
parameter_free_APPR( sply->nump-1, 2, sply->P, sply->u, NULL );
|
||||
}
|
||||
//-------------------------------------------->>>>>>>>>>>>>
|
||||
|
||||
/**********************************************************
|
||||
* Create new object, it's a spline as NURBS
|
||||
*/
|
||||
#pragma argsused
|
||||
static OSCAN_COD nrb_spline(lpOBJ obj, lpSCAN_CONTROL lpsc){
|
||||
lpNRB_DAT data = (lpNRB_DAT)(lpsc->data);
|
||||
BOOL ret=OSFALSE;
|
||||
SPLY_DAT sply_dat, sply_tmp;
|
||||
lpGEO_SPLINE gspline;
|
||||
|
||||
gspline = (lpGEO_SPLINE)obj->geo_data;
|
||||
|
||||
if(data->first){
|
||||
// if( !begin_use_sply(gspline, &data->sply_dat)) return OSFALSE;
|
||||
if( !unpack_geo_sply( gspline, &data->sply_dat)) return OSFALSE;
|
||||
if( data->sply_dat.sdtype & SPL_INT ) if( !change_INT_APPR(&data->sply_dat) ) return OSFALSE;
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline(&data->sply_dat) ) return OSFALSE;
|
||||
// parameter_free_APPR( data->sply_dat.nump-1, 2, data->sply_dat.P, data->sply_dat.u, NULL );
|
||||
data->first=FALSE;
|
||||
return OSTRUE;
|
||||
}else{
|
||||
//initializate spline structure
|
||||
if( !begin_use_sply(gspline, &sply_dat)) return OSFALSE;
|
||||
if( sply_dat.sdtype & SPL_INT ) if( !change_INT_APPR(&sply_dat) ) goto err;
|
||||
if( lpsc->status & ST_DIRECT) if( !Reorient_Spline(&sply_dat) ) goto err;
|
||||
parameter_free_APPR( data->sply_dat.nump-1, 2, data->sply_dat.P, data->sply_dat.u, NULL );
|
||||
|
||||
if( !Two_Spline_Union_Dat(&sply_dat, &data->sply_dat, &sply_tmp)) goto err1;
|
||||
free_sply_dat(&data->sply_dat);
|
||||
data->sply_dat=sply_tmp;
|
||||
free_sply_dat(&sply_dat);
|
||||
}
|
||||
return OSTRUE;
|
||||
err1:
|
||||
free_sply_dat(&sply_tmp);
|
||||
err:
|
||||
free_sply_dat(&sply_dat);
|
||||
return (OSCAN_COD)ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,421 @@
|
||||
#include "../sg.h"
|
||||
static void calculate_free_U(short n_c, short p_c, sgFloat *u_c, sgFloat *U_c );
|
||||
//static sgFloat calculate_R_Sum( short n_c, lpW_NODE P, sgFloat *N);
|
||||
static void calc_w_point(lpSPLY_DAT sply_dat, lpD_POINT p, sgFloat *N);
|
||||
static void calc_w_deriv(lpSPLY_DAT sply_dat, short degree,
|
||||
lpD_POINT p, short deriv, sgFloat *N_i, sgFloat *N_j);
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining B-sply basis function of degree p
|
||||
* U_p - parametrical vector
|
||||
* u_p - parameter
|
||||
*/
|
||||
sgFloat nurbs_basis_func( short i, short p, sgFloat *U_p, sgFloat u_p ){
|
||||
sgFloat N, N1;
|
||||
if( p == 0 ){ // degree 0
|
||||
if( U_p[i] <= u_p && u_p < U_p[i+1] ) return(1);
|
||||
if( U_p[i] < U_p[i+1] && u_p == U_p[i+1] && u_p == 1 ) return(1);
|
||||
return(0);
|
||||
}
|
||||
// degree p
|
||||
N=0;
|
||||
if( ( N1 = U_p[i+p] - U_p[i] ) != 0. )
|
||||
N = ( u_p - U_p[i] )*nurbs_basis_func( i, p-1, U_p, u_p )/N1;
|
||||
|
||||
if( ( N1 = U_p[i+p+1] - U_p[i+1] ) != 0. )
|
||||
N = N + ( U_p[i+p+1] - u_p )*nurbs_basis_func( i+1, p-1, U_p, u_p )/N1;
|
||||
return( N );
|
||||
}
|
||||
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method
|
||||
* n_c - number of points
|
||||
* p_c - degree of curve
|
||||
* Q_tmp - curve to creation parametrical vector
|
||||
* U_c - parametrical vector
|
||||
*/
|
||||
void parameter( lpSPLY_DAT sply_dat, short condition, BOOL p ){
|
||||
short i;
|
||||
sgFloat *U=NULL;
|
||||
if( p ) U = sply_dat->U;
|
||||
if( sply_dat->sdtype & SPL_APPR ){
|
||||
parameter_free_APPR( sply_dat->nump-1, sply_dat->degree, sply_dat->P,
|
||||
sply_dat->u, U );
|
||||
if( p ) sply_dat->numU = sply_dat->nump+sply_dat->degree+1;
|
||||
}else{
|
||||
if( p ) sply_dat->numU = sply_dat->nump+sply_dat->degree+1;
|
||||
switch(condition){
|
||||
case 0://free NURBS
|
||||
parameter_free( sply_dat->numk-1, sply_dat->degree, sply_dat->knots,
|
||||
sply_dat->u, U );
|
||||
if( p ) sply_dat->numU = sply_dat->numk+sply_dat->degree+1;
|
||||
break;
|
||||
case 1://closed NURBS without derivates
|
||||
parameter_closed( sply_dat->numk-1, sply_dat->degree, sply_dat->knots,
|
||||
sply_dat->u, U );
|
||||
if( p ) sply_dat->numU = sply_dat->numk+sply_dat->degree+1+2;
|
||||
break;
|
||||
case 2://free NURBS with derivates
|
||||
parameter_deriv( sply_dat->numk-1, sply_dat->degree, sply_dat->knots,
|
||||
sply_dat->derivates, sply_dat->u, U );
|
||||
if( p ) for( i=0; i<sply_dat->numk; i++)
|
||||
if( sply_dat->derivates[i].num ) sply_dat->numU++;
|
||||
break;
|
||||
case 3://closed NURBS with derivates
|
||||
parameter_deriv_closed( sply_dat->numk-1, sply_dat->degree, sply_dat->knots,
|
||||
sply_dat->derivates, sply_dat->u, U );
|
||||
if( p ){
|
||||
sply_dat->numU = sply_dat->numk+sply_dat->degree+1+2;
|
||||
for( i=1; i<sply_dat->numk-1; i++)
|
||||
if( sply_dat->derivates[i].num ) sply_dat->numU++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method and
|
||||
* creating U-vector
|
||||
*/
|
||||
void parameter_free( short n_c, short p_c, lpDA_POINT Q_tmp,
|
||||
sgFloat *u_c, sgFloat *U_c ){
|
||||
short i;
|
||||
sgFloat S;
|
||||
|
||||
//decrease nurbs degree
|
||||
while( n_c + 1 < p_c + 1 ) p_c--;
|
||||
if( p_c <= 0 ) return;
|
||||
|
||||
//calculate parameters for points, begin and end - interval [0,1]
|
||||
u_c[0]=0.; u_c[n_c]=1.;
|
||||
|
||||
// compute sum
|
||||
for( S=0., i=1; i<=n_c; i++ )
|
||||
S += sqrt(dpoint_distance( (lpD_POINT)Q_tmp[i], (lpD_POINT)Q_tmp[i-1] ));
|
||||
S = 1./S;
|
||||
|
||||
//compute parameters
|
||||
for( i=1; i<n_c; i++ )
|
||||
u_c[i] = u_c[i-1] +
|
||||
sqrt(dpoint_distance( (lpD_POINT)Q_tmp[i], (lpD_POINT)Q_tmp[i-1] ))*S;
|
||||
|
||||
//calculate vector U for computation
|
||||
if( U_c != NULL ) calculate_free_U( n_c, p_c, u_c, U_c );
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method and
|
||||
* creating U-vector
|
||||
*/
|
||||
void parameter_free_APPR( short n_c, short p_c, lpW_NODE P, sgFloat *u_c, sgFloat *U_c ){
|
||||
short i;
|
||||
sgFloat S;
|
||||
|
||||
//decrease nurbs degree
|
||||
while( n_c + 1 < p_c + 1 ) p_c--;
|
||||
if( p_c <= 0 ) return;
|
||||
|
||||
//calculate parameters for points, begin and end - interval [0,1]
|
||||
u_c[0]=0.; u_c[n_c]=1.;
|
||||
|
||||
// compute sum
|
||||
for( S=0, i=1; i<=n_c; i++ ) S+=dpoint_distance_4dl( &P[i], &P[i-1] );
|
||||
S=1./S;
|
||||
|
||||
//compute parameters
|
||||
for( i=1; i<n_c; i++ )
|
||||
u_c[i] = u_c[i-1] + dpoint_distance_4dl( &P[i], &P[i-1] )*S;
|
||||
|
||||
//calculate vector U for computation
|
||||
if( U_c != NULL ) calculate_free_U( n_c, p_c, u_c, U_c );
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* calculate vector U
|
||||
*/
|
||||
static void calculate_free_U(short n_c, short p_c, sgFloat *u_c, sgFloat *U_c ){
|
||||
short i, j, k;
|
||||
sgFloat S, l;
|
||||
|
||||
//calculate vector U for computation
|
||||
l = 1./(sgFloat)p_c;
|
||||
for( i=0; i<=p_c; i++ ) U_c[i] = 0;
|
||||
for( i=p_c+1, j=1; j<=n_c-p_c; i++, j++ ) {
|
||||
for( S=0, k=j; k<=j+p_c-1; k++ ) S = S + u_c[k];
|
||||
U_c[i] = l*S;
|
||||
}
|
||||
for( i=n_c+1; i<=n_c+p_c+1; i++ ) U_c[i] = 1;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method for
|
||||
* closed curve
|
||||
*/
|
||||
void parameter_closed( short n_c, short p_c, lpDA_POINT Q_tmp,
|
||||
sgFloat *u_c, sgFloat *U_c ){
|
||||
short i;
|
||||
parameter_free( n_c, p_c, Q_tmp, u_c, U_c );
|
||||
if( U_c != NULL ){
|
||||
for( i=n_c+p_c+1; i>=p_c+1; i-- ) U_c[i+1] = U_c[i];
|
||||
U_c[p_c+1] /= 2;
|
||||
for( i=n_c+p_c+2; i>=n_c+2; i-- ) U_c[i+1] = U_c[i];
|
||||
U_c[n_c+2] = (U_c[n_c+1]+1)/2;
|
||||
}
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method for
|
||||
* free curve with derivates
|
||||
*/
|
||||
void parameter_deriv( short n_c, short p_c, lpDA_POINT Q_tmp, lpSNODE deriv,
|
||||
sgFloat *u_c, sgFloat *U_c ){
|
||||
short i, j, k, l, t;
|
||||
parameter_free( n_c, p_c, Q_tmp, u_c, U_c );
|
||||
if( U_c != NULL ){
|
||||
l=n_c+p_c+1;
|
||||
if(dpoint_distance((lpD_POINT)Q_tmp[0],
|
||||
(lpD_POINT)Q_tmp[n_c - 1])< eps_d ) t=n_c-1;
|
||||
else t=n_c;
|
||||
for( i=0; i<=t; i++)
|
||||
if( deriv[i].num ){
|
||||
for( j=0; j<l; j++ )
|
||||
if( ( U_c[j] <= u_c[i] && u_c[i] < U_c[j+1] ) ||
|
||||
( U_c[j] < U_c[j+1] && u_c[i] == U_c[j+1] && u_c[i] == 1 ) ){
|
||||
for( k=l; k>=j+1; k-- ) U_c[k+1] = U_c[k];
|
||||
U_c[j+1] = ( U_c[j+1] + U_c[j])/2;
|
||||
l++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method for
|
||||
* closed curve with derivates
|
||||
*/
|
||||
void parameter_deriv_closed( short n_c, short p_c, lpDA_POINT Q_tmp, lpSNODE deriv,
|
||||
sgFloat *u_c, sgFloat *U_c ){
|
||||
short i, j, k, l;
|
||||
parameter_free( n_c, p_c, Q_tmp, u_c, U_c );
|
||||
|
||||
if( U_c != NULL ){
|
||||
for( i=n_c+p_c+1; i>=p_c+1; i-- ) U_c[i+1] = U_c[i];
|
||||
U_c[p_c+1] /= 2;
|
||||
for( i=n_c+p_c+2; i>=n_c+2; i-- ) U_c[i+1] = U_c[i];
|
||||
U_c[n_c+2] = (U_c[n_c+1]+1)/2;
|
||||
|
||||
l=n_c+p_c+3;//¤« ¢¥ª® U
|
||||
for( i=1; i<=n_c-1; i++)
|
||||
if( deriv[i].num ){
|
||||
for( j=0; j<l; j++ )
|
||||
if( ( U_c[j] <= u_c[i] && u_c[i] < U_c[j+1] ) ||
|
||||
( U_c[j] < U_c[j+1] && u_c[i] == U_c[j+1] && u_c[i] == 1 ) ){
|
||||
for( k=l; k>=j+1; k-- ) U_c[k+1] = U_c[k];
|
||||
U_c[j+1] = ( U_c[j+1] + U_c[j])/2;
|
||||
l++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
/*******************************************************************************
|
||||
* Obtaining parameter values by centripetal method for
|
||||
* closed curve with derivates
|
||||
*/
|
||||
/*static sgFloat calculate_R_Sum( short n_c, lpW_NODE P, sgFloat *N){
|
||||
short j;
|
||||
sgFloat S=0;
|
||||
for( j=0; j<n_c; j++ ) if( N[j]>0 ) S += P[j].vertex[3]*N[j];
|
||||
return( S );
|
||||
} */
|
||||
|
||||
/**********************************************************
|
||||
* Obtaining the point for NURBS with weight
|
||||
* sply_dat - spline
|
||||
* t - point's parameter to calculate derivates
|
||||
* p - calculated point
|
||||
*/
|
||||
BOOL Calculate_Weight_Point(lpSPLY_DAT sply_dat, short degree, sgFloat t, lpD_POINT p){
|
||||
short i;
|
||||
sgFloat *N;
|
||||
if((N=(sgFloat*)SGMalloc(sply_dat->nump*sizeof(sgFloat)))==NULL) return FALSE;
|
||||
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
if( sply_dat->U[i] <= t && t <= sply_dat->U[i + degree + 1] )
|
||||
N[i] = nurbs_basis_func( i, degree, sply_dat->U, t );
|
||||
|
||||
calc_w_point(sply_dat, p, N );
|
||||
SGFree(N);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void calc_w_point(lpSPLY_DAT sply_dat, lpD_POINT p, sgFloat *N){
|
||||
short i, k;
|
||||
sgFloat w;
|
||||
W_NODE node;
|
||||
|
||||
memset( &node, 0, sizeof(W_NODE));
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
if( N[i] != 0 )
|
||||
for( k=0; k<4; k++ ) node.vertex[k] += sply_dat->P[i].vertex[k]*N[i];
|
||||
|
||||
Create_3D_From_4D( &node, (sgFloat *)p, &w);
|
||||
}
|
||||
|
||||
|
||||
/**********************************************************
|
||||
* Obtaining the first degree derivates for NURBS with weight
|
||||
* sply_dat - spline
|
||||
* t - point's parameter to calculate derivates
|
||||
* p - calculated derivates
|
||||
* deriv - degree of derivate
|
||||
*/
|
||||
BOOL Calculate_Weight_Deriv(lpSPLY_DAT sply_dat, short degree, sgFloat t,
|
||||
lpD_POINT p, short deriv){
|
||||
BOOL ret=FALSE;
|
||||
short i;
|
||||
sgFloat *N_j, *N_i;
|
||||
|
||||
if((N_j=(sgFloat*)SGMalloc(sply_dat->nump*sizeof(sgFloat)))==NULL) return FALSE;
|
||||
|
||||
if((N_i=(sgFloat*)SGMalloc(sply_dat->nump*sizeof(sgFloat)))==NULL) goto err;
|
||||
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
if( sply_dat->U[i] <= t && t <= sply_dat->U[i+degree+1] )
|
||||
N_j[i] = nurbs_basis_func( i, degree, sply_dat->U, t );
|
||||
|
||||
for( i=1; i<sply_dat->nump; i++ )
|
||||
if( sply_dat->U[i] <= t && t <= sply_dat->U[i + degree] )
|
||||
N_i[i] = nurbs_basis_func( i, degree-1, sply_dat->U, t );
|
||||
|
||||
calc_w_deriv( sply_dat, degree, p, deriv, N_i, N_j);
|
||||
ret=TRUE;
|
||||
err:
|
||||
if(N_j) SGFree(N_j);
|
||||
if(N_i) SGFree(N_i);
|
||||
return ret;
|
||||
}
|
||||
|
||||
#pragma argsused
|
||||
static void calc_w_deriv(lpSPLY_DAT sply_dat, short degree,
|
||||
lpD_POINT p, short deriv, sgFloat *N_i, sgFloat *N_j){
|
||||
short i, j, k;
|
||||
sgFloat feet, w_w, w_der, wi, wj, wi0;
|
||||
sgFloat *ppoint;
|
||||
DA_POINT point={0,0,0}, A={0,0,0}, pi, pj, pi0 ;
|
||||
|
||||
ppoint = (sgFloat*)p;
|
||||
//------------>>>>>>>>>>weight
|
||||
w_w=0;
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
if( N_j[i] != 0 ) w_w += sply_dat->P[i].vertex[3]*N_j[i];
|
||||
|
||||
//------------>>>>>>>>>>derivates of weight
|
||||
w_der=0;
|
||||
for( i=1; i<sply_dat->nump; i++ ){
|
||||
if( N_i[i] != 0 )
|
||||
if( (feet = sply_dat->U[i+degree] - sply_dat->U[i]) > eps_n )
|
||||
w_der += (sply_dat->P[i].vertex[3] - sply_dat->P[i-1].vertex[3])*N_i[i]/feet;
|
||||
}
|
||||
w_der *= degree;
|
||||
|
||||
//------------>>>>>>>>>>point
|
||||
calc_w_point(sply_dat, (lpD_POINT)&point, N_j);
|
||||
|
||||
//------------>>>>>>>>>> A`[u]
|
||||
for(j=0; j<sply_dat->nump; j++)
|
||||
if( N_j[j] !=0 ){
|
||||
Create_3D_From_4D( &sply_dat->P[j], (sgFloat *)pj, &wj);
|
||||
for(i=1; i<sply_dat->nump; i++)
|
||||
if( N_i[i] !=0 ){
|
||||
Create_3D_From_4D( &sply_dat->P[i-1], (sgFloat *)pi0, &wi0);
|
||||
Create_3D_From_4D( &sply_dat->P[i], (sgFloat *)pi, &wi);
|
||||
if( (feet = (sply_dat->U[i+degree] - sply_dat->U[i])) > eps_n ){
|
||||
feet=N_j[j]*N_i[i]/feet;
|
||||
for( k=0; k<3; k++)
|
||||
// A[k] += feet*(sply_dat->P[j].vertex[3]*(sply_dat->P[i].vertex[k]-
|
||||
// sply_dat->P[i-1].vertex[k])+
|
||||
// sply_dat->P[j].vertex[k]*(sply_dat->P[i].vertex[3]-
|
||||
// sply_dat->P[i-1].vertex[3]));
|
||||
A[k] += feet*(wj*(pi[k]-pi0[k])+pj[k]*(wi-wi0));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
for(i=0; i<3; i++) ppoint[i]=(degree*A[i]-w_der*point[i])/w_w;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Obtaining the point for NURBS without weight
|
||||
* sply_dat - spline
|
||||
* t - point's parameter to calculate derivates
|
||||
* p - calculated point
|
||||
*/
|
||||
void Calculate_Point(lpSPLY_DAT sply_dat, short degree, sgFloat t, lpD_POINT p){
|
||||
short i, j;
|
||||
sgFloat N_j, *point;
|
||||
|
||||
point = (sgFloat*)p;
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
if( sply_dat->U[i] <= t && t <= sply_dat->U[i+degree+1] )
|
||||
if( ( N_j = nurbs_basis_func( i, degree, sply_dat->U, t )) != 0 ){
|
||||
for(j=0; j<3; j++) point[j] += sply_dat->P[i].vertex[j]*N_j;
|
||||
}
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Obtaining the first degree derivates for NURBS with weight
|
||||
* sply_dat - spline
|
||||
* t - point's parameter to calculate derivates
|
||||
* p - calculated derivates
|
||||
* deriv - degree of derivate
|
||||
*/
|
||||
void Calculate_Deriv(lpSPLY_DAT sply_dat, short degree, sgFloat t,
|
||||
lpD_POINT p, short deriv){
|
||||
short i, j, k;
|
||||
sgFloat N_j, feet, feet1;
|
||||
sgFloat *point;
|
||||
|
||||
point = (sgFloat*)p;
|
||||
if( deriv==1){
|
||||
for( i=1; i<sply_dat->nump; i++ )
|
||||
if( sply_dat->U[i] <= t && t <= sply_dat->U[i + degree] )
|
||||
if( ( N_j = nurbs_basis_func( i, degree-1, sply_dat->U, t )) != 0 ){
|
||||
feet = sply_dat->U[i+degree] - sply_dat->U[i];
|
||||
if ( feet >= eps_n ){
|
||||
N_j /= feet;
|
||||
for( j=0; j<3; j++) point[j] += (sply_dat->P[i].vertex[j] -
|
||||
sply_dat->P[i-1].vertex[j])*N_j;
|
||||
}
|
||||
}
|
||||
for( j=0; j<3; j++) point[j] *= degree;
|
||||
}else{
|
||||
for( j=2; j<sply_dat->nump; j++ ){
|
||||
// if( sply_dat->U[j] <= t && t <= sply_dat->U[j+degree-1] ){
|
||||
i=j-1;
|
||||
if( ( N_j = nurbs_basis_func( j, degree-2, sply_dat->U, t )) != 0 ){
|
||||
if( (feet = sply_dat->U[i+degree] - sply_dat->U[i+1]) >= eps_n ){
|
||||
N_j /= feet;
|
||||
if( (feet=sply_dat->U[i+degree+1] - sply_dat->U[i+1]) >= eps_n )
|
||||
feet = 1./feet;
|
||||
else feet = 0.;
|
||||
if( (feet1=sply_dat->U[i+degree] - sply_dat->U[i]) >= eps_n )
|
||||
feet1 = 1./feet1;
|
||||
else feet1 = 0.;
|
||||
for( k=0; k<3; k++ )
|
||||
point[k] += ( sply_dat->P[j-2].vertex[k]*feet1 -
|
||||
sply_dat->P[j-1].vertex[k]*(feet + feet1) +
|
||||
sply_dat->P[j ].vertex[k]*feet )*N_j;
|
||||
}
|
||||
}
|
||||
// }
|
||||
}
|
||||
for(j=0; j<3; j++) point[j] *= degree*(degree-1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,129 @@
|
||||
#include "../sg.h"
|
||||
static void write_P_from_knots(lpSPLY_DAT sply_dat, short condition );
|
||||
|
||||
BOOL calculate_P(lpSPLY_DAT sply_dat, short condition){
|
||||
short p_c, k, i, nnn;
|
||||
sgFloat **A;
|
||||
|
||||
nnn = (sply_dat->sdtype & SPL_INT) ? sply_dat->numk : sply_dat->nump;
|
||||
k=nnn+sply_dat->numd;
|
||||
p_c = sply_dat->degree;
|
||||
//decrease nurbs degree
|
||||
while( nnn < p_c+1 ) p_c--;
|
||||
if( p_c <= 0 ){
|
||||
nurbs_handler_err(SPL_INTERNAL_ERROR);
|
||||
return FALSE;
|
||||
}
|
||||
if( sply_dat->sdtype & SPL_APPR ){ // approximative spline
|
||||
if( ( sply_dat->sdtype&SPL_GEO ) ){
|
||||
// if( sply_dat->nump > sply_dat->numk){
|
||||
// SGFree( sply_dat->knots );
|
||||
// if( (sply_dat->knots = SGMalloc(sply_dat->nump*sizeof(DA_POINT)))== NULL) return FALSE;
|
||||
// }
|
||||
}
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
get_point_on_sply( sply_dat, sply_dat->u[i],
|
||||
(lpD_POINT)&sply_dat->knots[i], 0);
|
||||
sply_dat->numk = sply_dat->nump;
|
||||
}else{ // interpolative spline
|
||||
if( (A = mem_matrix( k+1, k+1 )) == NULL) return FALSE;
|
||||
write_P_from_knots( sply_dat, condition ); //rigth part of equation
|
||||
nurbs_matrix_create( sply_dat, p_c, A, condition );
|
||||
|
||||
if( condition == 0 || condition == 2 ){
|
||||
//NURBS free or NURBS free with derivates
|
||||
if( !gauss( A, sply_dat->P, k-1) ) goto err;
|
||||
sply_dat->nump = k; //number of point in convex
|
||||
}else if( (condition == 1 || condition == 3) && sply_dat->degree > 1 ){
|
||||
// NURBS is closed with/without derivates
|
||||
// for closer consider c(1) = c(n), c`(1)=c`(n), c``(1)=c``(n),
|
||||
// and in matrix - first string c(1)=P(1);
|
||||
// second string c`(1)-c`(n)=0;
|
||||
// n+1-th string c``(1)-c``(n)=0;
|
||||
if( !gauss( A, sply_dat->P, k ) ) goto err;
|
||||
memcpy(&sply_dat->P[k+1], &sply_dat->P[0], sizeof(DA_POINT));
|
||||
sply_dat->nump = k + 2; //number of point in convex
|
||||
}
|
||||
free_matrix( A, k+1 );
|
||||
}
|
||||
return TRUE;
|
||||
err:
|
||||
free_matrix( A, k+1 );
|
||||
nurbs_handler_err(SPL_INTERNAL_ERROR);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Put right part of equation
|
||||
*/
|
||||
static void write_P_from_knots(lpSPLY_DAT sply_dat, short condition ){
|
||||
short i, j, k;
|
||||
|
||||
switch(condition){
|
||||
case 0://free
|
||||
for( i=0; i<sply_dat->numk; i++ ){
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[i].vertex[j]=sply_dat->knots[i][j];
|
||||
sply_dat->P[i].vertex[3]=1.;
|
||||
}
|
||||
break;
|
||||
case 1://closed
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[0].vertex[j]=sply_dat->knots[0][j];
|
||||
sply_dat->P[0].vertex[3]=1.;
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[1].vertex[j]=0.;
|
||||
sply_dat->P[1].vertex[3]=1.;
|
||||
|
||||
for( i=2; i<=sply_dat->numk-1; i++ ){
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[i].vertex[j]=sply_dat->knots[i-1][j];
|
||||
sply_dat->P[i].vertex[3]=1.;
|
||||
}
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[sply_dat->numk].vertex[j]=0.;
|
||||
sply_dat->P[sply_dat->numk].vertex[3]=1.;
|
||||
break;
|
||||
case 2: //free with derivates
|
||||
for( i=0, j=0; i<sply_dat->numk-1; i++, j++ ){
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->knots[i][k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
if( sply_dat->derivates[i].num ){
|
||||
j++;
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->derivates[i].p[k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
}
|
||||
}
|
||||
if( sply_dat->derivates[sply_dat->numk-1].num ){
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->derivates[sply_dat->numk-1].p[k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
j++;
|
||||
}
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->knots[sply_dat->numk-1][k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
break;
|
||||
case 3://closed with derivates
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[0].vertex[j]=sply_dat->knots[0][j];
|
||||
sply_dat->P[0].vertex[3]=1.;
|
||||
for( j=0; j<3 ;j++ ) sply_dat->P[1].vertex[j]=0.;
|
||||
sply_dat->P[1].vertex[3]=1.;
|
||||
j=2;
|
||||
if( sply_dat->derivates[0].num ){
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->derivates[0].p[k];
|
||||
sply_dat->P[j++].vertex[3]=1.;
|
||||
}
|
||||
|
||||
for( i=1; i<sply_dat->numk-1; i++, j++ ){
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->knots[i][k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
|
||||
if( sply_dat->derivates[i].num ){
|
||||
j++;
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->derivates[i].p[k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
}
|
||||
}
|
||||
if( sply_dat->derivates[sply_dat->numk-1].num ){
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->derivates[sply_dat->numk-1].p[k];
|
||||
sply_dat->P[j++].vertex[3]=1.;
|
||||
}
|
||||
for( k=0; k<3; k++ ) sply_dat->P[j].vertex[k]=sply_dat->knots[sply_dat->numk-1][k];
|
||||
sply_dat->P[j].vertex[3]=1.;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,583 @@
|
||||
#include "../sg.h"
|
||||
static BOOL Extract_Spline_Part(lpSPLY_DAT spline, sgFloat t1, sgFloat t2,
|
||||
sgFloat **U_new1, lpW_NODE *P_new1, sgFloat **u_new1,
|
||||
short *k1, short *k2);
|
||||
static void Create_Trim_Spline( sgFloat *U_new, lpW_NODE P_new, sgFloat *u_new,
|
||||
short k1, short k2, short p, lpSPLY_DAT spline );
|
||||
//------------------------------------------------------------------------->>>>>
|
||||
//----------->>>Create break point into NURBS Curves <<<-----------
|
||||
//------------------------------------------------------------------------->>>>>
|
||||
/*******************************************************************************
|
||||
* Insert nessesary knot into NURBS curve to create break point.
|
||||
* That points are marked to edit together
|
||||
* type - =0 - create multiply point to change curve form
|
||||
* =1 - create break point to trim curve
|
||||
* type >20 - insert knot with multiplicity (type-20)
|
||||
*/
|
||||
BOOL Create_Multy_Point( lpSPLY_DAT spline, sgFloat t1, short type ){
|
||||
BOOL rt=FALSE;
|
||||
short nump_new, k, r, s;
|
||||
sgFloat *U_new=NULL, *u_new=NULL;
|
||||
W_NODE *P_new=NULL;
|
||||
|
||||
//k - position the t1 into control vector
|
||||
//s - number of t1 into control vector
|
||||
//r - number of insertion of the parameter t1 to create break point
|
||||
if( !Detect_Multy_Insertion( spline->U, spline->degree, t1, spline->nump, &s, &k )) return FALSE;
|
||||
r=spline->degree-s;
|
||||
if( type == 1 ) r++;
|
||||
if( type > 20 ) r=type-20;
|
||||
if( r>0 ){ // need to insert knots
|
||||
if( !Insert_Knot( spline->U, spline->P, spline->u, spline->nump, spline->degree,
|
||||
t1, &U_new, &P_new, &u_new, &nump_new, r, k, s ) ) goto err;
|
||||
|
||||
// modify spline structure
|
||||
if( !Modify_Spline_Str( spline, U_new, nump_new+spline->degree+1,
|
||||
P_new, u_new, nump_new, spline->degree ) ) goto err;
|
||||
}
|
||||
rt=TRUE;
|
||||
err:
|
||||
if( P_new ) SGFree( P_new );
|
||||
if( U_new ) SGFree( U_new );
|
||||
if( u_new ) SGFree( u_new );
|
||||
return rt;
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
// ------------------>>>Get NURBS Curve Part<<<------------------
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* get part of NURBS curve from parameter t1 to t2 by inserting
|
||||
* multiplying knots, reparametrizat to [0,1] and covert to Geo
|
||||
*/
|
||||
BOOL Get_Part_Spline_Geo(lpSPLY_DAT spline, sgFloat t1, sgFloat t2,
|
||||
lpGEO_SPLINE geo_spline){
|
||||
BOOL rt=FALSE;
|
||||
short bound1, bound2;
|
||||
sgFloat *U_new1=NULL, *u_new1=NULL;
|
||||
W_NODE *P_new1=NULL;
|
||||
SPLY_DAT sply_dat;
|
||||
|
||||
//extract spline part by inserting knots
|
||||
//bound1 - the begin point of changing into control vector
|
||||
//bound2 - the end point of changing into control vector
|
||||
if( !Extract_Spline_Part(spline, t1, t2, &U_new1, &P_new1, &u_new1, &bound1, &bound2 ) ) goto err0;
|
||||
Create_Trim_Spline( U_new1, P_new1, u_new1, bound1, bound2, spline->degree, &sply_dat );
|
||||
//reparametrization to [0,1]
|
||||
if( !Reparametrization( sply_dat.nump, sply_dat.numU, 0, 1, spline->degree,
|
||||
sply_dat.U, sply_dat.P, sply_dat.u )) goto err;
|
||||
if( !create_geo_sply(&sply_dat, geo_spline )) goto err;
|
||||
rt=TRUE;
|
||||
err:
|
||||
free_sply_dat( &sply_dat );
|
||||
err0:
|
||||
if( P_new1 ) SGFree( P_new1 );
|
||||
if( U_new1 ) SGFree( U_new1 );
|
||||
if( u_new1 ) SGFree( u_new1 );
|
||||
return rt;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* get part of NURBS curve from parameter t1 to t2 by inserting
|
||||
* multiplying knots and create new sply_dat with param [t1 t2]
|
||||
*/
|
||||
BOOL Get_Part_Spline_Dat(lpSPLY_DAT spline, sgFloat t1, sgFloat t2,
|
||||
lpSPLY_DAT sply_dat){
|
||||
BOOL rt=FALSE;
|
||||
short bound1, bound2;
|
||||
sgFloat *U_new1=NULL, *u_new1=NULL;
|
||||
W_NODE *P_new1=NULL;
|
||||
|
||||
//extract spline part by inserting knots
|
||||
if( !Extract_Spline_Part(spline, t1, t2, &U_new1, &P_new1, &u_new1, &bound1, &bound2 ) ) goto err;
|
||||
//create new spline
|
||||
Create_Trim_Spline( U_new1, P_new1, u_new1, bound1, bound2, spline->degree, sply_dat );
|
||||
rt=TRUE;
|
||||
err:
|
||||
if( P_new1 ) SGFree( P_new1 );
|
||||
if( U_new1 ) SGFree( U_new1 );
|
||||
if( u_new1 ) SGFree( u_new1 );
|
||||
return rt;
|
||||
}
|
||||
|
||||
/*******************************************************************
|
||||
* Extract part of spline [t1, t2]
|
||||
*/
|
||||
static BOOL Extract_Spline_Part(lpSPLY_DAT spline, sgFloat t1, sgFloat t2,
|
||||
sgFloat **U_new1, lpW_NODE *P_new1, sgFloat **u_new1,
|
||||
short *bound1, short *bound2 ){
|
||||
BOOL rt=FALSE;
|
||||
short i, nump_new1, nump_new2, k, r, s;
|
||||
sgFloat *U_new2=NULL, *u_new2=NULL;
|
||||
W_NODE *P_new2=NULL;
|
||||
|
||||
//insert knots on the first end
|
||||
if( (*P_new1=(W_NODE *)SGMalloc(spline->nump*sizeof(W_NODE))) == NULL ) goto err;
|
||||
if( (*U_new1=(sgFloat *)SGMalloc(spline->numU*sizeof(sgFloat))) == NULL ) goto err;
|
||||
if( (*u_new1=(sgFloat *)SGMalloc(spline->nump*sizeof(sgFloat))) == NULL ) goto err;
|
||||
|
||||
//convert into 4D-space
|
||||
for( i=0; i<spline->nump; i++ ){
|
||||
memcpy( &((*P_new1)[i]), &spline->P[i], sizeof(W_NODE) );
|
||||
(*u_new1)[i]=spline->u[i];
|
||||
}
|
||||
for( i=0; i<spline->numU; i++ ) (*U_new1)[i]=spline->U[i];
|
||||
nump_new1=spline->nump;
|
||||
|
||||
//k - position the t1 into control vector
|
||||
//s - number of t1 into control vector
|
||||
//r - number of insertion of the parameter t1
|
||||
if( !Detect_Multy_Insertion( *U_new1, spline->degree, t1, nump_new1, &s, &k )) goto err;
|
||||
if( (r=spline->degree-s+1)>0 ){ // need to insert knots
|
||||
if( !Insert_Knot( *U_new1, *P_new1, *u_new1, nump_new1, spline->degree,
|
||||
t1, &U_new2, &P_new2, &u_new2, &nump_new2, r, k, s ) ) goto err;
|
||||
SGFree(*U_new1); *U_new1=U_new2; U_new2=NULL;
|
||||
SGFree(*u_new1); *u_new1=u_new2; u_new2=NULL;
|
||||
SGFree(*P_new1); *P_new1=P_new2; P_new2=NULL;
|
||||
nump_new1=nump_new2;
|
||||
// for left end only
|
||||
(*bound1)=k-s+1;
|
||||
} else (*bound1)=k;
|
||||
|
||||
//insert knots on the second end
|
||||
if( !Detect_Multy_Insertion( *U_new1, spline->degree, t2, nump_new1, &s, &k )) goto err;
|
||||
if( (r=spline->degree-s+1)>0 ){
|
||||
if( !Insert_Knot( *U_new1, *P_new1, *u_new1, nump_new1, spline->degree,
|
||||
t2, &U_new2, &P_new2, &u_new2, &nump_new2, r, k, s ) ) goto err;
|
||||
SGFree(*U_new1); *U_new1=U_new2; U_new2=NULL;
|
||||
SGFree(*u_new1); *u_new1=u_new2; u_new2=NULL;
|
||||
SGFree(*P_new1); *P_new1=P_new2; P_new2=NULL;
|
||||
// nump_new1=nump_new2;
|
||||
// for right end only
|
||||
(*bound2)=k+r;
|
||||
} else (*bound2)=k;
|
||||
if( s==(spline->degree+1) ) (*bound2)+=spline->degree;
|
||||
rt=TRUE;
|
||||
err:
|
||||
if( P_new2 ) SGFree( P_new2 );
|
||||
if( U_new2 ) SGFree( U_new2 );
|
||||
if( u_new2 ) SGFree( u_new2 );
|
||||
return rt;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* create new spline respect to part of intial spline [t1,t2]
|
||||
*/
|
||||
static void Create_Trim_Spline( sgFloat *U_new, lpW_NODE P_new, sgFloat *u_new,
|
||||
short k1, short k2, short p, lpSPLY_DAT sply_dat ){
|
||||
short i, j;
|
||||
|
||||
//initialization of new spline
|
||||
init_sply_dat(SPL_NEW, p, SPL_APPR, sply_dat );
|
||||
|
||||
//write points and weights
|
||||
sply_dat->nump = sply_dat->numk = k2 - p - k1;
|
||||
|
||||
if (sply_dat->allocSizeFor_P<sply_dat->nump &&
|
||||
sply_dat->P)
|
||||
{
|
||||
SGFree(sply_dat->P);
|
||||
sply_dat->P = (W_NODE*)SGMalloc(sply_dat->nump*sizeof(W_NODE));
|
||||
sply_dat->allocSizeFor_P = sply_dat->nump;
|
||||
}
|
||||
|
||||
if (sply_dat->allocSizeFor_u<sply_dat->nump &&
|
||||
sply_dat->u)
|
||||
{
|
||||
SGFree(sply_dat->u);
|
||||
sply_dat->u = (sgFloat*)SGMalloc(sply_dat->nump*sizeof(sgFloat));
|
||||
sply_dat->allocSizeFor_u = sply_dat->nump;
|
||||
}
|
||||
|
||||
for(i=k1, j=0; i<=k2-p-1; i++, j++)
|
||||
{
|
||||
if (j<sply_dat->allocSizeFor_P)
|
||||
memcpy( &sply_dat->P[j], &P_new[i], sizeof(W_NODE) );
|
||||
else
|
||||
assert(0);
|
||||
if (j<sply_dat->allocSizeFor_u)
|
||||
sply_dat->u[j]=u_new[i];
|
||||
else
|
||||
assert(0);
|
||||
}
|
||||
|
||||
// write vector
|
||||
sply_dat->numU = sply_dat->nump+p+1;
|
||||
|
||||
if (sply_dat->allocSizeFor_U<sply_dat->numU &&
|
||||
sply_dat->U)
|
||||
{
|
||||
SGFree(sply_dat->U);
|
||||
sply_dat->U = (sgFloat*)SGMalloc(sply_dat->numU*sizeof(sgFloat));
|
||||
sply_dat->allocSizeFor_U = sply_dat->numU;
|
||||
}
|
||||
|
||||
for(i=k1, j=0; i<=k2; i++, j++ )
|
||||
if (j<sply_dat->allocSizeFor_U)
|
||||
sply_dat->U[j]=U_new[i];
|
||||
else
|
||||
assert(0);
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* create new NURBS curve from two splines( sply_dat2+sply_dat1)
|
||||
* by merge knot's vectors and P-vectors and deleting of the
|
||||
* multiplying knots degree of the splines must be !!! equal
|
||||
*/
|
||||
BOOL Two_Spline_Union_Dat(lpSPLY_DAT sply_dat1, lpSPLY_DAT sply_dat2,
|
||||
lpSPLY_DAT sply_new){
|
||||
|
||||
|
||||
/* BOOL ret;
|
||||
short i, d, new_nump; //,r;
|
||||
sgFloat delta;
|
||||
sgFloat *U_new1, *u_new1;//, *U_new2=NULL;
|
||||
W_NODE *P_new1;//, *P_new2=NULL;
|
||||
|
||||
//write points and weights from first spline
|
||||
if( (P_new1=(W_NODE *)SGMalloc((sply_dat1->nump+sply_dat2->nump)*sizeof(W_NODE))) == NULL ) return FALSE;
|
||||
memcpy( P_new1, sply_dat2->P, sply_dat2->nump*sizeof(W_NODE) );
|
||||
//add second spline into new one
|
||||
for( i=0; i<sply_dat1->nump; i++ ) P_new1[sply_dat2->nump+i-1] = sply_dat1->P[i];
|
||||
|
||||
// write parameters
|
||||
if( (u_new1=(sgFloat *)SGMalloc((sply_dat1->nump+sply_dat2->nump)*sizeof(sgFloat))) == NULL ) goto err;
|
||||
memcpy( u_new1, sply_dat2->u, sply_dat2->nump*sizeof(sgFloat) );
|
||||
delta=sply_dat2->u[sply_dat2->nump-1];
|
||||
for( i=0; i<sply_dat1->nump; i++ ) u_new1[sply_dat2->nump+i-1] = sply_dat1->u[i]+delta;
|
||||
|
||||
// write vector
|
||||
d=sply_dat1->degree+1;
|
||||
if( (U_new1=(sgFloat *)SGMalloc((sply_dat1->numU+sply_dat2->numU)*sizeof(sgFloat))) == NULL ) goto err;
|
||||
memcpy( U_new1, sply_dat2->U, sply_dat2->numU*sizeof(sgFloat) );
|
||||
delta=sply_dat2->U[sply_dat2->numU-1];
|
||||
for(i=d; i<sply_dat1->numU; i++ ) U_new1[sply_dat2->numU+i-d-1]=sply_dat1->U[i]+delta;
|
||||
|
||||
new_nump=sply_dat1->nump+sply_dat2->nump-1;
|
||||
|
||||
//initialization of new spline
|
||||
init_sply_dat(SPL_NEW, sply_dat1->degree, SPL_APPR, sply_new );
|
||||
|
||||
sply_new->nump = sply_new->numk = new_nump;
|
||||
memcpy( sply_new->P, P_new1, new_nump*sizeof(W_NODE) );
|
||||
memcpy( sply_new->u, u_new1, new_nump*sizeof(sgFloat) );
|
||||
|
||||
sply_new->numU = new_nump+sply_dat1->degree+1;
|
||||
memcpy( sply_new->U, U_new1, sply_new->numU*sizeof(sgFloat) );
|
||||
// SGFree( U_new2 ); SGFree( P_new2 );
|
||||
ret=TRUE;
|
||||
err:
|
||||
if(U_new1) SGFree( U_new1 );
|
||||
if(u_new1) SGFree( u_new1 );
|
||||
if(P_new1) SGFree( P_new1 );
|
||||
return ret;
|
||||
|
||||
*/
|
||||
|
||||
|
||||
BOOL ret=FALSE;
|
||||
short i, d, new_nump; //,r;
|
||||
sgFloat delta;
|
||||
sgFloat *U_new1, *u_new1;
|
||||
W_NODE *P_new1;
|
||||
|
||||
P_new1 = NULL;
|
||||
u_new1 = NULL;
|
||||
U_new1 = NULL;
|
||||
|
||||
//write points and weights from first spline
|
||||
if( (P_new1=(W_NODE *)SGMalloc((sply_dat1->nump+sply_dat2->nump)*sizeof(W_NODE))) == NULL ) return FALSE;
|
||||
memcpy( P_new1, sply_dat2->P, sply_dat2->nump*sizeof(W_NODE) );
|
||||
//add second spline into new one
|
||||
for( i=0; i<sply_dat1->nump; i++ )
|
||||
P_new1[sply_dat2->nump+i-1] = sply_dat1->P[i];
|
||||
|
||||
// write parameters
|
||||
if( (u_new1=(sgFloat *)SGMalloc((sply_dat1->nump+sply_dat2->nump)*sizeof(sgFloat))) == NULL ) goto err;
|
||||
memcpy( u_new1, sply_dat2->u, sply_dat2->nump*sizeof(sgFloat) );
|
||||
delta=sply_dat2->u[sply_dat2->nump-1];
|
||||
for( i=0; i<sply_dat1->nump; i++ ) u_new1[sply_dat2->nump+i-1] = sply_dat1->u[i]+delta;
|
||||
|
||||
// write vector
|
||||
d=sply_dat1->degree+1;
|
||||
if( (U_new1=(sgFloat *)SGMalloc((sply_dat1->numU+sply_dat2->numU)*sizeof(sgFloat))) == NULL ) goto err;
|
||||
memcpy( U_new1, sply_dat2->U, sply_dat2->numU*sizeof(sgFloat) );
|
||||
delta=sply_dat2->U[sply_dat2->numU-1];
|
||||
for(i=d; i<sply_dat1->numU; i++ ) U_new1[sply_dat2->numU+i-d-1]=sply_dat1->U[i]+delta;
|
||||
//try to delete multiply points
|
||||
new_nump=sply_dat1->nump+sply_dat2->nump-1;
|
||||
// r=Del_Knot( U_new1, P_new1, &new_nump,
|
||||
// sply_dat1->numu+sply_dat2->numu-d-1, sply_dat1->degree,
|
||||
// 1., d);
|
||||
|
||||
//initialization of new spline
|
||||
init_sply_dat(SPL_NEW, sply_dat1->degree, SPL_APPR, sply_new );
|
||||
|
||||
sply_new->nump = sply_new->numk = new_nump;
|
||||
// RA -
|
||||
// ( init_sply_dat 250 )
|
||||
// . ?
|
||||
// RA - memcpy( sply_new->P, P_new1, new_nump*sizeof(W_NODE) );
|
||||
// RA - memcpy( sply_new->u, u_new1, new_nump*sizeof(sgFloat) );
|
||||
|
||||
//RA Begin
|
||||
if (sply_new->allocSizeFor_knots<sply_new->numk &&
|
||||
sply_new->knots)
|
||||
{
|
||||
SGFree(sply_new->knots);
|
||||
sply_new->knots = (DA_POINT*)SGMalloc(sply_new->numk*sizeof(DA_POINT));
|
||||
sply_new->allocSizeFor_knots = sply_new->numk;
|
||||
}
|
||||
memset(sply_new->knots,0,sply_new->allocSizeFor_knots*sizeof(DA_POINT));
|
||||
|
||||
if (sply_new->allocSizeFor_P<new_nump &&
|
||||
sply_new->P)
|
||||
{
|
||||
SGFree(sply_new->P);
|
||||
sply_new->P = (W_NODE*)SGMalloc(new_nump*sizeof(W_NODE));
|
||||
sply_new->allocSizeFor_P = new_nump;
|
||||
}
|
||||
memcpy( sply_new->P, P_new1, new_nump*sizeof(W_NODE) );
|
||||
|
||||
if (sply_new->allocSizeFor_u<new_nump &&
|
||||
sply_new->u)
|
||||
{
|
||||
SGFree(sply_new->u);
|
||||
sply_new->u = (sgFloat*)SGMalloc(new_nump*sizeof(sgFloat));
|
||||
sply_new->allocSizeFor_u = new_nump;
|
||||
}
|
||||
memcpy( sply_new->u, u_new1, new_nump*sizeof(sgFloat) );
|
||||
//RA End
|
||||
|
||||
sply_new->numU = new_nump+sply_dat1->degree+1;
|
||||
// RA - memcpy( sply_new->U, U_new1, sply_new->numu*sizeof(sgFloat) );
|
||||
|
||||
//RA Begin
|
||||
if (sply_new->allocSizeFor_U<sply_new->numU &&
|
||||
sply_new->U)
|
||||
{
|
||||
SGFree(sply_new->U);
|
||||
sply_new->U = (sgFloat*)SGMalloc(sply_new->numU*sizeof(sgFloat));
|
||||
sply_new->allocSizeFor_U = sply_new->numU;
|
||||
}
|
||||
memcpy( sply_new->U, U_new1, sply_new->numU*sizeof(sgFloat) );
|
||||
//RA End
|
||||
|
||||
ret=TRUE;
|
||||
|
||||
err:
|
||||
if(U_new1) SGFree( U_new1 );
|
||||
if(u_new1) SGFree( u_new1 );
|
||||
if(P_new1) SGFree( P_new1 );
|
||||
return ret;
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
// ----------------->>>Increase NURBS Degree<<<------------------
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* Increase spline degree
|
||||
*/
|
||||
BOOL Increase_Spline_Degree(lpSPLY_DAT sply_dat, short new_degree){
|
||||
BOOL rt=FALSE;
|
||||
short i, j, k, l, m, r, s, k1, add, m_del;
|
||||
short nump_new1, nump_new2, degree_plus, *r_insert=NULL;
|
||||
sgFloat *U_new1, *u_new1, *U_new2=NULL, *u_new2=NULL, *par_insert=NULL;
|
||||
W_NODE *P_new1, *P_new2=NULL;
|
||||
|
||||
add=new_degree-sply_dat->degree;
|
||||
degree_plus=sply_dat->degree+1;
|
||||
|
||||
if( (P_new1=(W_NODE *)SGMalloc(sply_dat->nump*sizeof(W_NODE))) == NULL) return FALSE;
|
||||
if( (u_new1=(sgFloat *)SGMalloc(sply_dat->nump*sizeof(sgFloat))) == NULL) goto err;
|
||||
for( i=0; i<sply_dat->nump; i++ ){
|
||||
memcpy( &P_new1[i], &sply_dat->P[i], sizeof(W_NODE) );
|
||||
u_new1[i]=sply_dat->u[i];
|
||||
}
|
||||
|
||||
if( sply_dat->nump==degree_plus ){ //It's Beze Curve already!
|
||||
//increase degree for composite curve
|
||||
nump_new2=0;
|
||||
Increase_Composite_Curve_Degree( P_new1, &P_new2, u_new1, &u_new2, degree_plus,
|
||||
&nump_new2, sply_dat->degree, add);
|
||||
//create new control knots vector
|
||||
if( (U_new1=(sgFloat *)SGMalloc((nump_new2+new_degree+1)*sizeof(sgFloat)))==NULL) goto err;
|
||||
for(i=0; i<new_degree+1; i++) { U_new1[i]=0.; U_new1[i+new_degree+1]=1.;}
|
||||
k=2*(new_degree+1);
|
||||
}else{ // It's not Beze Curve - create composite curve from initial curve
|
||||
U_new1=(sgFloat *)SGMalloc(sply_dat->numU*sizeof(sgFloat));
|
||||
for( i=0; i<sply_dat->numU; i++ ) U_new1[i]=sply_dat->U[i];
|
||||
nump_new1=sply_dat->nump;
|
||||
|
||||
m=0;
|
||||
if((r_insert =(short *)SGMalloc((sply_dat->nump-degree_plus)*sizeof(short)))==NULL) goto err;
|
||||
if((par_insert=(sgFloat *)SGMalloc((sply_dat->nump-degree_plus)*sizeof(sgFloat)))==NULL) goto err1;
|
||||
|
||||
//insert knots on the each parameter
|
||||
for(i=degree_plus, j=1; j<=sply_dat->nump-degree_plus; i++, j++ ) {
|
||||
//detect r=number of the insertions for j-th knot
|
||||
if( !Detect_Multy_Insertion( U_new1, sply_dat->degree, sply_dat->U[i],
|
||||
nump_new1, &s, &k1)) goto err1;
|
||||
if( (r=sply_dat->degree-s+1)>0 ){ //if this node must be insert to create Beze Curve
|
||||
r_insert[m]=r;
|
||||
par_insert[m++]=sply_dat->U[i];
|
||||
|
||||
if( !Insert_Knot( U_new1, P_new1, u_new1, nump_new1, sply_dat->degree,
|
||||
sply_dat->U[i], &U_new2, &P_new2, &u_new2, &nump_new2, r, k1, s ) ) goto err;
|
||||
|
||||
SGFree(U_new1); U_new1=U_new2; U_new2=NULL;
|
||||
SGFree(u_new1); u_new1=u_new2; u_new2=NULL;
|
||||
SGFree(P_new1); P_new1=P_new2; P_new2=NULL;
|
||||
nump_new1=nump_new2; nump_new2=0;
|
||||
}
|
||||
}
|
||||
//increase degree for composite curve
|
||||
Increase_Composite_Curve_Degree(P_new1, &P_new2, u_new1, &u_new2, nump_new1,
|
||||
&nump_new2, sply_dat->degree, add);
|
||||
k=0;
|
||||
SGFree(U_new1);
|
||||
//create new control knots vector
|
||||
if( (U_new1=(sgFloat *)SGMalloc((nump_new2+new_degree+1)*sizeof(sgFloat)))==NULL) goto err1;
|
||||
|
||||
for(i=0; i<new_degree+1; i++) U_new1[k++]=0.;
|
||||
for( j=1; j<=sply_dat->nump-degree_plus; j+=s ) {
|
||||
//detect multiplisity
|
||||
for( s=0, l=1; l<=sply_dat->nump-degree_plus; l++ )
|
||||
if( fabs(sply_dat->U[j+sply_dat->degree]-sply_dat->U[l+sply_dat->degree]) < eps_n ) s++;
|
||||
for( l=0; l<new_degree+1; l++ ) U_new1[k++]=sply_dat->U[j+sply_dat->degree];
|
||||
}
|
||||
for(i=0; i<new_degree+1; i++) U_new1[k++]=1.;
|
||||
|
||||
//deleting if the inserted knots
|
||||
m_del=0;
|
||||
for( i=0; i<m; i++ )
|
||||
m_del+=Del_Knot( U_new1, P_new2, u_new2, &nump_new2, nump_new2+new_degree+1,
|
||||
new_degree, par_insert[i], r_insert[i]);
|
||||
|
||||
k-=m_del;
|
||||
SGFree( par_insert ); par_insert=NULL;
|
||||
SGFree( r_insert ); r_insert=NULL;
|
||||
}
|
||||
|
||||
//modify spline structure for increase degree
|
||||
if( !Modify_Spline_Str( sply_dat, U_new1, k, P_new2, u_new2, nump_new2, new_degree ) ) goto err;
|
||||
rt=TRUE;
|
||||
|
||||
err1:
|
||||
if( par_insert ) SGFree( par_insert ); if( r_insert ) SGFree( r_insert );
|
||||
err:
|
||||
if( P_new1 ) SGFree( P_new1 ); if( P_new2 ) SGFree( P_new2 );
|
||||
if( U_new1 ) SGFree( U_new1 ); if( U_new2 ) SGFree( U_new2 );
|
||||
if( u_new1 ) SGFree( u_new1 ); if( u_new2 ) SGFree( u_new2 );
|
||||
return rt;
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
// ------------->>>NURBS Curve reparametrization<<<------------------
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* [ab] - initial interval i.e [ab]->[cd]
|
||||
* [cd] - rezalting interval
|
||||
* p - degree
|
||||
* U - control parametrical vector
|
||||
*/
|
||||
BOOL Reparametrization( short nump, short numu, sgFloat c, sgFloat d, short p,
|
||||
sgFloat *U, lpW_NODE P, sgFloat *u ){
|
||||
short i, j;
|
||||
sgFloat w0, wn, Gamma, Alfa, S, t1, t2, a, b;
|
||||
a=U[0];
|
||||
b=U[numu-1];
|
||||
w0=pow((double)P[0].vertex[3], (double)1./p);
|
||||
wn=pow((double)P[nump-1].vertex[3], (double)1./p);
|
||||
for( i=0; i<numu; i++ ){
|
||||
t1=w0*(b-U[i]); t2=wn*(U[i]-a);
|
||||
if( (Alfa =( t1 + t2 ) ) < eps_n ) return FALSE;
|
||||
U[i] = ( t1*c + t2*d )/Alfa;
|
||||
}
|
||||
|
||||
a=u[0];
|
||||
b=u[nump-1];
|
||||
for( i=0; i<nump; i++ ){
|
||||
t1=w0*(b-u[i]); t2=wn*(u[i]-a);
|
||||
if( (Alfa =( t1 + t2 ) ) < eps_n ) return FALSE;
|
||||
u[i] = ( t1*c + t2*d )/Alfa;
|
||||
}
|
||||
// Gamma = wn-w0;
|
||||
// Alfa = wn*d-w0*c;
|
||||
Gamma = 0;
|
||||
Alfa = d-c;
|
||||
for( i=0; i<nump; i++ ){
|
||||
for( S=1., j=1; j<=p; j++ ) S = S*( Gamma*U[i+j] - Alfa );
|
||||
P[i].vertex[3] *= S;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
// ------------->>>NURBS Curve reorientation<<<------------------
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* reorientation of spline
|
||||
*/
|
||||
BOOL Reorient_Spline( lpSPLY_DAT sply_dat ){
|
||||
short ii;
|
||||
sgFloat *U;
|
||||
lpW_NODE P_tmp;
|
||||
|
||||
if( (P_tmp=(W_NODE *)SGMalloc( sply_dat->nump*sizeof(W_NODE)))==NULL) return FALSE;
|
||||
memcpy(P_tmp, sply_dat->P, sply_dat->nump*sizeof(W_NODE));
|
||||
for( ii=0; ii<sply_dat->nump; ii++)
|
||||
memcpy( &sply_dat->P[ii], &P_tmp[sply_dat->nump-1-ii], sizeof(W_NODE));
|
||||
SGFree(P_tmp);
|
||||
|
||||
if( (U=(sgFloat *)SGMalloc( sply_dat->numU*sizeof(sgFloat)))==NULL) goto err;
|
||||
memcpy(U, sply_dat->U, sply_dat->numU*sizeof(sgFloat));
|
||||
for( ii=0; ii<sply_dat->numU; ii++){
|
||||
U[ii]-=1.;
|
||||
sply_dat->U[sply_dat->numU-1-ii]=fabs(U[ii]);
|
||||
}
|
||||
memcpy(U, sply_dat->u, sply_dat->nump*sizeof(sgFloat));
|
||||
for( ii=0; ii<sply_dat->nump; ii++){
|
||||
U[ii]-=1.;
|
||||
sply_dat->u[sply_dat->nump-1-ii]=fabs(U[ii]);
|
||||
}
|
||||
SGFree(U);
|
||||
return TRUE;
|
||||
err:
|
||||
SGFree(P_tmp);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
// ------------->>>NURBS Modify work structure <<<------------------
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* Modify NURBS by new control points and vector
|
||||
*/
|
||||
BOOL Modify_Spline_Str( lpSPLY_DAT sply_dat, sgFloat *U_new, short numu,
|
||||
lpW_NODE P_new, sgFloat *u_new, short nump, short degree ){
|
||||
//write control vector
|
||||
if( numu>MAX_POINT_ON_SPLINE ){
|
||||
SGFree( sply_dat->U );
|
||||
if( (sply_dat->U=(sgFloat *)SGMalloc(numu*sizeof(sgFloat)))==NULL ) return FALSE;
|
||||
}
|
||||
memcpy( sply_dat->U, U_new, numu*sizeof(sgFloat) );
|
||||
sply_dat->numU=numu;
|
||||
|
||||
//write control points
|
||||
if( nump>MAX_POINT_ON_SPLINE ){
|
||||
SGFree( sply_dat->P );
|
||||
SGFree( sply_dat->knots );
|
||||
SGFree( sply_dat->u );
|
||||
if( (sply_dat->P=(W_NODE *)SGMalloc(nump*sizeof(W_NODE)))==NULL) return FALSE;
|
||||
if( (sply_dat->knots=(DA_POINT*)SGMalloc(nump*sizeof(DA_POINT)))==NULL) return FALSE;
|
||||
if( (sply_dat->u=(sgFloat *)SGMalloc(nump*sizeof(sgFloat)))==NULL ) return FALSE;
|
||||
}
|
||||
memcpy( sply_dat->P, P_new, nump*sizeof(W_NODE) );
|
||||
memcpy( sply_dat->u, u_new, nump*sizeof(sgFloat) );
|
||||
sply_dat->nump=nump;
|
||||
|
||||
sply_dat->degree = degree;
|
||||
calculate_P(sply_dat, 0);
|
||||
return TRUE;
|
||||
}
|
||||
@@ -0,0 +1,143 @@
|
||||
#include "../sg.h"
|
||||
/**********************************************************
|
||||
* obtain NURBScurve-to-point distance
|
||||
* spl - NURBS curve
|
||||
* point - point for curve-to-point distance
|
||||
* t - point or point projection lie on curve with parameter t
|
||||
* dist - distance between point and its projection
|
||||
*/
|
||||
BOOL Spline_By_Point( lpSPLY_DAT spl, void *point, sgFloat *t, sgFloat *dist ){
|
||||
sgFloat X[1];
|
||||
D_POINT node;
|
||||
|
||||
X[0]=*t;
|
||||
if( (*dist = Fletchjer_Paur( function_0, derivativs_0,
|
||||
(void *)spl, (void *)point,
|
||||
X, 1, 50 )) == -1 ) return FALSE;
|
||||
*t=X[0];
|
||||
// if( fabs(*dist) > eps_d ){ // point out of curve
|
||||
if( X[0] < -eps_n ) *t=0;
|
||||
if( X[0] > 1.+eps_n ) *t=1.;
|
||||
get_point_on_sply( spl, *t, &node, 0 );
|
||||
*dist= dpoint_distance( (D_POINT *)point, &node );
|
||||
// }
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* obtain NURBScurve-to-plane distance
|
||||
* spl - NURBS curve
|
||||
* plane - points on plane
|
||||
* t - point lie on curve with parameter t
|
||||
* dist - distance between point and its projection
|
||||
*/
|
||||
BOOL Spline_By_Plane( lpSPLY_DAT spl, void *plane,
|
||||
sgFloat *t, sgFloat *bound, sgFloat *dist ){
|
||||
sgFloat X[3];
|
||||
|
||||
//found parameter for the begining point on curve
|
||||
X[0]=*t;
|
||||
//found parameter for the begining point on plane
|
||||
//the plane parametrization is made trought beginning point =>
|
||||
X[1]=X[2]=0.;
|
||||
|
||||
// if(!Spline_By_Point( spl, &point[0], &X[0], dist )) return FALSE;
|
||||
|
||||
if( (*dist = Fletchjer_Paur( function_2, derivativs_2,
|
||||
(void *)spl, (void *)plane,
|
||||
X, 3, 400 )) == -1 ) return FALSE;
|
||||
|
||||
if( ( bound[0] <= X[0] && X[0] < bound[1] ) ||
|
||||
( X[0] == 1. && bound[1] == 1. )) *t=X[0];
|
||||
else *t=-1.;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* obtain NURBS curve-to-line distance
|
||||
* spl - NURBS curve
|
||||
* line - points on line
|
||||
* t - point or point projection lie on curve with parameter t
|
||||
* dist - distance between point and its projection
|
||||
* span - number of the interval intersection found on
|
||||
*/
|
||||
BOOL Spline_By_Line( lpSPLY_DAT spl, void *line,
|
||||
sgFloat *t_sply, sgFloat *t_line,
|
||||
sgFloat *bound, sgFloat *dist ){
|
||||
sgFloat X[2];
|
||||
D_POINT *point;
|
||||
|
||||
//found parameter for the begining point on curve and line
|
||||
X[0]=*t_sply;
|
||||
X[1]=*t_line;
|
||||
point=(D_POINT*)line;
|
||||
|
||||
if( (*dist = Fletchjer_Paur( function_1, derivativs_1,
|
||||
(void *)spl, (void *)point,
|
||||
X, 2, 400 )) == -1 ) return FALSE;
|
||||
|
||||
if( ( bound[0] <= X[0] && X[0] < bound[1] ) ||
|
||||
( X[0] == 1. && bound[1] == 1. )) *t_sply=X[0];
|
||||
else *t_sply=-1.;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* obtain sply-by-sply intersections
|
||||
* sply_dat1 - data for first spline
|
||||
* sply_dat2 - data for second spline
|
||||
* intersect_list - list of intersection points
|
||||
*/
|
||||
BOOL Spline_By_Spline( lpSPLY_DAT spl1, void *spl2,
|
||||
sgFloat *t1, sgFloat *t2,
|
||||
sgFloat *bound1, sgFloat *bound2,
|
||||
sgFloat *dist ){
|
||||
sgFloat X[2];
|
||||
|
||||
//found parameter for the begining point on the first curve
|
||||
X[0]=*t1;
|
||||
X[1]=*t2;
|
||||
|
||||
if( (*dist = Fletchjer_Paur( function_3, derivativs_3,
|
||||
(void *)spl1, (void *)spl2,
|
||||
X, 2, 400 )) == -1 ) return FALSE;
|
||||
|
||||
if( ( bound1[0] <= X[0] && X[0] < bound1[1] ) ||
|
||||
( X[0] == 1. && bound1[1] == 1. )) *t1=X[0];
|
||||
// else *t1=-1.;
|
||||
if( ( bound2[0] <= X[1] && X[1] < bound2[1] ) ||
|
||||
( X[1] == 1. && bound2[1] == 1. )) *t2=X[1];
|
||||
// else *t2=-1.;
|
||||
|
||||
// *t1=X[0]; *t2=X[1];
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* obtain NURBSsurface-to-point distance
|
||||
* srf - NURBS surface
|
||||
* point - point for curve-to-point distance
|
||||
* tu, tv- point or point projection lie on surface with parameter (tu,tv)
|
||||
* dist - distance between point and its projection
|
||||
*/
|
||||
BOOL Surface_By_Point( lpSURF_DAT srf, void *point, sgFloat *tu, sgFloat *tv, sgFloat *dist ){
|
||||
sgFloat X[2];
|
||||
D_POINT node;
|
||||
|
||||
X[0]=*tu;
|
||||
X[1]=*tv;
|
||||
if( (*dist = Fletchjer_Paur( function_0_0, derivativs_0_0,
|
||||
(void *)srf, (void *)point,
|
||||
X, 2, 50 )) == -1 ) return FALSE;
|
||||
*tu=X[0];
|
||||
*tv=X[1];
|
||||
// if( fabs(*dist) > eps_d ){ // point out of curve
|
||||
if( X[0] < -eps_n ) *tu=0;
|
||||
if( X[0] > 1.+eps_n ) *tu=1.;
|
||||
if( X[1] < -eps_n ) *tv=0;
|
||||
if( X[1] > 1.+eps_n ) *tv=1.;
|
||||
if( !get_point_on_surface( srf, *tu, *tv, &node ) ) return FALSE;
|
||||
*dist= dpoint_distance( (D_POINT *)point, &node );
|
||||
// }
|
||||
return TRUE;
|
||||
}
|
||||
@@ -0,0 +1,357 @@
|
||||
#include "../sg.h"
|
||||
static BOOL NURBS_Knot_Ins( short nump, short p, sgFloat *U, lpW_NODE P, sgFloat *u, sgFloat t,
|
||||
short k, short s, short r, sgFloat *U_new, lpW_NODE P_new, sgFloat *u_new);
|
||||
/*static short NURBS_Knot_Del( sgFloat *U, lpW_NODE P, short *nump, short d, sgFloat t,
|
||||
short k, short s, short r);
|
||||
// sgFloat *U_new, lpW_NODE P_new, short *nump_new,
|
||||
*/
|
||||
static void Increase_Beze_Degree(lpW_NODE beze1, sgFloat *u1, lpW_NODE beze2, sgFloat *u2,
|
||||
short degree, short add );
|
||||
static sgFloat c_beze(short k1, short k2);
|
||||
static short factor(short k);
|
||||
|
||||
/*******************************************************************************
|
||||
* Main function for multy inserting knot
|
||||
* U - initial control vector
|
||||
* P - initial control points
|
||||
* nump -number of control points
|
||||
* p - degree of spline
|
||||
* t - parameter of the inserting knot
|
||||
* U_new - new control vector
|
||||
* P_new - new control points
|
||||
* nump_new - number of new control points
|
||||
* r - number of insertions to create break point
|
||||
* k - number of span into control vector for new knot
|
||||
* s - number of existing inserting knot
|
||||
* type - =0 - insert full break point
|
||||
* =1 - insert knot r times
|
||||
*/
|
||||
BOOL Insert_Knot( sgFloat *U, lpW_NODE P, sgFloat *u, short nump, short p, sgFloat t,
|
||||
sgFloat **U_new, lpW_NODE *P_new, sgFloat **u_new, short *nump_new,
|
||||
short r, short k, short s ){
|
||||
short num;
|
||||
|
||||
num = nump + p + 1;
|
||||
|
||||
//number of inserting points = p-s+r-1;
|
||||
*nump_new=nump+r;
|
||||
if( (*U_new = (sgFloat*)SGMalloc( ( num + r )*sizeof(sgFloat) ) ) == NULL ) return FALSE;
|
||||
if( (*P_new = (W_NODE *)SGMalloc( ( *nump_new )*sizeof(W_NODE) ) ) == NULL ) return FALSE;
|
||||
if( (*u_new = (sgFloat*)SGMalloc( ( *nump_new )*sizeof(sgFloat) ) ) == NULL ) return FALSE;
|
||||
|
||||
//insert nessesary number of knots
|
||||
NURBS_Knot_Ins( nump, p, U, P, u, t, k, s, r, *U_new, *P_new, *u_new );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* multiplicity insert ( r times ) knot t
|
||||
* nump - length of P
|
||||
* d - degree
|
||||
* U - char. vector
|
||||
* P - points of the char. poligon
|
||||
* t - knot to be insert
|
||||
* k - number of knot to be insert into vector U
|
||||
* s - multiplicity of existing knot t into vector U ( may be =0 )
|
||||
* r - multiplicity of insertinon
|
||||
* U_new, P_New - new vector and poligon
|
||||
* type - =0 - insert full break point
|
||||
* =1 - insert knot r times
|
||||
*/
|
||||
static BOOL NURBS_Knot_Ins( short nump, short d, sgFloat *U, lpW_NODE P, sgFloat *u,
|
||||
sgFloat t, short k, short s, short r,
|
||||
sgFloat *U_new, lpW_NODE P_new, sgFloat *u_new){
|
||||
BOOL rt=FALSE;
|
||||
short i, j, mp, jjj, jj, L;//, number_t;
|
||||
sgFloat alpha, *uu;
|
||||
lpW_NODE PP;
|
||||
|
||||
if( (PP = (lpW_NODE)SGMalloc((d+1)*sizeof(W_NODE))) == NULL ) return FALSE;
|
||||
if( (uu = (sgFloat*)SGMalloc((d+1)*sizeof(sgFloat))) == NULL ) goto err;
|
||||
mp = nump + d + 1;
|
||||
|
||||
//create new knot vector
|
||||
for( i=0; i<=k; i++ ) U_new[i] = U[i];
|
||||
for( i=1; i<=r; i++ ) U_new[k+i] = t;
|
||||
for( i=k+1; i<mp; i++ ) U_new[i+r] = U[i];
|
||||
|
||||
//save knot points
|
||||
for( i=0; i<=k-d; i++ ) {P_new[i] = P[i]; u_new[i] = u[i];}
|
||||
for( i=k-s; i<nump; i++ ) {P_new[i+r] = P[i]; u_new[i+r] = u[i];}
|
||||
for( i=0; i<=d-s; i++ ) {PP[i] = P[k-d+i]; uu[i] = u[k-d+i];}
|
||||
|
||||
L=k-d;
|
||||
jjj=(r+s==d+1)?(r):(r+1);
|
||||
// jjj=r;
|
||||
//insert knor r times
|
||||
for( j=1; j<jjj; j++ ){
|
||||
L = k-d+j;
|
||||
for( i=0; i<=d-j-s; i++ ){
|
||||
alpha = (t-U[L+i])/(U[i+k+1]-U[L+i]);
|
||||
for( jj=0; jj<4; jj++ )
|
||||
PP[i].vertex[jj]=alpha*PP[i+1].vertex[jj] + (1.-alpha)*PP[i].vertex[jj];
|
||||
uu[i]=alpha*uu[i+1] + (1.-alpha)*uu[i];
|
||||
}
|
||||
P_new[L] = PP[0];
|
||||
if( j==jjj-1 ) u_new[L] = t;
|
||||
else u_new[L] = uu[0];
|
||||
P_new[k+r-j-s] = PP[d-j-s]; u_new[k+r-j-s] = uu[d-j-s];
|
||||
}
|
||||
|
||||
for( i=L+1; i<k-s; i++ ) { P_new[i] = PP[i-L]; u_new[i] = uu[i-L];}
|
||||
|
||||
// L=-1;
|
||||
// for( i=0; i<mp+r; i++ )
|
||||
// if( fabs(U_new[i]-t)<eps_n && fabs(U_new[i]-U_new[i+1])<eps_n){
|
||||
// L=i;
|
||||
// break;
|
||||
// }
|
||||
// if( L>0 ) u_new[L-1]=t;
|
||||
// for( i=0; i<nump+d-s; i++ ) u_new[i]=(U_new[i+1]+U_new[i+2])/2.;
|
||||
|
||||
rt=TRUE;
|
||||
SGFree(uu);
|
||||
err:
|
||||
SGFree(PP);
|
||||
return rt;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------->>>>
|
||||
/*******************************************************************************
|
||||
* multiplicity delete ( r times ) knot t
|
||||
* nump - length of P
|
||||
* d - degree
|
||||
* U - char. vector
|
||||
* P - points of the char. poligon
|
||||
* t - knot to be delete
|
||||
* k - number of knot to be delete into vector U
|
||||
* s - multiplicity of existing knot t into vector U ( may be =0 )
|
||||
* r - multiplicity of deleting
|
||||
* U_new, P_New - new vector and poligon
|
||||
*/
|
||||
short Del_Knot( sgFloat *U, lpW_NODE P, sgFloat *u, short *nump, short numu, short d,
|
||||
sgFloat t, short r ){
|
||||
short i, i1, j, j1, k, s, del=0;
|
||||
short iii;
|
||||
sgFloat alphai, alphaj;
|
||||
W_NODE node;
|
||||
|
||||
for( j1=0; j1<r; j1++ ){
|
||||
//detect number of knot to be deleted
|
||||
s=0; k=-1;
|
||||
for( i=0; i<numu; i++ ) if( fabs(t-U[i])<eps_n ) { k=i; s++; }
|
||||
if( k<0 ) return del;
|
||||
|
||||
//try to delete knot with number <k> r times
|
||||
// return(NURBS_Knot_Del( U, P, nump, d, t, k, s, r));
|
||||
i=k-d; j=k-s;
|
||||
while(j-i>0){
|
||||
alphai=(t-U[i])/(U[i+d+1]-U[i]);
|
||||
alphaj=(t-U[j])/(U[j+d+1]-U[j]);
|
||||
for( iii=0; iii<4; iii++ ){
|
||||
P[i].vertex[iii]=(P[i].vertex[iii]-(1-alphai)*P[i-1].vertex[iii])/alphai;
|
||||
P[j].vertex[iii]=(P[j].vertex[iii]-alphaj*P[j+1].vertex[iii])/(1-alphaj);
|
||||
}
|
||||
u[i]=(u[i]-(1-alphai)*u[i-1])/alphai;
|
||||
u[j]=(u[j]-alphaj*u[j+1])/(1-alphaj);
|
||||
i++; j--;
|
||||
}
|
||||
if( dpoint_distance_4d(&P[i-1], &P[j+1])<eps_d ){
|
||||
for( i1=i+1; i1<*nump; i1++ ){
|
||||
memcpy(&P[i1-1], &P[i1], sizeof(W_NODE));
|
||||
u[i1-1]=u[i1];
|
||||
}
|
||||
(*nump)--;
|
||||
for( i1=k; i1<numu-1; i1++ ) U[i1]=U[i1+1];
|
||||
numu--;
|
||||
del++;
|
||||
}else{
|
||||
alphai=(t-U[i])/(U[i+d+1]-U[i]);
|
||||
for( iii=0; iii<4; iii++ )
|
||||
node.vertex[iii]=alphai*P[i+1].vertex[iii]+(1-alphai)*P[i-1].vertex[iii];
|
||||
if( dpoint_distance_4d(&P[i], &node)<eps_d ){
|
||||
for( i1=i+1; i1<*nump; i1++ ){
|
||||
memcpy(&P[i1-1], &P[i1], sizeof(W_NODE));
|
||||
u[i1-1]=u[i1];
|
||||
}
|
||||
(*nump)--;
|
||||
for( i1=k; i1<numu; i1++ ) U[i1-1]=U[i1];
|
||||
numu--;
|
||||
del++;
|
||||
}
|
||||
}
|
||||
// if( del < j1+1 ) return 0;
|
||||
}
|
||||
return del;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* multiplicity delete ( r times ) knot t
|
||||
* nump - length of P
|
||||
* d - degree
|
||||
* U - char. vector
|
||||
* P - points of the char. poligon
|
||||
* t - knot to be delete
|
||||
* k - number of knot to be delete into vector U
|
||||
* s - multiplicity of existing knot t into vector U ( may be =0 )
|
||||
* r - multiplicity of deleting
|
||||
* U_new, P_New - new vector and poligon
|
||||
* nump_new - length of P_new
|
||||
*/
|
||||
/*static short NURBS_Knot_Del( sgFloat *U, lpW_NODE P, short *nump, short d, sgFloat t,
|
||||
short k, short s, short r){
|
||||
short i, j, l, m, n=0, iii;
|
||||
short first, last;
|
||||
sgFloat alphai, alphaj;
|
||||
W_NODE *point, p;
|
||||
|
||||
if( (point = SGMalloc((*nump)*sizeof(W_NODE))) == NULL ) return 0;
|
||||
for( i=0; i<*nump; i++ ) point[i]=P[i];
|
||||
|
||||
// memset(point, 0, nump*sizeof(W_NODE));
|
||||
|
||||
first=k-d+1;
|
||||
last =k-s-1;
|
||||
// if (s==d+1) last++;
|
||||
|
||||
for( l=1; l<=r; l++ ){
|
||||
j=++last;
|
||||
i=--first;
|
||||
while(j-i>l-1){
|
||||
alphai=(t-U[i])/(U[i+d+l]-U[i]);
|
||||
alphaj=(t-U[j-l+1])/(U[j+d+1]-U[j-l+1]);
|
||||
for( iii=0; iii<4; iii++ ){
|
||||
point[i].vertex[iii]=(point[i].vertex[iii]-(1-alphai)*point[i-1].vertex[iii])/alphai;
|
||||
point[j].vertex[iii]=(point[j].vertex[iii]-alphaj*point[j+1].vertex[iii])/(1-alphaj);
|
||||
}
|
||||
i++; j--;
|
||||
}
|
||||
if(dpoint_distance_4dl(&point[i-1], &point[j+1])<eps_d){
|
||||
//delete point and knot
|
||||
for (m=i-1; m<*nump-1; m++) point[m]=point[m+1];
|
||||
for (m=k; m<*nump+d+1-1; m++) U[m]=U[m+1];
|
||||
//k--; s--; r--;
|
||||
(*nump)--; n++;
|
||||
}else{
|
||||
alphai=(t-U[i])/(U[i+d+1+l]-U[i]);
|
||||
for( iii=0; iii<4; iii++ ) p.vertex[iii]=alphai*point[i+l+1].vertex[iii]+
|
||||
(1-alphai)*point[i-1].vertex[iii];
|
||||
if(fabs(dpoint_distance_4dl(&P[i], &p))<=eps_d){
|
||||
//delete point and knot
|
||||
for (m=i-1; m<*nump-1; m++) point[m]=point[m+1];
|
||||
for (m=k; m<*nump+d+1-1; m++) U[m]=U[m+1];
|
||||
//k--; s--; r--;
|
||||
(*nump)--; n++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if(n==0) goto err; // imposible to delete this knot
|
||||
|
||||
// modify control vector
|
||||
for( i=0; i<*nump; i++) P[i]=point[i];
|
||||
//err:
|
||||
SGFree(point);
|
||||
return (n);
|
||||
}
|
||||
*/
|
||||
|
||||
//------------------------------------------------------------------------>>>>>>
|
||||
/*******************************************************************************
|
||||
* create the Beze from the composite curve
|
||||
* P_new1 - initial control points
|
||||
* P_new2 - rezulting control points
|
||||
* span - number of span
|
||||
* degree - initial degree
|
||||
* add - increase degree on add
|
||||
*/
|
||||
BOOL Increase_Composite_Curve_Degree(lpW_NODE P_new1, lpW_NODE *P_new2,
|
||||
sgFloat *u_new1, sgFloat **u_new2,
|
||||
short nump_new1, short *nump_new2,
|
||||
short degree, short add){
|
||||
short j, k, j1, j2, nump;
|
||||
sgFloat *u1=NULL, *u2=NULL;
|
||||
lpW_NODE beze1/*=NULL*/, beze2=NULL;
|
||||
|
||||
if( (*P_new2 = (W_NODE *)SGMalloc( ( 2*nump_new1)*sizeof(W_NODE) ) ) == NULL ) return FALSE;
|
||||
if( (*u_new2 = (sgFloat*)SGMalloc( ( 2*nump_new1)*sizeof(sgFloat) ) ) == NULL ) return FALSE;
|
||||
|
||||
j1=j2=0;
|
||||
while( j2<nump_new1-1 ){
|
||||
for( j=j1+1; j<nump_new1-1; j++)
|
||||
if( (dpoint_distance_4d( &P_new1[j], &P_new1[j+1] )) < eps_d ) break;
|
||||
j2=j;
|
||||
nump=j2-j1+1;
|
||||
|
||||
if( (beze1 = (W_NODE *)SGMalloc( nump*sizeof(W_NODE) ) ) == NULL ) goto err;
|
||||
if( (u1 = (sgFloat*)SGMalloc( nump*sizeof(sgFloat) ) ) == NULL ) goto err;
|
||||
if( (beze2 = (W_NODE *)SGMalloc( ( nump+add)*sizeof(W_NODE) ) ) == NULL ) goto err;
|
||||
if( (u2 = (sgFloat*)SGMalloc( (nump+add)*sizeof(sgFloat) ) ) == NULL ) goto err;
|
||||
|
||||
for( j=j1, k=0; j<=j2; j++, k++ ) { beze1[k]=P_new1[j]; u1[k]=u_new1[j]; }
|
||||
|
||||
//increase degree of the Beze segment
|
||||
Increase_Beze_Degree( beze1, u1, beze2, u2, degree, add );
|
||||
|
||||
// if( j1==0 ) (*P_new2)[(*nump_new2)++]=beze1[0];
|
||||
// for( j=1; j<nump+add-1; j++ ) (*P_new2)[(*nump_new2)++]=beze2[j];
|
||||
// if( j2==nump_new1-1) (*P_new2)[(*nump_new2)++]=beze1[nump-1];
|
||||
|
||||
for( j=0; j<=nump+add-1; j++ ){
|
||||
(*P_new2)[(*nump_new2) ]=beze2[j];
|
||||
(*u_new2)[(*nump_new2)++]=u2[j];
|
||||
}
|
||||
// if( j2==nump_new1-1) (*P_new2)[(*nump_new2)++]=beze1[nump-1];
|
||||
|
||||
SGFree( beze1 ); /*beze1=NULL;*/
|
||||
SGFree( u1 ); u1 =NULL;
|
||||
SGFree( beze2 ); beze2=NULL;
|
||||
SGFree( u2 ); u2 =NULL;
|
||||
j1=j2+1;
|
||||
}
|
||||
|
||||
return TRUE;
|
||||
err:
|
||||
if(beze1) SGFree(beze1);
|
||||
if(u1) SGFree(u1);
|
||||
if(beze2) SGFree(beze2);
|
||||
if(u2) SGFree(u2);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* increase degree of the Beze segment
|
||||
* beze1 - initial segment
|
||||
* beze2 - resalting segment
|
||||
* degree - initial degree
|
||||
* add - changing degreee
|
||||
*/
|
||||
static void Increase_Beze_Degree(lpW_NODE beze1, sgFloat *u1,
|
||||
lpW_NODE beze2, sgFloat *u2, short degree, short add ){
|
||||
short i, j, k, i1, i2;
|
||||
sgFloat L, L_i;
|
||||
for( i=0; i<=degree+add; i++ ){//increase degree for span
|
||||
i1=max(0, i-add);
|
||||
i2=min(degree,i);
|
||||
L_i=1./c_beze(degree+add, i);
|
||||
for( j=i1; j<=i2; j++ ){
|
||||
L=c_beze(degree, j)*c_beze(add, i-j)*L_i;
|
||||
for( k=0; k<4; k++ ) beze2[i].vertex[k] += L*beze1[j].vertex[k];
|
||||
u2[i] += L*u1[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static sgFloat c_beze(short k1, short k2){
|
||||
return (factor(k1)/(factor(k2)*factor(k1-k2)));
|
||||
}
|
||||
|
||||
static short factor(short k){
|
||||
short i, l=1;
|
||||
if( k==0 ) return l;
|
||||
for( i=1; i<=k; i++ ) l*=i;
|
||||
return l;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------->>>>
|
||||
|
||||
@@ -0,0 +1,244 @@
|
||||
#include "../sg.h"
|
||||
static void first_deriv( short num_of_eqv, short p_c, sgFloat *u_c, sgFloat *U_c,
|
||||
sgFloat **A, short i, short num, short beg_end );
|
||||
static void second_deriv( short num_of_eqv, short p_c, sgFloat *u_c, sgFloat *U_c,
|
||||
sgFloat **A, short i, short num, short beg_end );
|
||||
static void mash_string(sgFloat **A, short num_of_eqv, short string,
|
||||
lpW_NODE P, short type );
|
||||
/**********************************************************
|
||||
* Create matrix for nurbs curve
|
||||
* n_c - number of points on curve
|
||||
* p_c - degree of curve
|
||||
* U_c - knot vector for curve
|
||||
* A - matrix
|
||||
* gran = 0 - free NURBS; 1 - closed NURBS
|
||||
* 2 - free NURBS with derivates; 3 - closed NURBS with derivates
|
||||
*/
|
||||
void nurbs_matrix_create( lpSPLY_DAT sply_dat, short p_c, sgFloat **A, short gran ){
|
||||
short i, j, k, num_of_eqv, n_c;
|
||||
|
||||
n_c = sply_dat->numk-1;
|
||||
num_of_eqv=n_c+sply_dat->numd;
|
||||
if( gran==1 || gran==3 ) num_of_eqv += 1;
|
||||
|
||||
switch( gran){
|
||||
case 0: //free NURBS
|
||||
A[0][0] = 1;
|
||||
for( i=1 ; i<n_c; i++ )
|
||||
for( j=0; j<=num_of_eqv; j++ )
|
||||
if( sply_dat->U[j] <= sply_dat->u[i] &&
|
||||
sply_dat->u[i] <= sply_dat->U[j+p_c+1] )
|
||||
A[i][j] = nurbs_basis_func( j, p_c, sply_dat->U, sply_dat->u[i] );
|
||||
A[n_c][n_c] = 1;
|
||||
break;
|
||||
case 1: //closed NURBS
|
||||
A[0][0] = 1;
|
||||
//first derivates into begining point
|
||||
first_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, 0, 1, 0 );
|
||||
//first derivates into end point
|
||||
first_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, n_c, 1, 1 );
|
||||
|
||||
mash_string ( A, num_of_eqv, 1, &sply_dat->P[1], 0 );
|
||||
|
||||
for( i=2 ; i<n_c; i++ )
|
||||
for( j=0; j<=num_of_eqv; j++ )
|
||||
if( sply_dat->U[j] <= sply_dat->u[i-1] &&
|
||||
sply_dat->u[i-1] <= sply_dat->U[j+p_c+1] )
|
||||
A[i][j] = nurbs_basis_func( j, p_c, sply_dat->U, sply_dat->u[i-1] );
|
||||
|
||||
//because have changed U-vector
|
||||
for( j=0; j<=num_of_eqv+1; j++ )
|
||||
if( sply_dat->U[j] <= sply_dat->u[i-1] &&
|
||||
sply_dat->u[i-1] <= sply_dat->U[j+p_c+1] )
|
||||
A[n_c][j%(n_c+2)] = nurbs_basis_func( j, p_c, sply_dat->U, sply_dat->u[i-1] );
|
||||
|
||||
//second derivates into begining point
|
||||
second_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, 0, n_c+1, 0 );
|
||||
//second derivates into end point
|
||||
second_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, n_c, n_c+1, 1 );
|
||||
|
||||
mash_string ( A, num_of_eqv, n_c+1, &sply_dat->P[n_c], 0 );
|
||||
|
||||
break;
|
||||
case 2://free NURBS with derivates
|
||||
for( k=0, i=0 ; i<n_c; i++, k++ ){
|
||||
for( j=0; j<=num_of_eqv; j++ )
|
||||
if( sply_dat->U[j] <= sply_dat->u[i] &&
|
||||
sply_dat->u[i] <= sply_dat->U[j+p_c+1] )
|
||||
A[k][j] = nurbs_basis_func( j, p_c, sply_dat->U, sply_dat->u[i] );
|
||||
if( sply_dat->derivates[i].num ){
|
||||
k++;
|
||||
first_deriv( num_of_eqv, p_c, sply_dat->u, sply_dat->U, A, i, k, 0 );
|
||||
for( j=0; j<=num_of_eqv; j++ ) A[k][j] *=p_c;
|
||||
mash_string ( A, num_of_eqv, k, &sply_dat->P[k], 1 );
|
||||
}
|
||||
}
|
||||
if( sply_dat->derivates[n_c].num ){
|
||||
// k++;
|
||||
first_deriv( num_of_eqv, p_c, sply_dat->u, sply_dat->U, A, n_c, k, 0 );
|
||||
for( j=0; j<=num_of_eqv; j++ ) A[k][j] *=p_c;
|
||||
mash_string ( A, num_of_eqv, k, &sply_dat->P[k], 1 );
|
||||
}
|
||||
A[num_of_eqv][num_of_eqv]=1;
|
||||
break;
|
||||
case 3://closed NURBS with derivates
|
||||
A[0][0] = 1;
|
||||
//first derivates into begining point
|
||||
first_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, 0, 1, 0 );
|
||||
//first derivates into end point
|
||||
first_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, n_c, 1, 1 );
|
||||
mash_string ( A, num_of_eqv, 1, &sply_dat->P[1], 0 );
|
||||
k=2;
|
||||
if( sply_dat->derivates[0].num ){
|
||||
first_deriv( num_of_eqv, p_c, sply_dat->u, sply_dat->U, A, 0, k, 0 );
|
||||
for( j=0; j<=num_of_eqv; j++ ) A[k][j] *=p_c;
|
||||
mash_string ( A, num_of_eqv, k, &sply_dat->P[k], 1 );
|
||||
k++;
|
||||
}
|
||||
|
||||
for( i=1; i<n_c; i++, k++ ){
|
||||
for( j=0; j<=num_of_eqv; j++ )
|
||||
if( sply_dat->U[j] <= sply_dat->u[i] &&
|
||||
sply_dat->u[i] <= sply_dat->U[j+p_c+1] )
|
||||
A[k][j] = nurbs_basis_func( j, p_c, sply_dat->U, sply_dat->u[i] );
|
||||
if( sply_dat->derivates[i].num ){
|
||||
k++;
|
||||
first_deriv( num_of_eqv, p_c, sply_dat->u, sply_dat->U, A, i, k, 0 );
|
||||
for( j=0; j<=num_of_eqv; j++ ) A[k][j] *=p_c;
|
||||
mash_string ( A, num_of_eqv, k, &sply_dat->P[k], 1 );
|
||||
}
|
||||
}
|
||||
|
||||
/* if( sply_dat->derivates[n_c].num ){
|
||||
// k++;
|
||||
first_deriv( num_of_eqv, p_c, sply_dat->u, sply_dat->U, A, n_c, k, 0 );
|
||||
for( j=0; j<=num_of_eqv; j++ ) A[k][j] *=p_c;
|
||||
mash_string ( A, num_of_eqv, k, &sply_dat->P[k], 1 );
|
||||
k++;
|
||||
}
|
||||
*/
|
||||
//second derivates into begining point
|
||||
second_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, 0, k, 0 );
|
||||
//second derivates into end point
|
||||
second_deriv( num_of_eqv+1, p_c, sply_dat->u, sply_dat->U, A, n_c, k, 1 );
|
||||
mash_string ( A, num_of_eqv, k, &sply_dat->P[k], 0 );
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* first derivates into point
|
||||
* beg_num = 0 - points from 0 till n-1
|
||||
* beg_num = 1 - point n
|
||||
*/
|
||||
static void first_deriv( short num_of_eqv, short p_c, sgFloat *u_c, sgFloat *U_c,
|
||||
sgFloat **A, short i, short num, short beg_end ){
|
||||
// sgFloat A[25][25], short i, short num, short beg_end ){
|
||||
short j;
|
||||
sgFloat t;
|
||||
for( j=1; j<=num_of_eqv; j++ ){
|
||||
if( U_c[j] <= u_c[i] && u_c[i] <= U_c[j+p_c] ){
|
||||
if( (t = U_c[j+p_c]-U_c[j] ) >= eps_n )
|
||||
t = nurbs_basis_func( j, p_c-1, U_c, u_c[i] )/t;
|
||||
else t = 0.;
|
||||
if(!beg_end){
|
||||
A[num][j-1] -= t;
|
||||
A[num][j ] += t;
|
||||
}else{
|
||||
A[num][j-1] +=t;
|
||||
A[num][j%num_of_eqv] -=t;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* second derivates into point
|
||||
* beg_num = 0 - points from 0 till n-1
|
||||
* beg_num = 1 - point n
|
||||
*/
|
||||
static void second_deriv( short num_of_eqv, short p_c, sgFloat *u_c, sgFloat *U_c,
|
||||
sgFloat **A, short k, short num, short beg_end ){
|
||||
// sgFloat A[25][25], short k, short num, short beg_end ){
|
||||
short i, j;
|
||||
sgFloat t, t1, t2;
|
||||
for( j=2; j<=num_of_eqv; j++ ){
|
||||
if( U_c[j] <= u_c[k] && u_c[k] <= U_c[j+p_c-1] ){
|
||||
i=j-1;
|
||||
if( (t = U_c[i+p_c]-U_c[i+1]) >= eps_n )
|
||||
t = nurbs_basis_func( j, p_c-2, U_c, u_c[k] )/t;
|
||||
else t = 0.;
|
||||
if( (t1 = U_c[i+p_c+1]-U_c[i+1]) >= eps_n ) t1 = 1./t1;
|
||||
else t1 = 0.;
|
||||
if( (t2 = U_c[i+p_c ]-U_c[i ]) >= eps_n ) t2 = 1./t2;
|
||||
else t2 = 0.;
|
||||
if(!beg_end){
|
||||
A[num][j-2] += t*t2;
|
||||
A[num][j-1] -= t*(t1+t2);
|
||||
A[num][j] += t*t1;
|
||||
}else{
|
||||
A[num][j-2] -= t*t2;
|
||||
A[num][j-1] += t*(t1+t2);
|
||||
A[num][j%num_of_eqv] -= t*t1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* masht. of string coeffisients to be <= 1
|
||||
*/
|
||||
static void mash_string(sgFloat **A, short num_of_eqv, short string,
|
||||
lpW_NODE P, short type ){
|
||||
short j;
|
||||
sgFloat max;
|
||||
for( max=0., j=0; j<=num_of_eqv; j++ )
|
||||
if( fabs(A[string][j]) > max ) max=fabs(A[string][j]);
|
||||
if( fabs(max) <= eps_n ) return;
|
||||
for( j=0; j<=num_of_eqv; j++ ) A[string][j] /= max;
|
||||
if( type ) for( j=0; j<3; j++ ) P->vertex[j] /=max;
|
||||
// (*P)[j] /= max;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Solution of equations sistem A*x=y, range of A = u
|
||||
*/
|
||||
BOOL gauss( sgFloat **A, lpW_NODE y, short u ){
|
||||
//BOOL gauss( sgFloat A[25][25], lpDA_POINT y, short u ){
|
||||
short i, j, k, l, n, m, t;
|
||||
sgFloat s;
|
||||
|
||||
for(n=0; n<=u; n++){
|
||||
for( k=-1, l=n; l<=u; l++ ){
|
||||
if( A[l][n] != 0 ){
|
||||
k=l;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if( k == -1 ) return FALSE;
|
||||
if( k != n ){
|
||||
for( m=n; m<=u; m++ ) change_AB(&A[n][m], &A[k][m]);
|
||||
for( t=0; t<3; t++ ) change_AB(&y[n].vertex[t], &y[k].vertex[t]);
|
||||
}
|
||||
s=1/A[n][n];
|
||||
for( t=0; t<3; t++ ) y[n].vertex[t] *= s;
|
||||
for(j=u; j>=n; j-- ) A[n][j] *= s;
|
||||
for( i=k+1; i<=u; i++){
|
||||
for( j=n+1; j<=u; j++)
|
||||
A[i][j] -= A[i][n]*A[n][j];
|
||||
for( t=0; t<3; t++ ) y[i].vertex[t] -= A[i][n]*y[n].vertex[t];
|
||||
}
|
||||
}
|
||||
for( i=u; i>=0; i--)
|
||||
for( k=i-1; k>=0; k-- ){
|
||||
if( fabs(A[k][i]) > eps_n )
|
||||
for( t=0; t<3; t++ ) y[k].vertex[t] = y[k].vertex[t] - A[k][i]*y[i].vertex[t];
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------->
|
||||
void change_AB( sgFloat *i, sgFloat *j ){
|
||||
sgFloat k;
|
||||
k = *i; *i = *j; *j = k;
|
||||
}
|
||||
@@ -0,0 +1,228 @@
|
||||
#include "../sg.h"
|
||||
static void create_work_vector( sgFloat *U, short numu, sgFloat *U_max,
|
||||
short *K_max, short*m);
|
||||
static void merge_work_vector(sgFloat *U_max, short *K_max, short *m_max,
|
||||
sgFloat *U_tmp, short *K_tmp, short m_tmp);
|
||||
|
||||
/**********************************************************
|
||||
* Merge two or more control vectors to create common
|
||||
* control vector vector
|
||||
*/
|
||||
BOOL Merge_Control_Vectors( lpVDIM ribs ){
|
||||
BOOL rt=FALSE;
|
||||
short nump_new1, nump_new2;
|
||||
short i=0, ii, r, k, s, m_max, m_tmp;
|
||||
short *K_max/*=NULL*/, *K_tmp=NULL;
|
||||
sgFloat *U_max/*=NULL*/, *U_tmp=NULL;
|
||||
SPLY_DAT sply;
|
||||
sgFloat *U_new1=NULL, *u_new1/*=NULL*/, *U_new2=NULL, *u_new2=NULL;
|
||||
W_NODE *P_new1=NULL, *P_new2=NULL;
|
||||
|
||||
long RA_max = 0;
|
||||
|
||||
while( read_elem( ribs, i++, &sply ))
|
||||
{
|
||||
RA_max+=sply.allocSizeFor_U;
|
||||
}
|
||||
|
||||
// RA - if((U_max=(sgFloat*)SGMalloc(2*MAX_POINT_ON_SPLINE*sizeof(sgFloat))) == NULL) return FALSE;
|
||||
// RA - if((K_max=(short *)SGMalloc(2*MAX_POINT_ON_SPLINE*sizeof(short))) == NULL) goto err;
|
||||
// RA - if((U_tmp=(sgFloat*)SGMalloc(MAX_POINT_ON_SPLINE *sizeof(sgFloat))) == NULL) goto err;
|
||||
// RA - if((K_tmp=(short *)SGMalloc(MAX_POINT_ON_SPLINE *sizeof(short))) == NULL) goto err;
|
||||
|
||||
if((U_max=(sgFloat*)SGMalloc(2*RA_max*sizeof(sgFloat))) == NULL) return FALSE;
|
||||
if((K_max=(short *)SGMalloc(2*RA_max*sizeof(short))) == NULL) goto err;
|
||||
if((U_tmp=(sgFloat*)SGMalloc(RA_max *sizeof(sgFloat))) == NULL) goto err;
|
||||
if((K_tmp=(short *)SGMalloc(RA_max *sizeof(short))) == NULL) goto err;
|
||||
|
||||
i=0;
|
||||
//create Max control vector
|
||||
while( read_elem( ribs, i++, &sply ))
|
||||
{
|
||||
if(i==1)
|
||||
{ //if first - put into vector
|
||||
create_work_vector(sply.U, sply.numU, U_max, K_max, &m_max);
|
||||
}
|
||||
else
|
||||
{
|
||||
create_work_vector(sply.U, sply.numU, U_tmp, K_tmp, &m_tmp);
|
||||
merge_work_vector(U_max, K_max, &m_max, U_tmp, K_tmp, m_tmp);
|
||||
}
|
||||
}
|
||||
|
||||
//add knots into all ribs
|
||||
ii=0;
|
||||
while( read_elem( ribs, ii++, &sply )){ //read current rib
|
||||
if( (P_new1=(W_NODE*)SGMalloc(sply.nump*sizeof(W_NODE))) == NULL) goto err;
|
||||
if( (u_new1=(sgFloat*)SGMalloc(sply.nump*sizeof(sgFloat))) == NULL) goto err1;
|
||||
for( i=0; i<sply.nump; i++ ){
|
||||
memcpy( &P_new1[i], &sply.P[i], sizeof(W_NODE) );
|
||||
u_new1[i]=sply.u[i];
|
||||
}
|
||||
|
||||
if( (U_new1=(sgFloat*)SGMalloc(sply.numU*sizeof(sgFloat))) == NULL ) goto err1;
|
||||
for( i=0; i<sply.numU; i++ ) U_new1[i]=sply.U[i];
|
||||
nump_new1=sply.nump;
|
||||
|
||||
//insert new knots
|
||||
for( i=0; i<m_max; i++ ){
|
||||
if( !Detect_Multy_Insertion( U_new1, sply.degree, U_max[i],
|
||||
nump_new1, &s, &k)) goto err1;
|
||||
r=K_max[i]-s;
|
||||
if( r>0 ){
|
||||
nump_new2=0;
|
||||
if( !Insert_Knot( U_new1, P_new1, u_new1, nump_new1, sply.degree, U_max[i],
|
||||
&U_new2, &P_new2, &u_new2, &nump_new2, r, k, s ) ) goto err1;
|
||||
SGFree(U_new1); U_new1=U_new2; U_new2=NULL;
|
||||
SGFree(P_new1); P_new1=P_new2; P_new2=NULL;
|
||||
SGFree(u_new1); u_new1=u_new2; u_new2=NULL;
|
||||
nump_new1=nump_new2;
|
||||
}
|
||||
}
|
||||
|
||||
// modify spline structure
|
||||
if( !Modify_Spline_Str( &sply, U_new1, nump_new1+sply.degree+1, P_new1, u_new1,
|
||||
nump_new1, sply.degree ) ) goto err1;
|
||||
if( P_new1 ) SGFree( P_new1 ); P_new1=NULL;
|
||||
if( U_new1 ) SGFree( U_new1 ); U_new1=NULL;
|
||||
if( u_new1 ) SGFree( u_new1 ); u_new1=NULL;
|
||||
if( !write_elem( ribs, ii-1, &sply )) goto err1;
|
||||
}
|
||||
rt=TRUE;
|
||||
err1:
|
||||
if( P_new1 ) SGFree( P_new1 ); if( P_new2 ) SGFree( P_new2 );
|
||||
if( U_new1 ) SGFree( U_new1 ); if( U_new2 ) SGFree( U_new2 );
|
||||
if( u_new1 ) SGFree( u_new1 ); if( u_new2 ) SGFree( u_new2 );
|
||||
err:
|
||||
if( K_tmp ) SGFree(K_tmp); if( U_tmp ) SGFree(U_tmp);
|
||||
if( K_max ) SGFree(K_max); if( U_max ) SGFree(U_max);
|
||||
return rt;
|
||||
}
|
||||
|
||||
static void create_work_vector( sgFloat *U, short numu, sgFloat *U_max,short *K_max, short*m)
|
||||
{
|
||||
short j=0, k, s;
|
||||
*m=0;
|
||||
|
||||
while(1)
|
||||
{
|
||||
for( s=0, k=j; k<numu; k++ )
|
||||
if( fabs(U[j]-U[k])<eps_n ) s++;
|
||||
U_max[(*m)]=U[j];
|
||||
K_max[(*m)++]=s;
|
||||
if( (j+=s)>=numu-1) break;
|
||||
}
|
||||
}
|
||||
|
||||
static void merge_work_vector(sgFloat *U_max, short *K_max, short *m_max,
|
||||
sgFloat *U_tmp, short *K_tmp, short m_tmp){
|
||||
short i, j, l;
|
||||
|
||||
for( i=0; i<m_tmp; i++ )
|
||||
for( j=0; j<*m_max; j++ )
|
||||
if( fabs(U_tmp[i]-U_max[j])<eps_n ){ //equal
|
||||
if( K_max[j]<K_tmp[i] ) K_max[j]=K_tmp[i];
|
||||
break;
|
||||
}else{
|
||||
if( (U_max[j]<U_tmp[i]) && (U_tmp[i]<U_max[j+1])){
|
||||
for(l=*m_max-1; l>=j+1; l-- ){
|
||||
U_max[l+1]=U_max[l];
|
||||
K_max[l+1]=K_max[l];
|
||||
}
|
||||
U_max[j+1]=U_tmp[i];
|
||||
K_max[j+1]=K_tmp[i];
|
||||
(*m_max)++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* Create 4d point from 3d weight point
|
||||
* for use nonrational B-spline
|
||||
*/
|
||||
void Create_4D_From_3D( lpW_NODE node, sgFloat *point, sgFloat w){
|
||||
short i;
|
||||
|
||||
for( i=0; i<3; i++ ) node->vertex[i] = w*point[i];
|
||||
node->vertex[3]=w;
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* Create 3d weight point from 4d point
|
||||
* for use rational B-spline
|
||||
*/
|
||||
void Create_3D_From_4D( lpW_NODE node, sgFloat *point, sgFloat *w){
|
||||
short i;
|
||||
sgFloat t;
|
||||
|
||||
*w=node->vertex[3];
|
||||
t = ((*w)==0) ? (1) : (*w);
|
||||
for( i=0; i<3; i++) point[i]=node->vertex[i]/t;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Calculate 2point_distance into 4D-space
|
||||
*/
|
||||
sgFloat dpoint_distance_4d(lpW_NODE p1, lpW_NODE p2){
|
||||
return(sqrt( (p1->vertex[0]-p2->vertex[0])*(p1->vertex[0]-p2->vertex[0])+
|
||||
(p1->vertex[1]-p2->vertex[1])*(p1->vertex[1]-p2->vertex[1])+
|
||||
(p1->vertex[2]-p2->vertex[2])*(p1->vertex[2]-p2->vertex[2])+
|
||||
(p1->vertex[3]-p2->vertex[3])*(p1->vertex[3]-p2->vertex[3]) ));
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Calculate 2point_distance into 3D-space
|
||||
* (convert 4D-point into 3D-point)
|
||||
*/
|
||||
sgFloat dpoint_distance_4dl(lpW_NODE p1, lpW_NODE p2){
|
||||
sgFloat t;
|
||||
D_POINT pp1, pp2;
|
||||
t = ((p1->vertex[3])==0) ? (1) : (p1->vertex[3]);
|
||||
pp1.x=p1->vertex[0]/t;
|
||||
pp1.y=p1->vertex[1]/t;
|
||||
pp1.z=p1->vertex[2]/t;
|
||||
|
||||
t = ((p2->vertex[3])==0) ? (1) : (p2->vertex[3]);
|
||||
pp2.x=p2->vertex[0]/t;
|
||||
pp2.y=p2->vertex[1]/t;
|
||||
pp2.z=p2->vertex[2]/t;
|
||||
|
||||
t=dpoint_distance( &pp1, &pp2 );
|
||||
return ( t );
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------->>>>>
|
||||
/**********************************************************
|
||||
* Detect multiplisity of insertion for parameter T and
|
||||
* interval of the insertion
|
||||
* U - control vector
|
||||
* p - degree
|
||||
* nump - number of points
|
||||
* s - number of existing parameter
|
||||
* kk - number of interval
|
||||
*/
|
||||
BOOL Detect_Multy_Insertion(sgFloat *U, short p, sgFloat t, short nump, short *s,
|
||||
short *k ){
|
||||
short i, num;
|
||||
|
||||
num = nump + p + 1;
|
||||
|
||||
//detect multiplisity
|
||||
for( *s=0, i=0; i<num; i++ ) if( fabs(U[i]-t) <= eps_n ) (*s)++;
|
||||
|
||||
if( (*s) == p+1 ){
|
||||
for( (*k)=0; (*k)<num; (*k)++ ) if( fabs( U[*k]-t ) <= eps_n ) break;
|
||||
//number of insertions to create break point
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//detect span for insertion
|
||||
for( (*k)=0; (*k)<num-1; (*k)++ ){
|
||||
if( U[*k] <= t && t < U[*k+1] ) break;
|
||||
if( U[*k] < U[*k+1] && t == U[*k+1] && t == 1. ) break;
|
||||
}
|
||||
if( *k>num-1 ) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
@@ -0,0 +1,505 @@
|
||||
#include "../sg.h"
|
||||
|
||||
static BOOL calculate_sply(lpSPLY_DAT sply_dat);
|
||||
|
||||
|
||||
BOOL add_sply_point(lpSPLY_DAT sply_dat, lpD_POINT point, short num){
|
||||
int i;
|
||||
|
||||
if( sply_dat->sdtype & SPL_INT) {
|
||||
if( sply_dat->numk >= MAX_POINT_ON_SPLINE )
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 0) ) return FALSE;
|
||||
if( num == 0 ){
|
||||
for( i=sply_dat->numk-1; i >= 0; i--) {
|
||||
memcpy(&sply_dat->knots[i+1], &sply_dat->knots[i], sizeof(DA_POINT));
|
||||
memcpy(&sply_dat->derivates[i+1], &sply_dat->derivates[i], sizeof(SNODE));
|
||||
}
|
||||
}
|
||||
memcpy(&sply_dat->knots[num], point, sizeof(D_POINT));
|
||||
sply_dat->derivates[num].num = 0; //
|
||||
sply_dat->numk++;
|
||||
return calculate_sply(sply_dat);
|
||||
} else {
|
||||
if( sply_dat->nump >= MAX_POINT_ON_SPLINE )
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 2) ) return FALSE;
|
||||
if( num == 0 )
|
||||
for( i=sply_dat->nump-1; i >= 0; i--){
|
||||
memcpy(&sply_dat->P[i+1], &sply_dat->P[i], sizeof(W_NODE));
|
||||
if( spl_work & EDIT_SPL )sply_dat->u[i+1] = sply_dat->u[i];
|
||||
}
|
||||
|
||||
Create_4D_From_3D( &sply_dat->P[num], (sgFloat *)point, 1. );//
|
||||
sply_dat->nump++;
|
||||
|
||||
if( sply_dat->nump > 2 ){
|
||||
if( spl_work & CRE_SPL ) parameter( sply_dat, 0, TRUE );
|
||||
if( spl_work & EDIT_SPL ) spl_work = (SPL_WORK)(spl_work | ADD_SPL);
|
||||
return calculate_P(sply_dat, 0); //
|
||||
}else{
|
||||
memcpy(&sply_dat->knots[num], point, sizeof(D_POINT));
|
||||
return TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BOOL insert_sply_point(lpSPLY_DAT sply_dat, short num, lpD_POINT point){
|
||||
short i;
|
||||
sgFloat D1, D2, w1, w2, s, u_ins;
|
||||
D_POINT point1, point2;
|
||||
|
||||
if (sply_dat->sdtype & SPL_INT) {
|
||||
if (sply_dat->numk >= MAX_POINT_ON_SPLINE)
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 0)) return FALSE;
|
||||
|
||||
//
|
||||
for( i=sply_dat->numk-1; i >= num; i-- ) {
|
||||
memcpy(&sply_dat->knots[i+1], &sply_dat->knots[i], sizeof(DA_POINT));
|
||||
memcpy(&sply_dat->derivates[i+1], &sply_dat->derivates[i], sizeof(SNODE));
|
||||
}
|
||||
memcpy(&sply_dat->knots[num], point, sizeof(D_POINT));
|
||||
sply_dat->derivates[num].num = 0; //
|
||||
sply_dat->numk++;
|
||||
} else {
|
||||
if (sply_dat->nump >= MAX_POINT_ON_SPLINE)
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 2)) return FALSE;
|
||||
//
|
||||
Create_3D_From_4D( &sply_dat->P[num-1], (sgFloat *)&point1, &w1);
|
||||
Create_3D_From_4D( &sply_dat->P[num], (sgFloat *)&point2, &w2);
|
||||
D1=dpoint_distance( point, &point1);
|
||||
D2=dpoint_distance( point, &point2);
|
||||
s=w1*D1/(w1*D1+w2*D2);
|
||||
u_ins=sply_dat->U[num]+s*(sply_dat->U[num+sply_dat->degree]-sply_dat->U[num]);
|
||||
//
|
||||
if( !Create_Multy_Point( sply_dat, u_ins, 21 )) return FALSE;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
BOOL close_sply(lpSPLY_DAT sply_dat){
|
||||
short i;
|
||||
sgFloat w;
|
||||
D_POINT point;
|
||||
//W_NODE point;
|
||||
GEO_LINE gline1, gline2;
|
||||
|
||||
if( sply_dat->sdtype & SPL_CLOSE ) {
|
||||
nurbs_handler_err(SPL_INTERNAL_ERROR);
|
||||
return FALSE;
|
||||
}
|
||||
if( sply_dat->sdtype & SPL_INT ) {
|
||||
if( sply_dat->numk<3 ) {
|
||||
nurbs_handler_err(SPL_EMPTY_DATA);
|
||||
return FALSE;
|
||||
}
|
||||
if( sply_dat->numk >= MAX_POINT_ON_SPLINE || sply_dat->nump+2 >= MAX_POINT_ON_SPLINE )
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 0)) return FALSE;
|
||||
|
||||
if( !dpoint_eq((lpD_POINT)&sply_dat->knots[0], (lpD_POINT)&sply_dat->knots[sply_dat->numk-1], eps_d)) sply_dat->numk++;
|
||||
memcpy(&sply_dat->knots[sply_dat->numk-1], &sply_dat->knots[0], sizeof(D_POINT));
|
||||
} else {
|
||||
if( sply_dat->nump<3 ) {
|
||||
nurbs_handler_err(SPL_EMPTY_DATA);
|
||||
return FALSE;
|
||||
}
|
||||
if( sply_dat->nump+2 >= MAX_POINT_ON_SPLINE )
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 2)) return FALSE;
|
||||
|
||||
/* if( sply_dat->degree > 1 ){
|
||||
//for smoth closer add new point r(n-1) = 2*r0 - r1
|
||||
for( i=0; i<4; i++) point.vertex[i]=2*sply_dat->P[0].vertex[i] - sply_dat->P[1].vertex[i];
|
||||
if( dpoint_distance_4dl( &sply_dat->P[sply_dat->nump-1], &point) > eps_d ){
|
||||
Create_3D_From_4D( &point, (sgFloat *)&point1, &w);
|
||||
if( !add_sply_point( sply_dat, &point1, sply_dat->nump) ) return FALSE;
|
||||
sply_dat->P[sply_dat->numk-1].vertex[3]=w;
|
||||
}
|
||||
}
|
||||
if( dpoint_distance_4dl( &sply_dat->P[0], &point) > eps_d ) sply_dat->nump++;
|
||||
*/
|
||||
|
||||
//create line on spline points
|
||||
Create_3D_From_4D( &sply_dat->P[0], (sgFloat *)&gline1.v1, &w);
|
||||
Create_3D_From_4D( &sply_dat->P[1], (sgFloat *)&gline1.v2, &w);
|
||||
Create_3D_From_4D( &sply_dat->P[sply_dat->nump-2], (sgFloat *)&gline2.v1, &w);
|
||||
Create_3D_From_4D( &sply_dat->P[sply_dat->nump-1], (sgFloat *)&gline2.v2, &w);
|
||||
//---------------------------------------------------------
|
||||
//detect intersection of the two lines
|
||||
if( !intersect_3d_ll(&gline1, 0, &gline2, 0, &point, &i) ) return FALSE;
|
||||
|
||||
//---------------------------------------------------------
|
||||
if ( i != 0 ){ // there are points of intersection
|
||||
if( !add_sply_point( sply_dat, &point, sply_dat->nump) ) return FALSE;
|
||||
sply_dat->nump++;
|
||||
memcpy(&sply_dat->P[sply_dat->nump-1], &sply_dat->P[0], sizeof(W_NODE));
|
||||
}else return FALSE;
|
||||
}
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | SPL_CLOSE);
|
||||
return calculate_sply(sply_dat);
|
||||
}
|
||||
|
||||
BOOL move_sply_point(lpSPLY_DAT sply_dat, short num, lpD_POINT point){
|
||||
short i;
|
||||
sgFloat w, l1, l2;
|
||||
sgFloat r, pp[3];
|
||||
|
||||
if (sply_dat->sdtype & SPL_INT) {
|
||||
l1=l2=10.*eps_d;
|
||||
if( num>0 ) l1 = dpoint_distance((lpD_POINT)&sply_dat->knots[num-1], point);
|
||||
else if( sply_dat->sdtype&SPL_CLOSE )
|
||||
l1 = dpoint_distance((lpD_POINT)&sply_dat->knots[sply_dat->numk-2], point);
|
||||
if( num<sply_dat->numk )l2 = dpoint_distance((lpD_POINT)&sply_dat->knots[num+1], point);
|
||||
else if( sply_dat->sdtype & SPL_CLOSE )
|
||||
l2 = dpoint_distance((lpD_POINT)&sply_dat->knots[1], point);
|
||||
if( l1<eps_d || l2<eps_d ) {
|
||||
// put_message(SPL_BAD_POINT, NULL, 0); //
|
||||
return FALSE;
|
||||
}
|
||||
memcpy(&sply_dat->knots[num], point, sizeof(D_POINT));
|
||||
if( sply_dat->sdtype&SPL_CLOSE && (num==0 || num==sply_dat->numk-1))
|
||||
memcpy(&sply_dat->knots[(num)?0:sply_dat->numk-1], point, sizeof(D_POINT));
|
||||
return calculate_sply(sply_dat);
|
||||
}else{
|
||||
w=sply_dat->P[num].vertex[3];
|
||||
Create_4D_From_3D( &sply_dat->P[num], (sgFloat *)point, w );//
|
||||
memcpy(&sply_dat->knots[num], point, sizeof(D_POINT));
|
||||
|
||||
if( sply_dat->sdtype&SPL_CLOSE && (num==0 || num==sply_dat->nump-1)){
|
||||
Create_4D_From_3D( &sply_dat->P[(num)?0:sply_dat->nump-1], (sgFloat *)point, w );
|
||||
memcpy(&sply_dat->knots[(num)?0:sply_dat->nump-1], point, sizeof(D_POINT));
|
||||
}
|
||||
if( sply_dat->nump > 2 ){
|
||||
if( spl_work & CRE_SPL ) parameter( sply_dat, 0, TRUE );
|
||||
if( spl_work & ADD_SPL ){
|
||||
spl_work = (SPL_WORK)(spl_work & (~ADD_SPL));
|
||||
if( num == 0 ){
|
||||
Create_3D_From_4D( &sply_dat->P[0], (sgFloat *)pp, &w );
|
||||
if( (r=dpoint_distance( (lpD_POINT)point, (lpD_POINT)pp ))<eps_d ) return TRUE;
|
||||
r/=spl_length(sply_dat);
|
||||
|
||||
for( i=sply_dat->numU-1; i >= 0; i--) sply_dat->U[i+1] = sply_dat->U[i]+r;
|
||||
sply_dat->u[0]=0;
|
||||
sply_dat->U[0]=sply_dat->U[1]=sply_dat->U[2]=0;
|
||||
}else{
|
||||
Create_3D_From_4D( &sply_dat->P[num-1], (sgFloat *)pp, &w );
|
||||
if( (r=dpoint_distance( (lpD_POINT)point, (lpD_POINT)pp ))<eps_d ) return TRUE;
|
||||
r/=spl_length_P(sply_dat);
|
||||
|
||||
sply_dat->u[num]=sply_dat->u[num-1]+r;
|
||||
for( i=0; i<sply_dat->degree+1; i++ )
|
||||
sply_dat->U[sply_dat->nump+i]=sply_dat->U[sply_dat->nump-1]+r;
|
||||
}
|
||||
Create_4D_From_3D( &sply_dat->P[num], (sgFloat *)point, 1. );//
|
||||
sply_dat->numU++;
|
||||
if( !Reparametrization( sply_dat->nump, sply_dat->numU, 0, 1, sply_dat->degree,
|
||||
sply_dat->U, sply_dat->P, sply_dat->u )) return FALSE;
|
||||
}
|
||||
return calculate_P(sply_dat, 0); //
|
||||
}else{
|
||||
memcpy(&sply_dat->knots[num], point, sizeof(D_POINT));
|
||||
return TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BOOL delete_sply_point(lpSPLY_DAT sply_dat, short num){
|
||||
short i, l, beg;
|
||||
|
||||
if (sply_dat->sdtype & SPL_INT) {
|
||||
//
|
||||
if( (sply_dat->sdtype & SPL_CLOSE) && sply_dat->numk<sply_dat->degree+2 ) {
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype & ~SPL_CLOSE);
|
||||
sply_dat->numk--;
|
||||
if( num==sply_dat->numk ) num = 0;
|
||||
}
|
||||
|
||||
if( num != sply_dat->numk - 1 && num != 0) { //
|
||||
if( dpoint_distance((lpD_POINT)&sply_dat->knots[num - 1],
|
||||
(lpD_POINT)&sply_dat->knots[num + 1])<eps_d) goto err;
|
||||
}else{ //
|
||||
if( sply_dat->sdtype & SPL_CLOSE ) { //
|
||||
num=0;
|
||||
if(dpoint_distance((lpD_POINT)&sply_dat->knots[1],
|
||||
(lpD_POINT)&sply_dat->knots[sply_dat->numk-2])<eps_d) goto err;
|
||||
// - 1
|
||||
memcpy(&sply_dat->knots[sply_dat->numk-1], &sply_dat->knots[1], sizeof(D_POINT));
|
||||
memcpy(&sply_dat->derivates[sply_dat->numk-1], &sply_dat->derivates[1], sizeof(SNODE));
|
||||
}
|
||||
}
|
||||
//
|
||||
if( (l = sply_dat->numk-num-1)>0 ) {
|
||||
if (sply_dat->derivates[num].num) sply_dat->numd--;
|
||||
memcpy(&sply_dat->knots[num], &sply_dat->knots[num+1], l*sizeof(D_POINT));
|
||||
memcpy(&sply_dat->derivates[num], &sply_dat->derivates[num+1], l*sizeof(SNODE));
|
||||
}
|
||||
sply_dat->numk--;
|
||||
return calculate_sply(sply_dat);
|
||||
} else {//--------------------->>>>>>>> APPR spline
|
||||
//
|
||||
if( (sply_dat->sdtype&SPL_CLOSE ) && sply_dat->nump<sply_dat->degree+2 ) {
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype & ~SPL_CLOSE);
|
||||
sply_dat->nump--;
|
||||
if( num == sply_dat->nump ) num=0;
|
||||
}
|
||||
|
||||
//
|
||||
if( sply_dat->sdtype & SPL_CLOSE && ( num == sply_dat->nump-1 || num == 0 ))
|
||||
memcpy( &sply_dat->P[sply_dat->nump-1], &sply_dat->P[1], sizeof(W_NODE));// 1
|
||||
|
||||
//
|
||||
if( (l = sply_dat->nump-num-1 ) > 0){
|
||||
memcpy(&sply_dat->P[num], &sply_dat->P[num+1], l*sizeof(W_NODE));
|
||||
memcpy(&sply_dat->u[num], &sply_dat->u[num+1], l*sizeof(sgFloat));
|
||||
}
|
||||
sply_dat->nump--;
|
||||
|
||||
if( spl_work&CRE_SPL ) parameter( sply_dat, 0, TRUE );
|
||||
else{
|
||||
if( num==0 ) beg=sply_dat->degree+2;
|
||||
else{
|
||||
if( num==sply_dat->nump ) beg=sply_dat->nump+2;
|
||||
else{
|
||||
if( num==sply_dat->nump-1 ) beg=sply_dat->degree+1+num-1;
|
||||
else beg = sply_dat->degree+1+num;
|
||||
}
|
||||
}
|
||||
for( i=beg; i<sply_dat->numU; i++ ) sply_dat->U[i-1]=sply_dat->U[i];
|
||||
sply_dat->numU--;
|
||||
if( num==0 || num==sply_dat->nump)
|
||||
Reparametrization( sply_dat->nump, sply_dat->numU, 0, 1, sply_dat->degree,
|
||||
sply_dat->U, sply_dat->P, sply_dat->u );
|
||||
|
||||
}
|
||||
return calculate_P(sply_dat, 0); //
|
||||
}
|
||||
err:
|
||||
nurbs_handler_err(SPL_BAD_POINT); //
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
BOOL break_close_sply( lpSPLY_DAT sply_dat, short num ){
|
||||
short i, n;
|
||||
DA_POINT da1;
|
||||
W_NODE wn;
|
||||
SNODE sn1;
|
||||
|
||||
if( sply_dat->sdtype&SPL_INT ) {
|
||||
if( num<sply_dat->numk-1 ) { //
|
||||
// num+1
|
||||
n=sply_dat->numk-2;
|
||||
for( i=num+1; i>0; i-- ) {
|
||||
memcpy(da1, sply_dat->knots[0], sizeof(DA_POINT));
|
||||
sn1 = sply_dat->derivates[0];
|
||||
memcpy(sply_dat->knots[0], sply_dat->knots[1], n * sizeof(DA_POINT));
|
||||
memcpy(&sply_dat->derivates[0], &sply_dat->derivates[1], n * sizeof(SNODE));
|
||||
memcpy(sply_dat->knots[n], da1, sizeof(DA_POINT));
|
||||
sply_dat->derivates[n] = sn1;
|
||||
}
|
||||
}
|
||||
sply_dat->numk--;
|
||||
}else{
|
||||
if( num<sply_dat->nump-1 ) { //
|
||||
// num+1
|
||||
n=sply_dat->nump-2;
|
||||
for( i=num+1; i>0; i-- ) {
|
||||
memcpy(&wn, &sply_dat->P[0], sizeof(W_NODE));
|
||||
memcpy(&sply_dat->P[0], &sply_dat->P[1], n*sizeof(W_NODE));
|
||||
memcpy(&sply_dat->P[n], &wn, sizeof(W_NODE));
|
||||
}
|
||||
}
|
||||
sply_dat->nump--;
|
||||
}
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype & ~SPL_CLOSE);
|
||||
return calculate_sply(sply_dat);
|
||||
}
|
||||
|
||||
BOOL set_derivate_to_point(lpSPLY_DAT sply_dat, short num, lpD_POINT derivates){
|
||||
if( sply_dat->sdtype & SPL_APPR) return TRUE;
|
||||
if( (num == 0) || (num == sply_dat->numk - 1) ){// first/end point && closed
|
||||
if( dpoint_distance((lpD_POINT)&sply_dat->knots[0],
|
||||
(lpD_POINT)&sply_dat->knots[sply_dat->numk - 1]) < eps_d ){
|
||||
if( dskalar_product(derivates,derivates) < eps_d )
|
||||
get_point_on_sply(sply_dat, sply_dat->u[num], derivates, 1);
|
||||
|
||||
memcpy(sply_dat->derivates[0].p, derivates, sizeof(D_POINT));
|
||||
memcpy(sply_dat->derivates[sply_dat->numk - 1].p, derivates, sizeof(D_POINT));
|
||||
if( sply_dat->derivates[0].num == 0 ){
|
||||
sply_dat->derivates[0].num=1;
|
||||
sply_dat->numd++;
|
||||
}
|
||||
if( sply_dat->derivates[sply_dat->numk - 1].num == 0 ){
|
||||
sply_dat->derivates[sply_dat->numk - 1].num=1;
|
||||
sply_dat->numd++;
|
||||
}
|
||||
if( sply_dat->nump+1 >= MAX_POINT_ON_SPLINE )
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 1 ))
|
||||
return FALSE;
|
||||
return calculate_sply(sply_dat);
|
||||
}
|
||||
}
|
||||
memcpy(sply_dat->derivates[num].p, derivates, sizeof(D_POINT));
|
||||
if (!sply_dat->derivates[num].num) {
|
||||
sply_dat->derivates[num].num = 1;
|
||||
sply_dat->numd++;
|
||||
}
|
||||
if( sply_dat->nump >= MAX_POINT_ON_SPLINE ){
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 1 ))
|
||||
return FALSE;
|
||||
}
|
||||
return calculate_sply(sply_dat);
|
||||
}
|
||||
|
||||
BOOL fix_free_derivate_to_point(lpSPLY_DAT sply_dat, short num, BOOL fix){
|
||||
D_POINT deriv;
|
||||
|
||||
if (sply_dat->sdtype & SPL_APPR) return TRUE;
|
||||
if (fix && !sply_dat->derivates[num].num) {
|
||||
get_point_on_sply(sply_dat, sply_dat->u[num], &deriv, 1);
|
||||
memcpy(sply_dat->derivates[num].p, &deriv, sizeof(D_POINT));
|
||||
sply_dat->derivates[num].num=1;
|
||||
sply_dat->numd++;
|
||||
}
|
||||
if (!fix && sply_dat->derivates[num].num) {
|
||||
sply_dat->derivates[num].num = 0;
|
||||
sply_dat->numd--;
|
||||
}
|
||||
if( sply_dat->nump >= MAX_POINT_ON_SPLINE )
|
||||
if( !expand_spl( sply_dat, MAX_POINT_ON_SPLINE/2, sply_dat->degree, 1 ))
|
||||
return FALSE;
|
||||
|
||||
return calculate_sply(sply_dat);
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Change spline type from INT to APPR
|
||||
*/
|
||||
BOOL change_INT_APPR(lpSPLY_DAT sply_dat){
|
||||
short i;
|
||||
if( sply_dat->sdtype & SPL_APPR) return FALSE;
|
||||
|
||||
// free arrays for short spline
|
||||
if (sply_dat->derivates) {
|
||||
SGFree(sply_dat->derivates);
|
||||
sply_dat->derivates = NULL;
|
||||
}
|
||||
|
||||
sply_dat->numd = 0;
|
||||
// new arrays for APPR spline
|
||||
|
||||
//change spline type
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype & ~SPL_INT);
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | SPL_APPR);
|
||||
|
||||
//all weight put into 1 at the begining
|
||||
for( i=0; i<sply_dat->nump; i++ ) sply_dat->P[i].vertex[3] = 1.;
|
||||
// calculate_sply(sply_dat);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Change spline type from APPR to INT
|
||||
*/
|
||||
BOOL change_APPR_INT(lpSPLY_DAT sply_dat){
|
||||
short j = MAX_POINT_ON_SPLINE, i;
|
||||
|
||||
if( sply_dat->sdtype & SPL_INT) return FALSE;
|
||||
// free array for APPR spline
|
||||
if( sply_dat->P ){
|
||||
SGFree( sply_dat->P );
|
||||
sply_dat->P = NULL;
|
||||
}
|
||||
|
||||
// new arrays for short spline
|
||||
if((sply_dat->P = (W_NODE*)SGMalloc((2*j+2)*sizeof(W_NODE))) == NULL)return FALSE;
|
||||
//all weight put into 1
|
||||
for( i=0; i<2*j+2; i++ ) sply_dat->P[i].vertex[3] = 1.;
|
||||
|
||||
if((sply_dat->derivates = (SNODE*)SGMalloc(j*sizeof(SNODE))) == NULL) return FALSE;
|
||||
memset(sply_dat->derivates, 0, j*sizeof(SNODE));
|
||||
sply_dat->numd = 0;
|
||||
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype & ~SPL_APPR);
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | SPL_INT);
|
||||
calculate_sply(sply_dat);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Change spline degree( for APPR spline ONLY!!!)
|
||||
*/
|
||||
BOOL change_degree(lpSPLY_DAT sply_dat, short new_p ){
|
||||
if( sply_dat->sdtype & SPL_INT ) return TRUE;
|
||||
if( new_p <= 0 ) return FALSE;
|
||||
if( new_p == sply_dat->degree ) return TRUE;
|
||||
if( new_p > sply_dat->degree ) if( !Increase_Spline_Degree( sply_dat, new_p)) return FALSE;
|
||||
// else if( !Decrease_Spline_Degree( sply_dat, new_p)) return FALSE;
|
||||
return calculate_P(sply_dat, 0);
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Change weight
|
||||
* num - number of point
|
||||
* new_w - new weight
|
||||
*/
|
||||
BOOL change_weight(lpSPLY_DAT sply_dat, short num, sgFloat new_w ){
|
||||
sgFloat w;
|
||||
D_POINT point;
|
||||
|
||||
if( sply_dat->sdtype & SPL_INT ) return TRUE;
|
||||
if( new_w<0 && new_w>1 ) return FALSE;
|
||||
Create_3D_From_4D( &sply_dat->P[num], (sgFloat *)&point, &w );
|
||||
Create_4D_From_3D( &sply_dat->P[num], (sgFloat *)&point, new_w );
|
||||
return calculate_P(sply_dat, 0);
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Put break into spline knots
|
||||
* if APR-spline - put sgFloat spline knot
|
||||
*/
|
||||
BOOL add_break_knots(lpSPLY_DAT sply_dat, short num ){
|
||||
if( sply_dat->sdtype & SPL_INT ) return TRUE;
|
||||
if( !Create_Multy_Point( sply_dat, sply_dat->u[num], 0 ) ) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Extract break from spline point
|
||||
* i.e. extract sgFloat spline knot
|
||||
*/
|
||||
BOOL del_break_knots(lpSPLY_DAT sply_dat, short num ){
|
||||
if( sply_dat->sdtype & SPL_INT ) return TRUE;
|
||||
if( !Del_Knot( sply_dat->U, sply_dat->P, sply_dat->u, &sply_dat->nump,
|
||||
sply_dat->numU, sply_dat->degree, sply_dat->u[num], sply_dat->degree) )
|
||||
return FALSE;
|
||||
return calculate_P(sply_dat, 0);
|
||||
}
|
||||
|
||||
/*******************************************************************************
|
||||
* Calculate spline
|
||||
*/
|
||||
static BOOL calculate_sply(lpSPLY_DAT sply_dat){
|
||||
short nnn, condition, i;
|
||||
sgFloat weight;
|
||||
|
||||
nnn = (sply_dat->sdtype & SPL_INT) ? sply_dat->numk : sply_dat->nump;
|
||||
if (nnn > 2) {
|
||||
//detect the spline type
|
||||
if( sply_dat->sdtype & SPL_CLOSE ){
|
||||
if( sply_dat->numd == 0 ) condition=1; // closed without derivation
|
||||
else{
|
||||
if( sply_dat->derivates[0].num == 1 ) condition=2; //special case of closed spline
|
||||
else condition=3; // closed with derivates
|
||||
}
|
||||
}else{
|
||||
if( sply_dat->numd == 0 ) condition=0; // without derivation
|
||||
else condition=2; //with derivates
|
||||
}
|
||||
//-------->>>>>>>>>>>>>>>>>>>>>>>>>>
|
||||
//!!!!!!!!! , .. .
|
||||
parameter( sply_dat, condition, TRUE );
|
||||
return calculate_P(sply_dat, condition); //
|
||||
} else if (sply_dat->sdtype & SPL_APPR) {
|
||||
sply_dat->numk = sply_dat->nump;
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
Create_3D_From_4D( &sply_dat->P[i], (sgFloat *)&sply_dat->knots[i], &weight);
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#include "../sg.h"
|
||||
|
||||
void nurbs_handler_err(short cod,...)
|
||||
{
|
||||
char s[255], ss[20];
|
||||
short len;
|
||||
|
||||
strncpy(s, GetIDS(IDS_SG159), sizeof(s) - 1);
|
||||
len = sizeof(s) - strlen(s);
|
||||
switch (cod) {
|
||||
case SPL_NO_MEMORY: //
|
||||
strncat(s,GetIDS(IDS_SG160),len);
|
||||
break;
|
||||
case SPL_NO_HEAP: //
|
||||
strncat(s,GetIDS(IDS_SG161),len);
|
||||
break;
|
||||
case SPL_MAX: //
|
||||
strncat(s,GetIDS(IDS_SG163),len);
|
||||
break;
|
||||
case SPL_INTERNAL_ERROR: //
|
||||
strncat(s,GetIDS(IDS_SG166),len);
|
||||
break;
|
||||
case SPL_ENPTY_DATA: //
|
||||
strncat(s,GetIDS(IDS_SG167),len);
|
||||
break;
|
||||
case SPL_BAD_POINT: //
|
||||
strncat(s, GetIDS(IDS_SG168),len);
|
||||
break;
|
||||
default:
|
||||
strncat(s,GetIDS(IDS_SG186),len);
|
||||
//itoa(cod,ss,10);
|
||||
strncat(s,ss,sizeof(s) - strlen(s));
|
||||
break;
|
||||
}
|
||||
put_message(EMPTY_MSG, s, 0);
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
#include "../sg.h"
|
||||
static BOOL check_line1(lpF_PLPOINT min,lpF_PLPOINT max,
|
||||
lpF_PLPOINT p1, lpF_PLPOINT p2);
|
||||
static BOOL check_point1(lpF_PLPOINT min, lpF_PLPOINT max, lpF_PLPOINT p);
|
||||
|
||||
BOOL find_sply_knot(lpD_POINT vp, lpSPLY_DAT spl, short *numint, short *numknot){
|
||||
sgFloat l1=0.0, l2, w;
|
||||
F_PLPOINT min, max, fp1, fp2;
|
||||
D_POINT p1, p2;
|
||||
BOOL knot = TRUE;
|
||||
short i;
|
||||
|
||||
//
|
||||
// l1 = f_tfm_x(&curr_w3->fwind, trap_size/2 + 1) - f_tfm_x(&curr_w3->fwind, 0);
|
||||
min.x = (float)(vp->x - l1);
|
||||
min.y = (float)(vp->y - l1);
|
||||
max.x = (float)(vp->x + l1);
|
||||
max.y = (float)(vp->y + l1);
|
||||
|
||||
//
|
||||
*numint = *numknot = 0;
|
||||
if (spl->sdtype & SPL_INT) {
|
||||
get_first_sply_point(spl, 0., FALSE, &p1);
|
||||
} else {
|
||||
Create_3D_From_4D( &spl->P[0], (sgFloat *)&p1, &w);
|
||||
i = 1;
|
||||
}
|
||||
gcs_to_vcs(&p1, &p1); fp1.x = (float)p1.x; fp1.y = (float)p1.y;
|
||||
|
||||
if(spl->numk == 1) return check_point1(&min, &max, &fp1);
|
||||
|
||||
l1 = 0.;
|
||||
while (TRUE) {
|
||||
if (spl->sdtype & SPL_INT) {
|
||||
if (!get_next_sply_point_and_knot(spl, &p2, &knot)) break;
|
||||
} else {
|
||||
if (i >= spl->nump) break;
|
||||
Create_3D_From_4D( &spl->P[i++], (sgFloat *)&p2, &w);
|
||||
knot = TRUE;
|
||||
}
|
||||
gcs_to_vcs(&p2, &p2); fp2.x = (float)p2.x; fp2.y = (float)p2.y;
|
||||
//
|
||||
if(check_line1(&min, &max, &fp1, &fp2))goto met_find;
|
||||
l1 += dpoint_distance_2d(&p1, &p2);
|
||||
if (knot) {
|
||||
(*numint)++;
|
||||
l1 = 0.;
|
||||
}
|
||||
p1 = p2; fp1 = fp2;
|
||||
}
|
||||
return FALSE;
|
||||
|
||||
met_find:
|
||||
l1 += dpoint_distance_2d(vp, &p1);
|
||||
l2 = dpoint_distance_2d(vp, &p2);
|
||||
while (!knot) {
|
||||
p1 = p2;
|
||||
get_next_sply_point_and_knot(spl, &p2, &knot);
|
||||
gcs_to_vcs(&p2, &p2);
|
||||
l2 += dpoint_distance_2d(&p1, &p2);
|
||||
}
|
||||
*numknot = (l1 > l2) ? *numint + 1 : *numint;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static BOOL check_line1(lpF_PLPOINT min,lpF_PLPOINT max,
|
||||
lpF_PLPOINT p1, lpF_PLPOINT p2){
|
||||
float x1,y1,x2,y2,rc;
|
||||
register short i,j,k;
|
||||
|
||||
x1=p1->x;
|
||||
y1=p1->y;
|
||||
x2=p2->x;
|
||||
y2=p2->y;
|
||||
if (x1 < min->x ) i=1;
|
||||
else {
|
||||
if (x1 > max->x) i=2;
|
||||
else i=0;
|
||||
}
|
||||
if (y1 < min->y) i |=4;
|
||||
else {
|
||||
if(y1 > max->y) i |=8;
|
||||
}
|
||||
|
||||
if (x2 < min->x ) j=1;
|
||||
else {
|
||||
if (x2 > max->x) j=2;
|
||||
else j=0;
|
||||
}
|
||||
if (y2 < min->y) j |=4;
|
||||
else {
|
||||
if(y2 > max->y) j |=8;
|
||||
}
|
||||
if ( !i || !j ) return TRUE;
|
||||
if ( (i & j) != 0) return FALSE;
|
||||
for (;;) {
|
||||
if ( !i || !j ) return TRUE;
|
||||
if ( (i & j)) return FALSE;
|
||||
if ( !i ) {
|
||||
k=i; i=j; j=k;
|
||||
rc=x1; x1=x2; x2=rc;
|
||||
rc=y1; y1=y2; y2=rc;
|
||||
}
|
||||
if ( (i & 1) ) {
|
||||
y1=y1+(y2-y1)*(min->x-x1)/(x2-x1);
|
||||
x1=min->x;
|
||||
} else if ( i & 2 ) {
|
||||
y1=y1+(y2-y1)*(max->x-x1)/(x2-x1);
|
||||
x1=max->x;
|
||||
}
|
||||
else if ( i & 4 ) {
|
||||
x1=x1+(x2-x1)*(min->y-y1)/(y2-y1);
|
||||
y1=min->y;
|
||||
}
|
||||
else if ( i & 8 ) {
|
||||
x1=x1+(x2-x1)*(max->y-y1)/(y2-y1);
|
||||
y1=max->y;
|
||||
}
|
||||
if (x1 < min->x ) i=1;
|
||||
else {
|
||||
if (x1 > max->x) i=2;
|
||||
else i=0;
|
||||
}
|
||||
if (y1 < min->y) i |=4;
|
||||
else {
|
||||
if(y1 > max->y) i |=8;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static BOOL check_point1(lpF_PLPOINT min, lpF_PLPOINT max, lpF_PLPOINT p){
|
||||
if ( p->x >= min->x && p->y >= min->y &&
|
||||
p->x <= max->x && p->y <= max->y) return TRUE;
|
||||
return FALSE;
|
||||
}
|
||||
@@ -0,0 +1,471 @@
|
||||
#include "../sg.h"
|
||||
static BOOL Intersect_Spline_By_Spline( lpOBJ obj, hOBJ hspl, short type,
|
||||
lpVDIM data, lpVDIM data1, short sign);
|
||||
|
||||
static OSCAN_COD inter_geo_scan(hOBJ hobj,lpSCAN_CONTROL lpsc);
|
||||
|
||||
static OSCAN_COD inter_line (hOBJ hobj,lpSCAN_CONTROL lpsc);
|
||||
static OSCAN_COD inter_circle (hOBJ hobj,lpSCAN_CONTROL lpsc);
|
||||
static OSCAN_COD inter_arc (hOBJ hobj,lpSCAN_CONTROL lpsc);
|
||||
static OSCAN_COD inter_spline (hOBJ hobj,lpSCAN_CONTROL lpsc);
|
||||
|
||||
static OSCAN_COD (**inter_typeg)(hOBJ obj,lpSCAN_CONTROL lpsc);
|
||||
|
||||
typedef struct {
|
||||
hOBJ hspl;
|
||||
short type;
|
||||
lpVDIM data;
|
||||
lpVDIM data1;
|
||||
} INTER_DAT; //
|
||||
typedef INTER_DAT * lpINTER_DAT;
|
||||
|
||||
BOOL intersection_with_spline( hOBJ hspl, hOBJ hcont, short type,
|
||||
lpVDIM data, lpVDIM data1){
|
||||
SCAN_CONTROL sc;
|
||||
INTER_DAT inter_dat;
|
||||
|
||||
inter_typeg = (OSCAN_COD(**)(hOBJ obj,lpSCAN_CONTROL lpsc))GetMethodArray(OMT_SPLINTER);
|
||||
|
||||
inter_typeg[OLINE] = inter_line; // OLINE
|
||||
inter_typeg[OCIRCLE]= inter_circle; // OCIRCLE
|
||||
inter_typeg[OARC] = inter_arc; // OARC
|
||||
inter_typeg[OSPLINE]= inter_spline; // OSPLINE
|
||||
|
||||
inter_dat.hspl =hspl;
|
||||
inter_dat.type =type;
|
||||
inter_dat.data =data;
|
||||
inter_dat.data1=data1;
|
||||
|
||||
init_scan(&sc);
|
||||
sc.user_geo_scan = inter_geo_scan;
|
||||
reinterpret_cast<lpINTER_DAT &>(sc.data) = &inter_dat;
|
||||
if (o_scan(hcont,&sc) == OSFALSE) return FALSE;
|
||||
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static OSCAN_COD inter_geo_scan(hOBJ hobj,lpSCAN_CONTROL lpsc){
|
||||
return( inter_typeg[lpsc->type](hobj,lpsc));
|
||||
}
|
||||
|
||||
/************************************************
|
||||
* --- OSCAN_COD inter_line ---
|
||||
*
|
||||
************************************************/
|
||||
/**********************************************************
|
||||
* detect intersection spline by line
|
||||
* spl - spline
|
||||
* hline - line
|
||||
* type - type of the returning data
|
||||
* =0 - returns VIM for parameters of intersection points
|
||||
* =1 - returns VIM for intersection points
|
||||
* data - returning VDIM ( type is detected by user )
|
||||
*/
|
||||
#pragma argsused
|
||||
static OSCAN_COD inter_line(hOBJ hobj, lpSCAN_CONTROL lpsc){
|
||||
BOOL rt=OSFALSE;
|
||||
short i, j, j1, n_p, k, l, span_base=60, span_base1=10, span;
|
||||
sgFloat *t_intersec;
|
||||
sgFloat t, t1, t_beg, dist, t_sply, t_line, bound[2], tmp_bound;
|
||||
//------------------------>>
|
||||
lpOBJ obj, obj1;
|
||||
lpGEO_SPLINE gspline;
|
||||
SPLY_DAT sply_dat;
|
||||
//------------------------>>
|
||||
lpGEO_LINE gline;
|
||||
GEO_LINE gline1;
|
||||
D_POINT p, v1, v2, vv2;
|
||||
lpINTER_DAT data = (lpINTER_DAT)(lpsc->data);
|
||||
|
||||
obj1 = (lpOBJ)data->hspl;
|
||||
gspline = (lpGEO_SPLINE)(obj1->geo_data);
|
||||
|
||||
obj = (lpOBJ)hobj;
|
||||
gline = (lpGEO_LINE)obj->geo_data;
|
||||
|
||||
if((t_intersec=(sgFloat*)SGMalloc(100*sizeof(sgFloat))) == NULL ) return OSFALSE;
|
||||
l=0;
|
||||
|
||||
//found beginning point for intersection
|
||||
if( !begin_use_sply( gspline, &sply_dat) ) goto err1;
|
||||
|
||||
for(i=0; i<sply_dat.numk-1; i++ ){
|
||||
t = sply_dat.u[i+1] - sply_dat.u[i];
|
||||
span=span_base + (short)(t*100);
|
||||
t = t/span;
|
||||
memcpy(&v1, &sply_dat.knots[i], sizeof(DA_POINT));
|
||||
bound[0]=sply_dat.u[i];
|
||||
//-------->>>>>>>>>>>>>>>>>>>>
|
||||
for( j=1; j<=span; j++ ){
|
||||
t_beg = sply_dat.u[i]+t*j;
|
||||
bound[1] = tmp_bound = t_beg;
|
||||
if( j==span ) memcpy(&v2, &sply_dat.knots[i+1], sizeof(DA_POINT));
|
||||
else get_point_on_sply(&sply_dat, t_beg, &v2, 0);
|
||||
|
||||
//create line on spline points
|
||||
gline1.v1=v1;
|
||||
gline1.v2=v2;
|
||||
|
||||
//detect intersection of the two lines
|
||||
if( !intersect_3d_ll(gline, 1, &gline1, 1, &p, &n_p) ) goto err2;
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
if ( n_p != 0 ){ // there are points of intersection
|
||||
t_beg=bound[0];
|
||||
t1 = t/span_base1;
|
||||
for( j1=1; j1<=span_base1; j1++ ){
|
||||
t_beg += t1;
|
||||
bound[1]=t_beg;
|
||||
if( j==span_base1 ) memcpy(&vv2, &v2, sizeof(DA_POINT));
|
||||
else get_point_on_sply(&sply_dat, t_beg, &vv2, 0);
|
||||
|
||||
//create line on spline points
|
||||
gline1.v1=v1;
|
||||
gline1.v2=vv2;
|
||||
|
||||
//detect intersection of the two lines
|
||||
if( !intersect_3d_ll(gline, 1, &gline1, 1, &p, &n_p) ) goto err2;
|
||||
|
||||
if ( n_p != 0 ){ // there are points of intersection
|
||||
t_sply=bound[0];
|
||||
if(!Spline_By_Point( &sply_dat, (void *)&p, &t_sply, &dist )) goto err2;
|
||||
t_line = dpoint_distance(&p, &gline->v1)/
|
||||
dpoint_distance(&gline->v2, &gline->v1);
|
||||
|
||||
//call function for calculate real point of intersection
|
||||
if( !Spline_By_Line( &sply_dat, gline,
|
||||
&t_sply, &t_line, bound, &dist ) ) goto err2;
|
||||
// if( dist >= eps_d || t_beg < 0. ) continue; // tested point is out of the spline
|
||||
if( t_sply < 0. ) continue; //tested point out of span
|
||||
|
||||
if( l!=0 ){
|
||||
for( k=0; k<l; k++ )
|
||||
if( fabs( t_intersec[k] - t_sply ) < eps_n )// this point already exists
|
||||
break;
|
||||
if( k>=l ) t_intersec[l++] = t_sply;
|
||||
}else t_intersec[l++] = t_sply;
|
||||
}
|
||||
v1=vv2;
|
||||
bound[0]=bound[1];
|
||||
}
|
||||
}
|
||||
//---------------------------------------------------------------------------
|
||||
v1=v2;
|
||||
bound[0]=tmp_bound;
|
||||
}
|
||||
//-------->>>>>>>>>>>>>>>>>>>>
|
||||
}
|
||||
if( data->type==0 ){ //returns only parameter of intersection points
|
||||
for( i=0; i<l; i++) if( !add_elem( data->data, &t_intersec[i] ) ) goto err2;
|
||||
}else{
|
||||
for( i=0; i<l; i++){
|
||||
get_point_on_sply(&sply_dat, t_intersec[i], &p, 0);
|
||||
if( !add_elem( data->data, &p ) ) goto err2;
|
||||
}
|
||||
}
|
||||
rt=OSTRUE;
|
||||
|
||||
err2:
|
||||
end_use_sply( gspline, &sply_dat );
|
||||
err1:
|
||||
SGFree(t_intersec);
|
||||
return (OSCAN_COD)rt;
|
||||
}
|
||||
|
||||
/************************************************
|
||||
* --- OSCAN_COD inter_spline ---
|
||||
*
|
||||
************************************************/
|
||||
#pragma argsused
|
||||
static OSCAN_COD inter_spline(hOBJ hobj, lpSCAN_CONTROL lpsc){
|
||||
lpINTER_DAT data = (lpINTER_DAT)(lpsc->data);
|
||||
lpOBJ obj;
|
||||
|
||||
obj = (lpOBJ)hobj;
|
||||
return (OSCAN_COD)(Intersect_Spline_By_Spline( obj, data->hspl, data->type,
|
||||
data->data, data->data1, 0 ));
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* detect intersection spline by spline
|
||||
* hspl1 - first spline
|
||||
* hspl2 - second spline
|
||||
* type - type of the returning data
|
||||
* =0 - returns VIM for parameters of intersection points
|
||||
* =1 - returns VIM for intersection points
|
||||
* data - VDIM for points of intersection
|
||||
* data1 - VDIM for parameters of intersection points(in term 2-d spl)
|
||||
* in case of type=0
|
||||
* sign - =0-detect sply_by_sply intersection
|
||||
* =1-detect autointersection
|
||||
*/
|
||||
static BOOL Intersect_Spline_By_Spline( lpOBJ obj, hOBJ hspl, short type,
|
||||
lpVDIM data, lpVDIM data1, short sign){
|
||||
BOOL rt=OSFALSE;
|
||||
short span1, span2, span_base=5;
|
||||
short j, jj, i, ii, n_p, l, k;
|
||||
sgFloat *intersect, *intersect1=NULL;
|
||||
sgFloat t1, t2, t_beg1, t_beg2, dist;
|
||||
sgFloat t_sply1, t_sply2, bound1[2], bound2[2], bound1_tmp, bound2_tmp;
|
||||
//------------------------>>
|
||||
lpOBJ obj1;
|
||||
lpGEO_SPLINE gspline1, gspline2;
|
||||
SPLY_DAT sply_dat1, sply_dat2;
|
||||
//------------------------>>
|
||||
GEO_LINE gline1, gline2;
|
||||
D_POINT p, p1, p2, v1, v2; //, pp2, vv2;
|
||||
|
||||
// spline
|
||||
obj1 = (lpOBJ)hspl;
|
||||
gspline1 = (lpGEO_SPLINE)(obj1->geo_data);
|
||||
gspline2 = (lpGEO_SPLINE)(obj->geo_data);
|
||||
|
||||
if((intersect =(sgFloat*)SGMalloc(100*sizeof(sgFloat))) == NULL ) return OSFALSE;
|
||||
if( type != 0 ) if((intersect1=(sgFloat*)SGMalloc(100*sizeof(sgFloat))) == NULL ) goto err0;
|
||||
l=0;
|
||||
|
||||
//begin
|
||||
if( !begin_use_sply( gspline1, &sply_dat1) ) goto err1;
|
||||
// if( gspline1->type & SPLY_APPR) change_APPR_INT(&sply_dat1);
|
||||
|
||||
if( !begin_use_sply( gspline2, &sply_dat2) ) goto err2;
|
||||
// if( gspline2->type & SPLY_APPR) change_APPR_INT(&sply_dat2);
|
||||
|
||||
//---------------------------------------------------------
|
||||
for(i=0; i<sply_dat1.numk-1; i++ ){ //first spline
|
||||
t1 = sply_dat1.u[i+1] - sply_dat1.u[i]; //take i-th interval
|
||||
span1 = span_base + (short)(t1*100);
|
||||
t1 = t1/span1;
|
||||
memcpy(&v1, &sply_dat1.knots[i], sizeof(DA_POINT));
|
||||
bound1[0]=sply_dat1.u[i]; //--------------->>>>>
|
||||
//---------------------------------------------------------
|
||||
for( j=1; j<=span1; j++ ){ //devide i-th interval into the smaller ones
|
||||
t_beg1 = sply_dat1.u[i]+t1*j;
|
||||
bound1[1]=bound1_tmp=t_beg1;//--------------->>>>>
|
||||
if( j==span1 ) memcpy(&v2, &sply_dat1.knots[i+1], sizeof(DA_POINT));
|
||||
else get_point_on_sply(&sply_dat1, t_beg1, &v2, 0);
|
||||
|
||||
//create line on spline points
|
||||
gline1.v1=v1;
|
||||
gline1.v2=v2;
|
||||
//---------------------------------------------------------
|
||||
for(ii=0; ii<sply_dat2.numk-1; ii++ ){ //second spline
|
||||
if( sign&&ii==i ) continue; // imposible to intersect the same segment(for autointer.)
|
||||
t2 = sply_dat2.u[ii+1] - sply_dat2.u[ii]; //take ii-th interval
|
||||
span2 = span_base + (short)(t2*100);
|
||||
t2 = t2/span2;
|
||||
memcpy(&p1, &sply_dat2.knots[ii], sizeof(DA_POINT));
|
||||
bound2[0]=sply_dat2.u[ii];//--------------->>>>>
|
||||
//---------------------------------------------------------
|
||||
for( jj=1; jj<=span2; jj++ ){ //devide ii-th interval into the smaller ones
|
||||
t_beg2 = sply_dat2.u[ii]+t2*jj;
|
||||
bound2[1]=bound2_tmp=t_beg2;//--------------->>>>>
|
||||
if( jj==span2 ) memcpy(&p2, &sply_dat2.knots[ii+1], sizeof(DA_POINT));
|
||||
else get_point_on_sply(&sply_dat2, t_beg2, &p2, 0);
|
||||
|
||||
//create line on spline points
|
||||
gline2.v1=p1;
|
||||
gline2.v2=p2;
|
||||
//---------------------------------------------------------
|
||||
//detect intersection of the two lines
|
||||
if( !intersect_3d_ll(&gline1, 1, &gline2, 1, &p, &n_p) ) goto err3;
|
||||
|
||||
//---------------------------------------------------------
|
||||
if ( n_p != 0 ){ // there are points of intersection
|
||||
//--------------------------------->>>>>>
|
||||
//call function for calculate real point of intersection
|
||||
t_sply1=bound1[0];
|
||||
// if(!Spline_By_Point( &sply_dat1, (void *)&p, &t_sply1, &dist )) goto err3;
|
||||
t_sply2=bound2[0];
|
||||
// if(!Spline_By_Point( &sply_dat2, (void *)&p, &t_sply2, &dist )) goto err3;
|
||||
|
||||
if( !Spline_By_Spline( &sply_dat1, (void *)&sply_dat2, &t_sply1,
|
||||
&t_sply2, bound1, bound2, &dist ) ) goto err3;
|
||||
// if( dist >= eps_d || t_sply1 < 0.||t_sply2 < 0. ) continue; // tested point is out of the spline
|
||||
|
||||
if( l!=0 ){
|
||||
for( k=0; k<l; k++ )
|
||||
if( fabs( intersect[k] - t_sply1 ) < eps_n )// this point already exists
|
||||
break;
|
||||
if( k>=l ){
|
||||
intersect[l++] = t_sply1;
|
||||
if( type==0 ) intersect1[l-1]=t_sply2;
|
||||
}
|
||||
}else{
|
||||
intersect[l++] = t_sply1;
|
||||
if( type==0 ) intersect1[l-1]=t_sply2;
|
||||
}
|
||||
}
|
||||
//---------------------------------------------------------
|
||||
p1=p2;
|
||||
bound2[0]=bound2_tmp;
|
||||
}
|
||||
}
|
||||
v1=v2;
|
||||
bound1[0]=bound1_tmp;
|
||||
}
|
||||
}
|
||||
if(type==0){
|
||||
for( i=0; i<l; i++){
|
||||
if( !add_elem( data, &intersect[i] ) ) goto err3;
|
||||
if( sign !=1 ) if( !add_elem( data1, &intersect1[i] ) ) goto err3;
|
||||
}
|
||||
}else{
|
||||
for( i=0; i<l; i++){
|
||||
get_point_on_sply(&sply_dat1, intersect[i], &p, 0);
|
||||
if( !add_elem( data, &p ) ) goto err3;
|
||||
}
|
||||
}
|
||||
rt=OSTRUE;
|
||||
|
||||
err3:
|
||||
end_use_sply(gspline1, &sply_dat2);
|
||||
err2:
|
||||
end_use_sply(gspline2, &sply_dat1);
|
||||
err1:
|
||||
if( intersect1 != NULL ) SGFree(intersect1);
|
||||
err0:
|
||||
SGFree(intersect);
|
||||
return rt;
|
||||
}
|
||||
|
||||
/************************************************
|
||||
* --- OSCAN_COD inter_arc ---
|
||||
*
|
||||
************************************************/
|
||||
/**********************************************************
|
||||
* detect intersection spline by arc
|
||||
* hspl - spline
|
||||
* harc - circular arc
|
||||
* type - type of the returning data
|
||||
* =0 - returns VIM for parameters of intersection points
|
||||
* =1 - returns VIM for intersection points
|
||||
* data - returning VDIM ( type is detected by user )
|
||||
*/
|
||||
static OSCAN_COD inter_arc(hOBJ hobj, lpSCAN_CONTROL lpsc){
|
||||
BOOL rt;
|
||||
//------------------------>>
|
||||
hOBJ hspl_arc=NULL;
|
||||
lpOBJ obj;
|
||||
//------------------------>>
|
||||
lpINTER_DAT data = (lpINTER_DAT)(lpsc->data);
|
||||
|
||||
//create new hobj
|
||||
if(!transform_to_NURBS( hobj, &hspl_arc )) return OSFALSE;
|
||||
|
||||
obj = (lpOBJ)hspl_arc;
|
||||
//detect intersection of two splines
|
||||
rt=Intersect_Spline_By_Spline( obj, data->hspl, data->type,
|
||||
data->data, data->data1, 0 );
|
||||
o_free(hspl_arc, NULL);
|
||||
return (OSCAN_COD)rt;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* detect intersection spline by circle
|
||||
* hspl - spline
|
||||
* harc - circle
|
||||
* type - type of the returning data
|
||||
* =0 - returns VIM for parameters of intersection points
|
||||
* =1 - returns VIM for intersection points
|
||||
* data - VDIM for points of intersection
|
||||
*/
|
||||
static OSCAN_COD inter_circle(hOBJ hobj, lpSCAN_CONTROL lpsc){
|
||||
BOOL rt;
|
||||
//------------------------>>
|
||||
hOBJ hspl_circ=NULL;
|
||||
lpOBJ obj;
|
||||
//------------------------>>
|
||||
lpINTER_DAT data = (lpINTER_DAT)(lpsc->data);
|
||||
|
||||
//create new hobj
|
||||
if(!transform_to_NURBS( hobj, &hspl_circ )) return OSFALSE;
|
||||
|
||||
obj = (lpOBJ)hspl_circ;
|
||||
//detect intersection of two splines
|
||||
rt=Intersect_Spline_By_Spline( obj, data->hspl, data->type,
|
||||
data->data, data->data1, 0 );
|
||||
o_free(hspl_circ, NULL);
|
||||
return (OSCAN_COD)rt;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* devide spline by point into two splines
|
||||
* spl - spline
|
||||
* type -=0 point.x is point parameter
|
||||
* =1 - point - point of the deviding
|
||||
* spl_l, spl_r - obtaining splines after the deviding
|
||||
*/
|
||||
BOOL Devide_Spline_By_Point(hOBJ hspl, short type, lpD_POINT point,
|
||||
hOBJ hspl_l, hOBJ hspl_r){
|
||||
BOOL rt=OSFALSE;
|
||||
sgFloat t, dist;
|
||||
lpOBJ obj, obj1;
|
||||
lpGEO_SPLINE gspline;
|
||||
SPLY_DAT sply_dat;
|
||||
lpGEO_SPLINE spline;
|
||||
|
||||
obj = (lpOBJ)hspl;
|
||||
gspline = (lpGEO_SPLINE)(obj->geo_data);
|
||||
|
||||
//begin use spline
|
||||
if( !begin_use_sply( gspline, &sply_dat) ) return OSFALSE;
|
||||
|
||||
//detect parameter for the dividing point
|
||||
if( type==1 ) if( !Spline_By_Point( &sply_dat, point, &t, &dist )) goto err1;
|
||||
else t=point->x;
|
||||
|
||||
//split spline into two parts
|
||||
//first part
|
||||
if((hspl_l = o_alloc(OSPLINE)) == NULL) goto err1;
|
||||
|
||||
obj1 = (lpOBJ)hspl_l;
|
||||
obj1->color = obj->color; //
|
||||
obj1->ltype = obj->ltype; //
|
||||
obj1->lthickness = obj->lthickness; //
|
||||
spline = (lpGEO_SPLINE)(obj1->geo_data);
|
||||
if( !Get_Part_Spline_Geo(&sply_dat, 0, t, spline )){
|
||||
o_free(hspl_l,NULL);
|
||||
goto err1;
|
||||
}
|
||||
copy_obj_attrib(hspl, hspl_l);
|
||||
|
||||
//second part
|
||||
if((hspl_r = o_alloc(OSPLINE)) == NULL) goto err1;
|
||||
|
||||
obj1 = (lpOBJ)hspl_r;
|
||||
obj1->color = obj->color; //
|
||||
obj1->ltype = obj->ltype; //
|
||||
obj1->lthickness = obj->lthickness; //
|
||||
spline = (lpGEO_SPLINE)(obj1->geo_data);
|
||||
if( !Get_Part_Spline_Geo(&sply_dat, t, 0, spline )){
|
||||
o_free(hspl_r,NULL);
|
||||
goto err1;
|
||||
}
|
||||
copy_obj_attrib(hspl, hspl_r);
|
||||
rt=OSTRUE;
|
||||
|
||||
err1:
|
||||
end_use_sply(gspline, &sply_dat);
|
||||
return rt;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* detect autointersection spline
|
||||
* hspl - spline
|
||||
* type - type of the returning data
|
||||
* =0 - returns VIM for parameters of intersection points
|
||||
* =1 - returns VIM for intersection points
|
||||
* data - VDIM for points of intersection
|
||||
*/
|
||||
BOOL AutoIntersect_Spline(hOBJ hspl, short type, lpVDIM data){
|
||||
BOOL rt;
|
||||
hOBJ hobj_n;
|
||||
|
||||
if ( !o_copy_obj(hspl, &hobj_n,"") ) return FALSE;
|
||||
rt=Intersect_Spline_By_Spline((OBJ*)hspl, hobj_n, type, data, NULL, 1);
|
||||
o_free(hobj_n, NULL);
|
||||
return rt;
|
||||
}
|
||||
//----------------------------------------------------->>>>
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
#include "../sg.h"
|
||||
|
||||
/**********************************************************
|
||||
* Spline length foundation
|
||||
*/
|
||||
sgFloat spl_length( lpSPLY_DAT sply_dat ){
|
||||
sgFloat S=0, sigma = c_h_tolerance;
|
||||
D_POINT p1, p2;
|
||||
short i;
|
||||
|
||||
if( sply_dat->degree == 1 ){
|
||||
for( i=0; i<sply_dat->numk-1; i++ )
|
||||
S += dpoint_distance( (lpD_POINT)&sply_dat->knots[i],
|
||||
(lpD_POINT)&sply_dat->knots[i+1] );
|
||||
} else {
|
||||
get_first_sply_point( sply_dat, sigma, FALSE, &p1);
|
||||
// while( get_next_sply_point_tolerance( sply_dat, &p2, &knot ) ){
|
||||
while( get_next_sply_point( sply_dat, &p2 ) ){
|
||||
S += dpoint_distance( &p1, &p2 );
|
||||
p1.x=p2.x; p1.y=p2.y; p1.z=p2.z;
|
||||
}
|
||||
}
|
||||
return S;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Spline length foundation
|
||||
*/
|
||||
sgFloat spl_length_P( lpSPLY_DAT sply_dat ){
|
||||
short i;
|
||||
sgFloat S=0;
|
||||
|
||||
for( i=0; i<sply_dat->nump-1; i++ ) S += dpoint_distance_4dl( &sply_dat->P[i], &sply_dat->P[i+1] );
|
||||
return S;
|
||||
}
|
||||
@@ -0,0 +1,254 @@
|
||||
#include "../sg.h"
|
||||
|
||||
static sgFloat calc_sply_step(lpSPLY_DAT sply_dat, sgFloat t, sgFloat sigma);
|
||||
static void set_interval_data(lpSPLY_DAT sply_dat, lpD_POINT p);
|
||||
static short get_close_interval(lpSPLY_DAT sply_dat);
|
||||
|
||||
static short int_flag; // (//)(1/0/-1)
|
||||
static short first_interval; //
|
||||
static short last_interval; //
|
||||
static short interval; //
|
||||
static short num_int_p; //
|
||||
static short cur_int_p; //
|
||||
static sgFloat s_tolerance; //
|
||||
static sgFloat step_int; //
|
||||
//static sgFloat min_step_size; //
|
||||
static sgFloat length; //
|
||||
|
||||
/**********************************************************
|
||||
*
|
||||
* deriv =0 - get point with parameter t
|
||||
* =1 - first derivates into point with parameter t
|
||||
* =2 - second derivates into point with parameter t
|
||||
*/
|
||||
void get_point_on_sply(lpSPLY_DAT sply_dat, sgFloat t,
|
||||
lpD_POINT p, short deriv){
|
||||
short degree, nnn;
|
||||
//sgFloat weight;
|
||||
//D_POINT point, point1;
|
||||
|
||||
p->x=p->y=p->z=0.;
|
||||
|
||||
if (t < eps_n) t = 0.;
|
||||
if (t > 1 - eps_n) t = 1.;
|
||||
|
||||
if (-eps_n >= t || t >= 1 + eps_n) {
|
||||
memset(p, 0, sizeof(D_POINT));
|
||||
return;
|
||||
}
|
||||
nnn = (sply_dat->sdtype&SPL_INT) ? sply_dat->numk : sply_dat->nump;
|
||||
if( nnn <= 2 ){
|
||||
p->x = sply_dat->knots[0][0]*(1-t)+sply_dat->knots[1][0]*t;
|
||||
p->y = sply_dat->knots[0][1]*(1-t)+sply_dat->knots[1][1]*t;
|
||||
p->z = sply_dat->knots[0][2]*(1-t)+sply_dat->knots[1][2]*t;
|
||||
return;
|
||||
}
|
||||
degree = sply_dat->degree;
|
||||
while( nnn < degree + 1 ) degree--;
|
||||
if( degree <= 0 ) return;
|
||||
|
||||
if( sply_dat->sdtype & SPL_INT ){ // short spline
|
||||
switch( deriv ){
|
||||
case 0: //point with parameter t
|
||||
Calculate_Point( sply_dat, degree, t, p);
|
||||
break;
|
||||
case 1: //firs,second derivates into point with parameter t
|
||||
case 2:
|
||||
Calculate_Deriv( sply_dat, degree, t, p, deriv);
|
||||
break;
|
||||
}
|
||||
}else{ // APPR spline with weight
|
||||
switch( deriv ){
|
||||
case 0://point with parameter t
|
||||
Calculate_Weight_Point(sply_dat, degree, t, p);
|
||||
break;
|
||||
case 1://first derivates into point with parameter t
|
||||
Calculate_Weight_Deriv(sply_dat, degree, t, p, deriv);
|
||||
break;
|
||||
case 2://second derivates into point with parameter t
|
||||
break;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void get_first_sply_point(lpSPLY_DAT sply_dat, sgFloat sigma, BOOL back,
|
||||
lpD_POINT p){
|
||||
short i;
|
||||
sigma = 0.; // : , !
|
||||
// if( sply_dat->degree == 1 ) sigma = 0.;
|
||||
|
||||
if( sigma > 0. ){
|
||||
s_tolerance = sigma; //
|
||||
length = 0;
|
||||
for( i=0; i<sply_dat->numk-1; i++ )
|
||||
length += dpoint_distance( (lpD_POINT)&sply_dat->knots[i],
|
||||
(lpD_POINT)&sply_dat->knots[i+1] );
|
||||
int_flag = ( back ) ? -1 : 1; // /
|
||||
step_int = ( back ) ? 1. : 0.; //
|
||||
cur_int_p = ( back ) ? sply_dat->numk-2 : 1; //
|
||||
memcpy(p, &sply_dat->knots[ cur_int_p-int_flag], sizeof(D_POINT));
|
||||
// get_point_on_sply(sply_dat, step_int, p, 0 ); //
|
||||
// min_step_size = 1./(sply_dat->numk*INTERNAL_SPLINE_POINT);
|
||||
} else {
|
||||
i = sply_dat->numk - 1;
|
||||
get_first_sply_segment_point(sply_dat, sigma, (back)?i:0, (back)?0:i, p);
|
||||
}
|
||||
}
|
||||
|
||||
void get_first_sply_segment_point(lpSPLY_DAT sply_dat, sgFloat sigma,
|
||||
short bknot, short eknot, lpD_POINT p){
|
||||
sigma = 0.; // : , !
|
||||
if(sply_dat->sdtype&SPL_CLOSE){
|
||||
if(abs(eknot - bknot) + 1 >= sply_dat->numk){
|
||||
bknot = 0;
|
||||
eknot = sply_dat->numk - 1;
|
||||
}
|
||||
}else{
|
||||
if(bknot < 0) bknot = 0;
|
||||
if(bknot > 0 && bknot > sply_dat->numk - 1) bknot = sply_dat->numk - 1;
|
||||
if(eknot < 0) eknot = 0;
|
||||
if(eknot > 0 && eknot > sply_dat->numk - 1) eknot = sply_dat->numk - 1;
|
||||
}
|
||||
first_interval = min(bknot, eknot);
|
||||
last_interval = max(bknot, eknot) - 1;
|
||||
int_flag = isg(eknot - bknot);
|
||||
s_tolerance = sigma;
|
||||
|
||||
if(!int_flag)
|
||||
memcpy(p, &sply_dat->knots[bknot], sizeof(D_POINT));
|
||||
else
|
||||
{
|
||||
interval = (int_flag > 0) ? first_interval - 1 : last_interval + 1;
|
||||
set_interval_data(sply_dat, p);
|
||||
}
|
||||
}
|
||||
|
||||
BOOL get_next_sply_point(lpSPLY_DAT sply_dat, lpD_POINT p){
|
||||
BOOL knot;
|
||||
if( s_tolerance > 0. )
|
||||
return get_next_sply_point_tolerance( sply_dat, p, &knot );
|
||||
else return get_next_sply_point_and_knot(sply_dat, p, &knot);
|
||||
}
|
||||
|
||||
BOOL get_next_sply_point_tolerance(lpSPLY_DAT sply_dat, lpD_POINT p, BOOL *knot ){
|
||||
sgFloat t;
|
||||
//------------------------------------------------------------------------------
|
||||
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
||||
return get_next_sply_point_and_knot(sply_dat, p, knot);
|
||||
//------------------------------------------------------------------------------
|
||||
if( int_flag > 0 && step_int >= 1. ) return FALSE; //
|
||||
if( int_flag < 0 && step_int <= 0. ) return FALSE; //
|
||||
*knot = FALSE;
|
||||
if( sply_dat->degree == 1 ){
|
||||
step_int = sply_dat->u[cur_int_p];
|
||||
memcpy(p, &sply_dat->knots[cur_int_p], sizeof(D_POINT));
|
||||
*knot = TRUE;
|
||||
cur_int_p += int_flag;
|
||||
return TRUE;
|
||||
}
|
||||
t = calc_sply_step( sply_dat, step_int, s_tolerance);
|
||||
t /= length;
|
||||
// if( t > min_step_size ) t=min_step_size;
|
||||
t = step_int + int_flag*t;
|
||||
if( t < eps_n ) t = 0.;
|
||||
if( t > 1.-eps_n ) t = 1.;
|
||||
if( fabs( t-sply_dat->u[cur_int_p] )< eps_n ||
|
||||
(t>sply_dat->u[cur_int_p]&&int_flag==1) ||
|
||||
(t<sply_dat->u[cur_int_p]&&int_flag==-1) ){ //
|
||||
step_int = sply_dat->u[cur_int_p];
|
||||
memcpy(p, &sply_dat->knots[cur_int_p], sizeof(D_POINT));
|
||||
*knot = TRUE;
|
||||
cur_int_p += int_flag;
|
||||
}else{
|
||||
step_int = t;
|
||||
get_point_on_sply(sply_dat, step_int, p, 0 ); //
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
BOOL get_next_sply_point_and_knot(lpSPLY_DAT sply_dat, lpD_POINT p, BOOL *knot){
|
||||
short i, ic;
|
||||
|
||||
if(!int_flag) return FALSE; //
|
||||
if(++cur_int_p > num_int_p){
|
||||
set_interval_data(sply_dat, p);
|
||||
*knot = TRUE;
|
||||
}else{
|
||||
i = (int_flag > 0) ? cur_int_p : num_int_p - cur_int_p + 1;
|
||||
ic = get_close_interval(sply_dat);
|
||||
get_point_on_sply(sply_dat, sply_dat->u[ic] + i*step_int, p, FALSE);
|
||||
*knot = FALSE;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void set_interval_data(lpSPLY_DAT sply_dat, lpD_POINT p){
|
||||
short ic;
|
||||
sgFloat t;
|
||||
|
||||
interval += int_flag;
|
||||
ic = get_close_interval(sply_dat);
|
||||
memcpy(p, &sply_dat->knots[(int_flag > 0) ? ic : ic + 1], sizeof(D_POINT));
|
||||
|
||||
if(interval > last_interval || interval < first_interval)
|
||||
int_flag = 0;
|
||||
else{
|
||||
// if(s_tolerance > 0.) num_int_p = calc_sply_step(sply_dat, ic, s_tolerance);
|
||||
// else
|
||||
if( sply_dat->numk > 2 ){
|
||||
/* while(1){
|
||||
if( (t=sply_dat->u[ic+1] - sply_dat->u[ic]) > eps_n) break;
|
||||
interval += int_flag;
|
||||
ic = get_close_interval(sply_dat);
|
||||
memcpy(p, &sply_dat->knots[(int_flag > 0) ? ic : ic + 1], sizeof(D_POINT));
|
||||
|
||||
if(interval > last_interval || interval < first_interval){
|
||||
int_flag = 0;
|
||||
return;
|
||||
}
|
||||
}
|
||||
*/
|
||||
t=sply_dat->u[ic+1] - sply_dat->u[ic];
|
||||
num_int_p = INTERNAL_SPLINE_POINT+(short)(t*100);
|
||||
step_int = t/(num_int_p +1);
|
||||
} else {
|
||||
num_int_p = INTERNAL_SPLINE_POINT;
|
||||
step_int = 1./(num_int_p +1);
|
||||
}
|
||||
cur_int_p = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static sgFloat calc_sply_step(lpSPLY_DAT sply_dat, sgFloat t, sgFloat sigma){
|
||||
sgFloat ro, r1, r2, r3;
|
||||
D_POINT p_1, p_2;
|
||||
|
||||
get_point_on_sply( sply_dat, t, &p_1, 1 );
|
||||
get_point_on_sply( sply_dat, t, &p_2, 2 );
|
||||
//ro( )= 1/
|
||||
r1 = dskalar_product( &p_1, &p_1 );
|
||||
r2 = dskalar_product( &p_2, &p_2 );
|
||||
r3 = dskalar_product( &p_1, &p_2 );
|
||||
ro = r1*sqrt(r1);
|
||||
r1 = r1*r2 - r3*r3;
|
||||
if( r1 <= eps_n ) return ( 2*sigma );
|
||||
else{
|
||||
ro= ro/sqrt( r1 );
|
||||
if( (r1 = 2*ro-sigma ) <= eps_n ) return ( sqrt(8*sigma*ro) );
|
||||
else return ( sqrt(4*sigma*(2*ro-sigma)) );
|
||||
}
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
*
|
||||
*/
|
||||
static short get_close_interval(lpSPLY_DAT sply_dat){
|
||||
short num;
|
||||
if(sply_dat->sdtype & SPL_CLOSE){
|
||||
num = sply_dat->numk - 1;
|
||||
if(interval < 0) return interval + num;
|
||||
if(interval >= num) return interval - num;
|
||||
}
|
||||
return interval;
|
||||
}
|
||||
@@ -0,0 +1,618 @@
|
||||
#include "../sg.h"
|
||||
static BOOL check_spl_max(lpSPLY_DAT spl, short num, short degree );
|
||||
|
||||
BOOL begin_use_sply(lpGEO_SPLINE geo_sply, lpSPLY_DAT sply_dat){
|
||||
short i, j, type;
|
||||
sgFloat *u;//, w;
|
||||
lpDA_POINT p;
|
||||
lpSNODE d;
|
||||
|
||||
//
|
||||
memset(sply_dat, 0, sizeof(SPLY_DAT));
|
||||
|
||||
if (geo_sply->type & SPLY_NURBS){ // NURBS
|
||||
if ( geo_sply->type & SPLY_APPR ) {//
|
||||
//
|
||||
type = SPL_APPR;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
//
|
||||
if((sply_dat->knots = (DA_POINT*)SGMalloc(geo_sply->nump*sizeof(DA_POINT))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_knots = geo_sply->nump;
|
||||
if((sply_dat->P = (W_NODE *)SGMalloc(geo_sply->nump*sizeof(W_NODE ))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_P = geo_sply->nump;
|
||||
|
||||
//
|
||||
if((sply_dat->u = (sgFloat *)SGMalloc((geo_sply->nump)*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_u = geo_sply->nump;
|
||||
|
||||
|
||||
//
|
||||
p = (lpDA_POINT)geo_sply->hpoint;
|
||||
d = (lpSNODE)geo_sply->hderivates;
|
||||
//
|
||||
memcpy(sply_dat->knots, p, geo_sply->nump*sizeof(DA_POINT));
|
||||
|
||||
for( i=0; i<geo_sply->nump; i++ ){
|
||||
Create_4D_From_3D( &sply_dat->P[i], (sgFloat *)&p[i], d[i].p[1] );
|
||||
sply_dat->u[i]=d[i].p[0]; //
|
||||
}
|
||||
|
||||
sply_dat->nump = sply_dat->numk = geo_sply->nump;
|
||||
sply_dat->numd = 0;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
//
|
||||
if( geo_sply->numu )
|
||||
i = geo_sply->numu;
|
||||
else
|
||||
i = geo_sply->nump+geo_sply->degree+1;
|
||||
|
||||
if((sply_dat->U = (sgFloat *)SGMalloc(i*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_U = i;
|
||||
|
||||
|
||||
//
|
||||
if( geo_sply->numu )
|
||||
{
|
||||
u = (sgFloat*)geo_sply->hvector;
|
||||
memcpy(sply_dat->U, u, geo_sply->numu*sizeof(sgFloat));
|
||||
sply_dat->numU = i;
|
||||
}
|
||||
} else { //
|
||||
//
|
||||
type = SPL_INT;
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------------
|
||||
//
|
||||
if((sply_dat->knots = (DA_POINT*)SGMalloc(geo_sply->nump*sizeof(DA_POINT))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_knots = geo_sply->nump;
|
||||
|
||||
|
||||
//
|
||||
if((sply_dat->u = (sgFloat *)SGMalloc((geo_sply->nump)*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_u = geo_sply->nump;
|
||||
|
||||
|
||||
//
|
||||
p = (lpDA_POINT)geo_sply->hpoint;
|
||||
memcpy(sply_dat->knots, p, geo_sply->nump*sizeof(DA_POINT));
|
||||
sply_dat->numk = geo_sply->nump;
|
||||
|
||||
// -
|
||||
i=geo_sply->numd+geo_sply->nump;
|
||||
if(geo_sply->type & SPLY_CLOSE) i+=2;
|
||||
if((sply_dat->P = (W_NODE *)SGMalloc(i*sizeof(W_NODE))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_P = i;
|
||||
|
||||
|
||||
// == 1.
|
||||
for( j=0; j<i; j++ ) sply_dat->P[j].vertex[3]=1.;
|
||||
sply_dat->nump = i;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------
|
||||
//
|
||||
if((sply_dat->derivates = (SNODE *)SGMalloc(geo_sply->nump*sizeof(SNODE ))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_derivates = geo_sply->nump;
|
||||
|
||||
|
||||
//
|
||||
if( geo_sply->numd ){
|
||||
d = (lpSNODE)geo_sply->hderivates;
|
||||
for (i = 0; i < geo_sply->numd; i++) {
|
||||
if (0 <= d[i].num && d[i].num < geo_sply->numd) {
|
||||
memcpy(sply_dat->derivates[d[i].num].p, d[i].p, sizeof(DA_POINT));
|
||||
sply_dat->derivates[d[i].num].num = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
sply_dat->numd = geo_sply->numd;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
//
|
||||
if( geo_sply->numu ) i = geo_sply->numu;
|
||||
else i = 2*(geo_sply->nump+geo_sply->degree+1);
|
||||
if((sply_dat->U = (sgFloat *)SGMalloc(i*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_U = i;
|
||||
|
||||
|
||||
//
|
||||
if( geo_sply->numu ){
|
||||
u = (sgFloat*)geo_sply->hvector;
|
||||
memcpy(sply_dat->U, u, geo_sply->numu*sizeof(sgFloat));
|
||||
sply_dat->numU = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if( geo_sply->type & SPLY_CLOSE ) sply_dat->sdtype = SPL_CLOSE;
|
||||
//
|
||||
if( !init_sply_dat( SPL_GEO, geo_sply->degree, type, sply_dat)) goto err1;
|
||||
return TRUE;
|
||||
|
||||
err:
|
||||
nurbs_handler_err(SPL_NO_MEMORY);
|
||||
err1:
|
||||
free_sply_dat(sply_dat);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
*
|
||||
* , sply_dat
|
||||
*/
|
||||
#pragma argsused
|
||||
void end_use_sply(lpGEO_SPLINE geo_sply, lpSPLY_DAT sply_dat){
|
||||
free_sply_dat( sply_dat );
|
||||
}
|
||||
|
||||
/*********************************************************
|
||||
* sply_dat
|
||||
*/
|
||||
BOOL init_sply_dat(char sdtype, short degree, short type, lpSPLY_DAT sply_dat ){
|
||||
short num_point, i, condition;
|
||||
|
||||
if( sdtype & SPL_GEO ){ // ,
|
||||
//
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | sdtype); //
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | type); ///
|
||||
|
||||
// num_point = sply_dat->numk;
|
||||
if(sply_dat->sdtype & SPL_CLOSE){
|
||||
if( sply_dat->numd == 0 ) condition=1; // closed without derivation
|
||||
else{
|
||||
if( sply_dat->derivates[0].num == 1 ) condition=2; //special case of closed spline
|
||||
else condition=3; // closed with derivates
|
||||
}
|
||||
}else{
|
||||
if( sply_dat->numd == 0 ) condition=0; // without derivation
|
||||
else condition=2; //with derivates
|
||||
}
|
||||
} else { // .
|
||||
memset(sply_dat, 0, sizeof(SPLY_DAT));
|
||||
num_point = MAX_POINT_ON_SPLINE;
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | sdtype); //
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | SPL_FREE); //
|
||||
condition=0;
|
||||
|
||||
if ( type & SPL_APPR ) { //
|
||||
if((sply_dat->knots = (DA_POINT*)SGMalloc(num_point*sizeof(DA_POINT))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_knots = num_point;
|
||||
if((sply_dat->P = (W_NODE *)SGMalloc(num_point*sizeof(W_NODE ))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_P = num_point;
|
||||
if((sply_dat->U = (sgFloat *)SGMalloc((num_point+degree+1)*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_U = num_point+degree+1;
|
||||
for (i=0; i<num_point; i++) sply_dat->P[i].vertex[3] = 1.;//
|
||||
|
||||
} else { //
|
||||
if((sply_dat->knots = (DA_POINT*)SGMalloc(num_point*sizeof(DA_POINT))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_knots = num_point;
|
||||
if((sply_dat->derivates = (SNODE *)SGMalloc(num_point*sizeof(SNODE))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_derivates = num_point;
|
||||
if((sply_dat->P = (W_NODE *)SGMalloc((2*num_point+2)*sizeof(W_NODE))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_P = 2*num_point+2;
|
||||
if((sply_dat->U = (sgFloat *)SGMalloc((2*(num_point+degree+1))*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_U = 2*(num_point+degree+1);
|
||||
|
||||
for (i = 0; i < num_point; i++) sply_dat->P[i].vertex[3] = 1.;
|
||||
}
|
||||
//
|
||||
if((sply_dat->u = (sgFloat *)SGMalloc((num_point)*sizeof(sgFloat))) == NULL) goto err;
|
||||
sply_dat->allocSizeFor_u = num_point;
|
||||
|
||||
}
|
||||
sply_dat->degree = degree;
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | type); ///
|
||||
|
||||
if( sply_dat->numk > 2 ){
|
||||
if( sply_dat->sdtype & SPL_INT ){
|
||||
if( sply_dat->numU ) parameter( sply_dat, condition, FALSE );
|
||||
else{
|
||||
sply_dat->numU = 2*(sply_dat->nump+sply_dat->degree+1); ;
|
||||
parameter( sply_dat, condition, TRUE );
|
||||
}
|
||||
}
|
||||
calculate_P(sply_dat, condition);
|
||||
}
|
||||
return TRUE;
|
||||
err:
|
||||
free_sply_dat( sply_dat );
|
||||
nurbs_handler_err(SPL_NO_MEMORY);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
void free_sply_dat( lpSPLY_DAT sply_dat){
|
||||
if (sply_dat->knots) {
|
||||
SGFree(sply_dat->knots);
|
||||
sply_dat->knots = NULL;
|
||||
sply_dat->allocSizeFor_knots = 0;
|
||||
}
|
||||
if (sply_dat->derivates) {
|
||||
SGFree(sply_dat->derivates);
|
||||
sply_dat->derivates = NULL;
|
||||
sply_dat->allocSizeFor_derivates = 0;
|
||||
}
|
||||
if (sply_dat->P) {
|
||||
SGFree(sply_dat->P);
|
||||
sply_dat->P = NULL;
|
||||
sply_dat->allocSizeFor_P = 0;
|
||||
}
|
||||
if (sply_dat->U) {
|
||||
SGFree(sply_dat->U);
|
||||
sply_dat->U = NULL;
|
||||
sply_dat->allocSizeFor_U = 0;
|
||||
}
|
||||
if (sply_dat->u) {
|
||||
SGFree(sply_dat->u);
|
||||
sply_dat->u = NULL;
|
||||
sply_dat->allocSizeFor_u = 0;
|
||||
}
|
||||
}
|
||||
|
||||
BOOL create_geo_sply(lpSPLY_DAT sply_dat, lpGEO_SPLINE geo_sply){
|
||||
short i, j;
|
||||
sgFloat *u;
|
||||
lpDA_POINT p;
|
||||
lpSNODE d;
|
||||
|
||||
if( sply_dat->numk < sply_dat->degree+1 ){
|
||||
put_message(SPL_EMPTY_DATA, NULL, 0);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
// ,
|
||||
memset( geo_sply, 0, sizeof(GEO_SPLINE));
|
||||
|
||||
//
|
||||
geo_sply->degree = sply_dat->degree;
|
||||
|
||||
//
|
||||
geo_sply->type = SPLY_NURBS;
|
||||
if( sply_dat->sdtype & SPL_APPR ) geo_sply->type |= SPLY_APPR;
|
||||
if( sply_dat->sdtype & SPL_CLOSE) geo_sply->type |= SPLY_CLOSE;
|
||||
|
||||
if( sply_dat->sdtype & SPL_APPR ){ //
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------------
|
||||
if((geo_sply->hpoint =
|
||||
SGMalloc(sply_dat->nump*sizeof(DA_POINT))) == NULL) return FALSE;
|
||||
p = (lpDA_POINT)geo_sply->hpoint;
|
||||
if((geo_sply->hderivates =
|
||||
SGMalloc(sply_dat->nump*sizeof(SNODE))) == NULL) return FALSE;
|
||||
d = (lpSNODE)geo_sply->hderivates;
|
||||
|
||||
for( i=0; i<sply_dat->nump; i++ )
|
||||
{
|
||||
if (i<sply_dat->allocSizeFor_P)
|
||||
Create_3D_From_4D( &sply_dat->P[i], (sgFloat *)&p[i], &d[i].p[1]);
|
||||
else
|
||||
{
|
||||
assert(0);
|
||||
}
|
||||
|
||||
if (i<sply_dat->allocSizeFor_u)
|
||||
d[i].p[0]=sply_dat->u[i]; //
|
||||
else
|
||||
{
|
||||
assert(0);
|
||||
}
|
||||
}
|
||||
|
||||
geo_sply->nump = geo_sply->numd = sply_dat->nump;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
if((geo_sply->hvector =
|
||||
SGMalloc(sply_dat->numU*sizeof(sgFloat))) == NULL) return FALSE;
|
||||
u = (sgFloat*)geo_sply->hvector;
|
||||
memcpy( u, sply_dat->U, sply_dat->numU*sizeof(sgFloat));
|
||||
geo_sply->numu = sply_dat->numU;
|
||||
|
||||
}else{ //
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------------
|
||||
if((geo_sply->hpoint =
|
||||
SGMalloc(sply_dat->numk*sizeof(DA_POINT))) == NULL) return FALSE;
|
||||
p = (lpDA_POINT)geo_sply->hpoint;
|
||||
memcpy( p, sply_dat->knots, sply_dat->numk*sizeof(DA_POINT));
|
||||
geo_sply->nump = sply_dat->numk;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------
|
||||
if( sply_dat->numd ){
|
||||
if((geo_sply->hderivates =
|
||||
SGMalloc(sply_dat->numd*sizeof(SNODE))) == NULL) return FALSE;
|
||||
d = (lpSNODE)geo_sply->hderivates;
|
||||
for( j=0, i=0; i<sply_dat->numk; i++ )
|
||||
if( sply_dat->derivates[i].num ){
|
||||
memcpy( d[j].p, sply_dat->derivates[i].p, sizeof(DA_POINT));
|
||||
d[j++].num = i;
|
||||
}
|
||||
}
|
||||
geo_sply->numd = sply_dat->numd;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
if((geo_sply->hvector =
|
||||
SGMalloc(sply_dat->numU*sizeof(sgFloat))) == NULL) return FALSE;
|
||||
u = (sgFloat*)geo_sply->hvector;
|
||||
memcpy( u, sply_dat->U, sply_dat->numU*sizeof(sgFloat));
|
||||
geo_sply->numu = sply_dat->numU;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
BOOL unpack_geo_sply(lpGEO_SPLINE geo_sply, lpSPLY_DAT sply_dat){
|
||||
short i, condition;
|
||||
sgFloat *u;
|
||||
lpDA_POINT p;
|
||||
lpSNODE d;
|
||||
|
||||
if(sply_dat->sdtype & SPL_GEO) {
|
||||
put_message(SPL_INTERNAL_ERROR, NULL, 0);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
if(geo_sply->type & SPLY_NURBS)
|
||||
{
|
||||
if( (geo_sply->type & SPLY_APPR) != 0 )
|
||||
{//
|
||||
sply_dat->sdtype = SPL_APPR;
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------
|
||||
p = (lpDA_POINT)geo_sply->hpoint;
|
||||
d = (lpSNODE)geo_sply->hderivates;
|
||||
// RA-BEGIN
|
||||
if (geo_sply->nump>sply_dat->allocSizeFor_knots &&
|
||||
sply_dat->knots)
|
||||
{
|
||||
SGFree(sply_dat->knots);
|
||||
sply_dat->knots = (lpDA_POINT)SGMalloc(geo_sply->nump*sizeof(DA_POINT));
|
||||
sply_dat->allocSizeFor_knots = geo_sply->nump;
|
||||
}
|
||||
if (geo_sply->nump>sply_dat->allocSizeFor_P &&
|
||||
sply_dat->P)
|
||||
{
|
||||
SGFree(sply_dat->P);
|
||||
sply_dat->P = (W_NODE*)SGMalloc(geo_sply->nump*sizeof(W_NODE));
|
||||
sply_dat->allocSizeFor_P = geo_sply->nump;
|
||||
}
|
||||
if (geo_sply->nump>sply_dat->allocSizeFor_u &&
|
||||
sply_dat->u)
|
||||
{
|
||||
SGFree(sply_dat->u);
|
||||
sply_dat->u = (sgFloat*)SGMalloc(geo_sply->nump*sizeof(sgFloat));
|
||||
sply_dat->allocSizeFor_u = geo_sply->nump;
|
||||
}
|
||||
// RA - END
|
||||
memcpy(sply_dat->knots, p, geo_sply->nump*sizeof(DA_POINT));
|
||||
|
||||
for( i=0; i<geo_sply->nump; i++ )
|
||||
{
|
||||
Create_4D_From_3D( &sply_dat->P[i], (sgFloat *)&p[i], d[i].p[1] );
|
||||
sply_dat->u[i]=d[i].p[0]; //
|
||||
}
|
||||
|
||||
sply_dat->nump = sply_dat->numk = geo_sply->nump;
|
||||
sply_dat->numd = 0;
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
if( geo_sply->numu )
|
||||
{
|
||||
// RA-BEGIN
|
||||
if (geo_sply->numu>sply_dat->allocSizeFor_U &&
|
||||
sply_dat->U)
|
||||
{
|
||||
SGFree(sply_dat->U);
|
||||
sply_dat->U =(sgFloat*)SGMalloc(geo_sply->numu*sizeof(sgFloat));
|
||||
sply_dat->allocSizeFor_U = geo_sply->numu;
|
||||
}
|
||||
|
||||
// RA - END
|
||||
u = (sgFloat*)geo_sply->hvector;
|
||||
memcpy(sply_dat->U, u, geo_sply->numu*sizeof(sgFloat));
|
||||
sply_dat->numU = geo_sply->numu;
|
||||
}
|
||||
}
|
||||
else
|
||||
{ //
|
||||
sply_dat->sdtype = SPL_INT;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------------
|
||||
// RA-BEGIN
|
||||
if (geo_sply->nump>sply_dat->allocSizeFor_knots &&
|
||||
sply_dat->knots)
|
||||
{
|
||||
SGFree(sply_dat->knots);
|
||||
sply_dat->knots = (lpDA_POINT)SGMalloc(geo_sply->nump*sizeof(DA_POINT));
|
||||
sply_dat->allocSizeFor_knots = geo_sply->nump;
|
||||
}
|
||||
// RA - END
|
||||
p = (lpDA_POINT)geo_sply->hpoint;
|
||||
memcpy(sply_dat->knots, p, geo_sply->nump*sizeof(DA_POINT));
|
||||
sply_dat->numk = geo_sply->nump;
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<--------------------
|
||||
if( geo_sply->numd )
|
||||
{
|
||||
d = (lpSNODE)geo_sply->hderivates;
|
||||
for(i=0; i<sply_dat->numd; i++ )
|
||||
{
|
||||
if (d[i].num<sply_dat->allocSizeFor_derivates)
|
||||
{
|
||||
memcpy(sply_dat->derivates[d[i].num].p, d[i].p, sizeof(DA_POINT));
|
||||
sply_dat->derivates[d[i].num].num = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
assert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
sply_dat->numd = geo_sply->numd;
|
||||
|
||||
// ---------------->>>>>>>>> <<<<<<<<<<-------------
|
||||
if( geo_sply->numu )
|
||||
{
|
||||
u = (sgFloat*)geo_sply->hvector;
|
||||
if (geo_sply->numu<sply_dat->allocSizeFor_U)
|
||||
{
|
||||
memcpy(sply_dat->U, u, geo_sply->numu*sizeof(sgFloat));
|
||||
sply_dat->numU = geo_sply->numu;
|
||||
}
|
||||
else
|
||||
{
|
||||
assert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(geo_sply->type & SPLY_CLOSE)
|
||||
{
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | SPL_CLOSE);
|
||||
if( sply_dat->numd > 0 )
|
||||
{
|
||||
if( sply_dat->derivates[0].num == 1 )
|
||||
condition=2;
|
||||
else
|
||||
condition=3;
|
||||
}else
|
||||
condition=1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sply_dat->sdtype = (SPLY_TYPE_IN)(sply_dat->sdtype | SPL_FREE);
|
||||
if( sply_dat->numd > 0 )
|
||||
condition=2;
|
||||
else
|
||||
condition=0;
|
||||
}
|
||||
|
||||
sply_dat->degree=geo_sply->degree;
|
||||
|
||||
if( sply_dat->numk > 2 )
|
||||
{
|
||||
if( sply_dat->sdtype & SPL_INT )
|
||||
{
|
||||
if( sply_dat->numU )
|
||||
parameter( sply_dat, condition, FALSE );
|
||||
else
|
||||
{
|
||||
sply_dat->numU = geo_sply->nump+geo_sply->degree+1;
|
||||
parameter( sply_dat, condition, TRUE );
|
||||
}
|
||||
}
|
||||
calculate_P(sply_dat, condition); //
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* expand memory for spline
|
||||
* num - number of add element
|
||||
* degree - degree of curve
|
||||
* type = 0 - add point
|
||||
* 1 - add derivation
|
||||
* 2 - add knot
|
||||
*/
|
||||
BOOL expand_spl( lpSPLY_DAT spline, short num, short degree, short type ){
|
||||
UINT size;
|
||||
VLD vld;
|
||||
VI_LOCATION loc;
|
||||
|
||||
if ( !check_spl_max( spline, num, degree) ) return FALSE;
|
||||
init_vld(&vld);
|
||||
|
||||
// Save information
|
||||
if( spline->P && spline->nump )
|
||||
if( !add_vld_data( &vld, sizeof(W_NODE)*(spline->nump), &spline->P[0])) goto err;
|
||||
|
||||
if( spline->U && spline->numU )
|
||||
if( !add_vld_data( &vld, sizeof(sgFloat)*(spline->numU), &spline->U[0])) goto err;
|
||||
|
||||
if( type == 0 || type == 2 ){
|
||||
if( spline->knots && spline->numk )
|
||||
if( !add_vld_data( &vld, sizeof(DA_POINT)*(spline->numk), &spline->knots[0])) goto err;
|
||||
|
||||
if( spline->u && spline->numk )
|
||||
if( !add_vld_data( &vld, sizeof(sgFloat)*(spline->numk), &spline->u[0])) goto err;
|
||||
|
||||
if( spline->derivates && spline->numd ){
|
||||
if( !add_vld_data( &vld, sizeof(SNODE)*(spline->numk), &spline->derivates[0])) goto err;}
|
||||
}
|
||||
|
||||
//
|
||||
if( spline->P ){ SGFree(spline->P); spline->P = NULL; spline->allocSizeFor_P=0;}
|
||||
if( spline->U ){ SGFree(spline->U); spline->U = NULL; spline->allocSizeFor_U=0;}
|
||||
if( type == 0 || type == 2 ){
|
||||
if( spline->knots){ SGFree(spline->knots); spline->knots = NULL; spline->allocSizeFor_knots=0;}
|
||||
if( spline->u) { SGFree(spline->u); spline->u = NULL; spline->allocSizeFor_u=0;}
|
||||
if( spline->derivates ){ SGFree(spline->derivates); spline->derivates = NULL; spline->allocSizeFor_derivates=0;}
|
||||
}
|
||||
|
||||
// Alloc memory
|
||||
size = sizeof(W_NODE)*(MAX_POINT_ON_SPLINE+num);
|
||||
if( ( spline->P = (W_NODE *)SGMalloc(size)) == NULL ) goto err1;
|
||||
spline->allocSizeFor_P=MAX_POINT_ON_SPLINE+num;
|
||||
|
||||
size = sizeof(sgFloat)*(MAX_POINT_ON_SPLINE+num+degree+1);
|
||||
if( ( spline->U = (sgFloat *)SGMalloc(size)) == NULL ) goto err1;
|
||||
spline->allocSizeFor_U=MAX_POINT_ON_SPLINE+num+degree+1;
|
||||
|
||||
if( type == 0 || type == 2 ){
|
||||
size = sizeof(DA_POINT)*(MAX_POINT_ON_SPLINE+num);
|
||||
if( ( spline->knots = (DA_POINT*)SGMalloc(size)) == NULL ) goto err1;
|
||||
spline->allocSizeFor_knots=MAX_POINT_ON_SPLINE+num;
|
||||
|
||||
size = sizeof(sgFloat)*(MAX_POINT_ON_SPLINE+num);
|
||||
if( ( spline->u = (sgFloat *)SGMalloc(size)) == NULL ) goto err1;
|
||||
spline->allocSizeFor_u=MAX_POINT_ON_SPLINE+num;
|
||||
|
||||
if( type == 0 ){
|
||||
size = sizeof(SNODE)*(MAX_POINT_ON_SPLINE+num);
|
||||
if( ( spline->derivates = (SNODE *)SGMalloc(size)) == NULL ) goto err1;
|
||||
spline->allocSizeFor_derivates=MAX_POINT_ON_SPLINE+num;
|
||||
}
|
||||
}
|
||||
|
||||
// rearrange the spline
|
||||
loc.hpage = vld.listh.hhead;
|
||||
loc.offset = sizeof(RAW);
|
||||
begin_read_vld(&loc);
|
||||
|
||||
size = sizeof(W_NODE)*(spline->nump);
|
||||
read_vld_data(size,&spline->P[0]);
|
||||
|
||||
size = sizeof(sgFloat)*(spline->numU);
|
||||
read_vld_data(size,&spline->U[0]);
|
||||
|
||||
if( type == 0 || type == 1 ){
|
||||
size = sizeof(DA_POINT)*(spline->numk);
|
||||
read_vld_data(size,&spline->knots[0]);
|
||||
|
||||
size = sizeof(sgFloat)*(spline->numk);
|
||||
read_vld_data(size,&spline->u[0]);
|
||||
|
||||
if( type == 0 ){
|
||||
size = sizeof(SNODE)*(spline->numk);
|
||||
read_vld_data(size,&spline->derivates[0]);
|
||||
}
|
||||
}
|
||||
end_read_vld();
|
||||
free_vld_data(&vld);
|
||||
|
||||
return TRUE;
|
||||
err:
|
||||
nurbs_handler_err(SPL_NO_MEMORY);
|
||||
free_vld_data(&vld);
|
||||
return FALSE;
|
||||
err1:
|
||||
nurbs_handler_err(SPL_NO_HEAP);
|
||||
free_vld_data(&vld);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
static BOOL check_spl_max(lpSPLY_DAT spl, short num, short degree ){
|
||||
if ( sizeof(*spl->P)*(long)(spl->nump+num) >= 65535L ) goto err;
|
||||
if ( sizeof(*spl->U)*(long)(spl->nump+degree+num+1) >= 65535L ) goto err;
|
||||
if ( sizeof(*spl->knots)*(long)(spl->numk+num) >= 65535L ) goto err;
|
||||
if ( sizeof(*spl->u)*(long)(spl->numk+num) >= 65535L ) goto err;
|
||||
if ( sizeof(*spl->derivates)*(long)(spl->numk+num) >= 65535L ) goto err;
|
||||
return TRUE;
|
||||
err:
|
||||
nurbs_handler_err(SPL_MAX);
|
||||
return FALSE;
|
||||
}
|
||||
Reference in New Issue
Block a user