mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-05 05:07:48 +00:00
374 lines
11 KiB
C++
374 lines
11 KiB
C++
#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 ) );
|
|
}
|