matrix.cpp

Hi guys, I have a B-spline curve algorithm by using Genetic algorithm. Any interested?

    bmatcb (t1,ns,aitemp2,vtx,x1); 
	for(k=0;k<3;k++){
		d1[k]=x1[k]/del[ns-1];
	} // k 

    t2[0]=0.0; 
    t2[1]=0.0;  
    t2[2]=2.0;
    t2[3]=6.0*tl; 

    bmatcb (t2,ns,aitemp2,vtx,x2); 
	for(k=0;k<3;k++){
		d2[k]=x2[k]/del[ns-1]/del[ns-1];
	} // k 

    a[0]=d1[1]*d2[2]-d1[2]*d2[1];
    a[1]=d1[2]*d2[0]-d1[0]*d2[2];
    a[2]=d1[0]*d2[1]-d1[1]*d2[0];

    a1=sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
    a2=sqrt(d1[0]*d1[0]+d1[1]*d1[1]+d1[2]*d1[2]);
    a3=(a2*a2*a2);
    if (fabs(a3)<tiny) goto i_999; 
    *cm=a1/a3;

    return istat;
i_999:
	istat=1;
	return istat;
}
//----------------------------------------------------------------------*
int crossp(float aaa[3],float bbb[3],float ccc[3])
{
	float ddd;
	int istat;
	istat=0;

	ccc[0]=aaa[1]*bbb[2]-aaa[2]*bbb[1];
	ccc[1]=aaa[2]*bbb[0]-aaa[0]*bbb[2];
	ccc[2]=aaa[0]*bbb[1]-aaa[1]*bbb[0];

	ddd=sqrt(ccc[0]*ccc[0]+ccc[1]*ccc[1]+ccc[2]*ccc[2]);
	if(ddd==0.0) goto i_999;

	ccc[0]=ccc[0]/ddd;
	ccc[1]=ccc[1]/ddd;
	ccc[2]=ccc[2]/ddd;
	return istat;
i_999:
	istat=1;
	return istat;
}
//----------------------------------------------------------------------*
void gpa2lpa(int kord,int npts,float xv[],float tpa,int *nseg,float *tl) 
{
	int i,k;
	float dd;

    *nseg=0;
    k=kord-1; 
	for(i=0;i<npts-1;i++){
        dd=(xv[k+i+1]-tpa)*(tpa-xv[k+i]); 
		if(dd>=0.0){
			*nseg=i+1; 
			*tl=(tpa-xv[k+i])/(xv[k+i+1]-xv[k+i]); 
          return ;
        } //endif
	} //i
    return ;
}
//----------------------------------------------------------------------*

void bmatcb(float t[],int ns,float aitemp3[][4][4],float vtx[][3],float x[]) 
{
	int j,k;
	float s[4];

	for(k=0;k<4;k++){
        s[k]=0.0;
		for(j=0;j<4;j++){
			s[k]=s[k]+t[j]*aitemp3[ns-1][k][j];
		} //j
	} //k
	for(k=0;k<3;k++){
		x[k]=0.0;
		for(j=0;j<4;j++){
			x[k]=x[k]+s[j]*vtx[ns+j-1][k];
		} //j
	} //k
}
//----------------------------------------------------------------------*
void mkt0(float d[],int npts,float s0[3],float s1[3])
{
	float x[4][4],r[3],dl1[2],dm1[2],dn1[2];
	float q1[3],q2[3];
	float d1,d2,s,ss2,r20;
	int ierr,is;
	int i,j,k,ii,kk;

/*	if(npts<3) {
		q1[0]=d[0];
		q1[1]=d[1];
		q1[2]=d[2];

		q2[0]=d[3];
		q2[1]=d[4];
		q2[2]=d[5];
		linvec(q1,q2,s0,s1);
		return;
	}*/

	for(is=1;is<=2;is++){
		for(i=1;i<=3;i++){
			if(is==1){
				ii=i;
			}else{
				ii=npts-3+i;
			}

		kk=(ii-1)*3;

		for(j=1;j<=3;j++) x[j-1][i-1]=d[kk+j-1];

		} //i
// 210

	d1=   pow( (x[0][1]-x[0][0]) , 2.) 
		+ pow( (x[1][1]-x[1][0]) , 2.) 
		+ pow( (x[2][1]-x[2][0]) , 2.) ;

	d2=   pow( (x[0][2]-x[0][1]) , 2.) 
		+ pow( (x[1][2]-x[1][1]) , 2.) 
		+ pow( (x[2][2]-x[2][1]) , 2.) ; 
	d1=sqrt(d1);
	d2=sqrt(d2);

	s=d1/(d1+d2);
	ss2=s-s*s;

	if(is==1) {
		for(j=1;j<=3;j++){
			r20=x[j-1][2]-x[j-1][0];
			r[j-1]=r20-(r20*s - x[j-1][1] + x[j-1][0])/ss2;
		} //j
//220

	} else {
	
		for(j=1;j<=3;j++){
			r20=x[j-1][2]-x[j-1][0];
			r[j-1]=r20+(r20*s - x[j-1][1] + x[j-1][0])/ss2;
		} //j
//230
 
	} // if else

		d1=sqrt( r[0]*r[0]+r[1]*r[1]+r[2]*r[2]);
		dl1[is-1]=r[0]/d1;
		dm1[is-1]=r[1]/d1;
		dn1[is-1]=r[2]/d1;

	} //is

// 200

	s0[0]=dl1[0];
	s0[1]=dm1[0];
	s0[2]=dn1[0];

	s1[0]=dl1[1];
	s1[1]=dm1[1];
	s1[2]=dn1[1];
}
//----------------------------------------------------------------------*
static float a[mmax][mmax];
//----------------------------------------------------------------------*
void curfit(int kord,int npts,float dp[][3],float t0[3],float t1[3],float xv[],float vone[][3])
{
	float indx[mmax];
	float del[mmax],d1[mmax][3];
	int n,n2,n3;

	int i,j,k;

	n=npts-1;
	n2=n+2;
	n3=n+3;

	mkdel(npts,dp,del);
	mksdel(npts,del,xv);
	mkamat(npts,del,a);

	for(k=0;k<3;k++){
	   for(j=2;j<=npts-1;j++){
		 d1[j][k]=dp[j-1][k];
	   } //j
// 61
	  d1[0][k]	= dp[0][k];
	  d1[1][k]    = t0[k];
	  d1[n2-1][k] = t1[k];
	  d1[n3-1][k] = dp[npts-1][k];
	} //k

// 61
	tridi(n3,a,3,d1);

	for(i=0;i<n3;i++){
		for(j=0;j<3;j++){
			vone[i][j]=d1[i][j];
		}
	}

// 60
}
//----------------------------------------------------------------------*
void mkdel(int n,float dp[][3],float del[])
{
	int i;
	for(i=1;i<=n-1;i++){
		del[i-1]=sqrt(  pow( dp[i][0]-dp[i-1][0] ,2.)
					  +	pow( dp[i][1]-dp[i-1][1] ,2.)
					  + pow( dp[i][2]-dp[i-1][2] ,2.) );
	} // i
}
//----------------------------------------------------------------------*
void mksdel(int n,float del[],float xv[])
{
	int i;


// 10
	xv[0]=0.;
	xv[1]=0.;
	xv[2]=0.;
	xv[3]=0.;

	for(i=1;i<=n-1;i++)
		xv[i+3]=xv[i+2]+del[i-1];
// 20
	
	xv[n+3]=xv[n+2];
	xv[n+4]=xv[n+2];
	xv[n+5]=xv[n+2];

	return ;
}
//----------------------------------------------------------------------*
void mkamat(int npts,float del[],float a[][mmax])
{

	
	float del1,del2,del3,del4,del5;
	float f,g;
	int n1,n2,n3,m,ik;


	int i,j,k;
	
    n1=npts-1;
	n2=npts+1;
	n3=npts+2;

// reset zero

	for(i=0;i<n3;i++){
		for(j=0;j<n3;j++){
			a[j][i]=0.;
		} //j
	} //i
// 25

// Make the A Matrix

	a[0][0]=1.0;
	a[0][1]=-3.0/del[0];
	a[1][1]=3.0/del[0];

	for(m=4;m<=n2;m++){

		del1=delval(npts,del,m  ,1);
		del2=delval(npts,del,m-1,2);
		del3=delval(npts,del,m-2,3);
		del4=delval(npts,del,m-1,1);
		del5=delval(npts,del,m-1,3);

		f=del1*del1/del2/del3;
		g=del4*del4/del2/del5;
		
		ik=m-1;

		a[ik-2][ik-1]=f;
		a[ik-1][ik-1]=( 1.0-f-g );
		a[ik  ][ik-1]=g;

	} //m

	a[n2-1][n2-1]=-3.0/del[n1-1];
	a[n3-1][n2-1]= 3.0/del[n1-1];
	a[n3-1][n3-1]= 1.0;
	return;
} 
// --------------------------------------------------------------------------*
// Function Start
// --------------------------------------------------------------------------*
// -----------------------------------------------------------------*
//  tridiagonal       change Fortran -> C++
//  void tridi (n,mp,am,m,d);    old function
//  void tridi (n,am,m,d);       new function
// -----------------------------------------------------------------*
// void tridi(int n,float am[][mmax],int m,float d[][3]);

// -----------------------------------------------------------------*
//                   Input Data
//		 
//           Ax=d 
//
//  int n             :  Vertex Number  (Point+TangentVecter)
//  float am[][mmax]  :	 Matrix   n * n	       -> A
// 	int m             :  if m=2 x,y     if m=3 x,y,z
//  float d[mmax][3]  :  Point+TanglentVecter  -> d
// 
//                   Output Data
//  float d[mmax][3]  :  Vertex     	       -> x 
// -----------------------------------------------------------------*

int tridi(int n,float am[][300],int m,float d[][3])
{
	float a[300],b[300],c[300];
	float gam[300],y[300][300];

	float tin;
	float zero;
	float z1,z2,zz;
	int i,j,k;
	
	tin=1.0e-20f;
	zero=0.;

	for(i=1;i<=n;i++){
	 if(i!=1) a[i-1] = am[i-2][i-1];
	 b[i-1]= am[i-1][i-1];
	 if(i!=n) c[i-1]=am[i][i-1];
	}
// 100 i

	a[0]=0.;
	c[n-1]=0.;

	for(i=1;i<=m;i++)
	 y[i-1][0] = d[0][i-1] / b[0];

// 110 i
	gam[0]=c[0]/b[0];


	for(i=2;i<=n-1;i++){
	  		  
//	  z1 = fabs( b[i-1] );
//	  z2 = fabs( c[i-2] ) + fabs ( a[i] );

//	  if(z1 < z2)  return -1; // return value input!
  
	  zz=b[i-1]-a[i-1]*gam[i-2];

	  if(zz==zero) zz=tin;
	  gam[i-1]=c[i-1]/zz;

	  for(j=1;j<=m;j++){
	    zz=b[i-1]-a[i-1] * gam[i-2];
	    if(zz==zero) zz=tin;

	    y[j-1][i-1] =(  d[i-1][j-1]-  a[i-1] * y[j-1][i-2]  )/zz;
	  }// j

	} // i
	
// 120 j,i
	  for(i=n;i<=n;i++){
		zz= b[i-1]-a[i-1] * gam[i-2];
	    if(zz==zero) zz=tin;
		gam[i-1]=c[i-1]/zz;

	    for(j=1;j<=m;j++){
	      zz=b[i-1]-a[i-1]*gam[i-2];
	      if(zz==zero) zz=tin;
		  y[j-1][i-1]= ( d[i-1][j-1] - a[i-1] * y[j-1][i-2] ) /zz;
	    } //j
	  }   //i
// 125  j,i;

	for(j=1;j<=m;j++){
	  d[n-1][j-1]= y[j-1][n-1];

	  for(i=1;i<=n-1;i++){
		k=n-i;
		d[k-1][j-1] = y[j-1][k-1] - gam[k-1] * d[k][j-1];
	  } //i
	} // j
// 140 , 130
		return 0;
}
//----------------------------------------------------------------------*
float delval(int npts,float del[],int i,int k)
{
	float delv,dd;
	int j;
	delv=0.;
	for(j=i;j<=i+k-1;j++){
		dd=0.;
		if(j>2 && j< (npts+2)) dd=del[j-3];

		delv=delv+dd;
	} //j
	return delv;
}
//----------------------------------------------------------------------*
int refang2(float s1[3],float s2[3],float *angle)
{
	double a[3],b[3],ang,sum1,sum2,sum3,dtol;
	int istat;

	istat=0;
	dtol=1.0e-07;

	a[0]=(double)s1[0];
	a[1]=(double)s1[1];
	a[2]=(double)s1[2];

	b[0]=(double)s2[0];
	b[1]=(double)s2[1];
	b[2]=(double)s2[2];

//----------------------------------------------------------------------*
// vector a             
//----------------------------------------------------------------------*
  	
	sum1=sqrt( a[0]*a[0]+a[1]*a[1]+a[2]*a[2] );

//----------------------------------------------------------------------*
// vector b             
//----------------------------------------------------------------------*
	sum2=sqrt( b[0]*b[0]+b[1]*b[1]+b[2]*b[2] );

//----------------------------------------------------------------------*
// a.b                  
//----------------------------------------------------------------------*

	sum3= a[0]*b[0]+a[1]*b[1]+a[2]*b[2];

//----------------------------------------------------------------------*
// relative angle       
//----------------------------------------------------------------------*
  
    if( sum1< dtol) {
		istat=1;
		return istat;
	}
	if( sum2< dtol) {
		istat=1;
		return istat;
	}

	sum3=sum3/sum2/sum1;
	
	sum3=min(sum3,1.0f);
	sum3=max(sum3,-1.0f);
	ang=acos( sum3);
	*angle=(float)ang;
	return istat;
}
//----------------------------------------------------------------------*
int unitvc(float a[3],float *dist)
{

	int istat;
	istat = 0;
	*dist = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
	if( *dist < 1.0e-08f )goto L_999;
	a[0] = a[0]/ *dist;
	a[1] = a[1]/ *dist;
	a[2] = a[2]/ *dist;

	return istat;
L_999:
	;
	istat = 1;
	return istat;
}
//----------------------------------------------------------------------*

int dptPlaneq(int np,float dp[][3],float plq[4])
{
	int ierr, i,k,istat;
	float tol,dd;
	float a[3],b[3],c[3];

	/* istat=0 : no plane curve  */

	istat = 0;
	if (np < 3)	return istat;
//	getEpsCvNode(&tol);

//	tol=tol*10.0f;

	ierr=avgNvec(np,dp,plq);
	plq[3] = 0.0f;
	for (i = 1; i <= np; i++) {
		plq[3] =plq[3]-( plq[0]*dp[i-1][0] + plq[1]*dp[i-1][1] + plq[2]*dp[i-1][2]);
	}
	plq[3] /=(float) np;

	dd=0.0f;
	for(i=0;i<np;i++){
		dd+=fabs(
			plq[0]*dp[i][0]+
			plq[1]*dp[i][1]+
			plq[2]*dp[i][2]+
			plq[3]);
	}
	dd=dd/(float)np;
//	if(dd>tol) return istat;

	istat = 1;

	return istat;
}
//----------------------------------------------------------------------*
int avgNvec(int n,float v[][3],float pn[3])
{
	int istat;
	int i, j;
	float tiny, a1, b1, c1, d1;
	/* average normal vector in loop 
	 * loop is not closed  */

	istat = 0;
	tiny = 1.0e-12f;

	a1 = 0.0f;
	b1 = 0.0f;
	c1 = 0.0f;
	for( i = 1; i <= n; i++ ){
		if( i == n ){j = 1;
			}
		else{
			j = i + 1;
			}
		a1 = a1 + (v[i - 1][1] - v[j - 1][1])*(v[i - 1][2] + v[j - 1][2]);
		b1 = b1 + (v[i - 1][2] - v[j - 1][2])*(v[i - 1][0] + v[j - 1][0]);
		c1 = c1 + (v[i - 1][0] - v[j - 1][0])*(v[i - 1][1] + v[j - 1][1]);
		}
	a1 = a1/ n;
	b1 = b1/ n;
	c1 = c1/ n;
	d1 = sqrt( a1*a1 + b1*b1 + c1*c1 );
	if( d1 < tiny )
		goto L_999;
	pn[0] = a1/d1;
	pn[1] = b1/d1;
	pn[2] = c1/d1;

	return istat;
L_999:
	istat = 1;
	return istat;
}