matrix.cpp

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

//**************************< description >******************************
//                                                                      *
//         Matrix                        vers-1.0       97-04-11        *
//                                                                      *
//                                                                      *
//         file: matrix.f                matrix handler                 *
//                                                                      *
//         part:                                                        *
//----------------------------------------------------------------------*
//                                                                      *
//         copyright by                                                 *
//         t.  k.  yoon                hanjin heavy industries co. ltd. *
//                                                                      *
//***********************************************************************
//     subroutine invmat (a,n,d)  
//----------------------------------------------------------------------*
//     Purpose
//        invert a matrix 
// 
//     Variable lists  
//        a()     input matrix, destroyed in computation and replaces by
//                resultant inverce.
//        n       order of matrix a
//        det     resultant determinant
//        l()     work vector of length n
//        m()     work vector of length n
//
//    Remarks
//        matrix a must be a general matrix
//
//     Method
//        The standard Gauss-Jordan method is used. The determinant
//        is also calculated. A determinant of zero indicates that
//        the matrix is singular.
//----------------------------------------------------------------------*
#include "stdafx.h"

#include <math.h>
#include "matrix.h"

#include "GA.h"
#include "dlgPlotting.h"

#define maxar 4000
#define data 900
#define mzmax 600
#define lmax 600
#define mmax 300  // Bspline Max Node

	extern int space;
	extern float x[data],y[data],z[data];
	extern float Px[data],Py[data],Pz[data],B2x[data],B2y[data],B2z[data];
	extern float tknot[100];
	extern int countBest;
	extern float IndiCrossBest[2][100];
	extern float DrawB2x[data],DrawB2y[data],DrawB2z[data];

	float txx[maxar],tyy[maxar],tzz[maxar],rvect[lmax+6],ctlpt[lmax+2][3],pocx0[data],pocy0[data],pocx1[data],pocy1[data];
	int countPoc;
//----------------------------------------------------------------------*
void invmat(float a[],int n,float *d)
{
	int i,j;
	int l[maxar],m[maxar];
	double biga,hold;
	int nk,k,kk,iz,ij,ki;
	int ji,jp,jk,ik,kj,jq,jr;

	double ppp;

	*d=1.0;
	nk=-n;

	for(k=1;k<=n;k++){
		nk+=n;
		l[k-1]=k;
		m[k-1]=k;

		kk=nk+k;
		biga=a[kk-1];

		for(j=k;j<=n;j++){
			iz=n*(j-1);

			for(i=k;i<=n;i++){
				ij=iz+i;

				ppp=fabs(biga)-fabs( a[ij-1]);

				if(ppp<0.) goto i_15;
				if(ppp>=0.) continue;

i_15:
				biga=a[ij-1];
				l[k-1]=i;
				m[k-1]=j;
			} //i

		} //j
// 20
		
// interchange rows


		j=l[k-1];

		if( (j-k)<=0) goto i_35;
		if( (j-k)> 0) goto i_25;

i_25:
		ki=k-n;
		for(i=1;i<=n;i++){
			ki+=n;
			hold=-a[ki-1];
			ji=ki-k+j;

			a[ki-1]=a[ji-1];
			a[ji-1]=hold;
		} //i
// 30

// interchange columns

i_35:
		i=m[k-1];

		if( (i-k)<=0) goto i_45;
		if( (i-k)>0)  goto i_38;

i_38:
		jp=n*(i-1);

		for(j=1;j<=n;j++){
			jk=nk+j;
			ji=jp+j;

			hold=-a[jk-1];
			a[jk-1]=a[ji-1];

			a[ji-1]=hold;
		} //j


// divide column by minus pivot
//	( value of pivot element is contained in biga )

i_45:
		if(biga==0.) goto i_46;
		if(biga!=0.) goto i_48;

i_46:
		*d=0.;
		return;

i_48:
		for(i=1;i<=n;i++){

			if( (i-k) ==0 ) continue;
			if( (i-k) !=0 ) goto i_50;


i_50:
			ik=nk+i;
			a[ik-1]=a[ik-1]/(-biga);
		} //i
// 55
// reduce matrix
		for(i=1;i<=n;i++){
			ik=nk+i;
			hold=a[ik-1];
			ij=i-n;
			for(j=1;j<=n;j++){
				ij+=n;
				if( (i-k) ==0 ) continue;
				if( (i-k) !=0 ) goto i_60;
	   
i_60:
				if( (j-k) ==0 ) continue;
				if( (j-k) !=0 ) goto i_62;

i_62:
				kj=ij-i+k;

				a[ij-1]=hold*a[kj-1]+a[ij-1];

			} //j
		}// i

// divide row by pivot

		kj=k-n;

		for(j=1;j<=n;j++){
			kj+=n;
			if( (j-k)==0) continue;
			if( (j-k)!=0) goto i_70;

i_70:
			a[kj-1]=a[kj-1]/biga;
		} //j




// product of pivots

		*d=(*d)*biga;

// replace pivot by reciprocal

		a[kk-1]=1.0/biga;

	} //k
// i_80


	k=n;


i_100:

	k=k-1;
	
	if(k<=0) goto i_150;
	if(k>0) goto i_105;


i_105:
	i=l[k-1];
	if( (i-k)<=0) goto i_120;
	if( (i-k) >0) goto i_108;
	
i_108:

	jq=n*(k-1);
	jr=n*(i-1);

	for(j=1;j<=n;j++){

		jk=jq+j;
		hold=a[jk-1];
		ji=jr+j;
		a[jk-1]=-a[ji-1];
		a[ji-1]=hold;

	} // j

i_120:
	j=m[k-1];

	if( (j-k) <=0) goto i_100;
	if( (j-k) >0) goto i_125;

i_125:
	ki=k-n;

	for(i=1;i<=n;i++){
		ki+=n;
		hold=a[ki-1];
		ji=ki-k+j;

		a[ki-1]=-a[ji-1];
		a[ji-1]=hold;
	} //i
	
	goto i_100;

i_150:

	return;

}
//----------------------------------------------------------------------*
void CreateOpPocupine()
{
	float tlen,slen,scale,cmax;
	int nel,npcs,np;
	float pn[3],por[501][3][2]; //Max Porcupine Node 500
	float pq[4];
	int istat,ierr,i;

	float pt[3],pt0[3];
	float datpt[data][3],B2temp[data][3],datsv[3],datev[3];
	char temp[80];

	np = space;
	invertxz(np,Px,Pz,datpt);
	tlen=chordLength(np,datpt);
	slen=14.875;
	nel=(int)(tlen/slen*1000.0);
	nel=max(nel,(np-1)*10);
	nel=min(nel,500);
		
	npcs=countBest-4;

	for (int j=1; j<=countBest; j++) {
		rvect[j-1] = IndiCrossBest[1][j]*tlen;
	}
	invertB2(npcs,DrawB2x,DrawB2z,ctlpt);	
	curvatLine(npcs,rvect,ctlpt,nel,por,&cmax);

	scale=20*cmax;
	countPoc = 0;

	for(i=1;i<nel-1;i++){
		txx[0]=por[i][0][0];
		tyy[0]=por[i][1][0];

		txx[1]=txx[0]+por[i][0][1]*scale;
		tyy[1]=tyy[0]+por[i][1][1]*scale;
			
		pt0[0]=txx[0];
		pt0[1]=tyy[0];

		pt[0]=txx[1];
		pt[1]=tyy[1];

		pocx0[i] = pt0[0];
		pocy0[i] = pt0[1];
		pocx1[i] = pt[0];
		pocy1[i] = pt[1];
		countPoc = countPoc+1;
	}
}
//----------------------------------------------------------------------*
void invertxz(int n, float x1[], float z1[], float tempxz1[][3])
{
	for (int i=1; i<=n; i++){
		tempxz1[i-1][0] = x1[i];
		tempxz1[i-1][1] = z1[i];
	}
}
//----------------------------------------------------------------------*
float chordLength(int n,float dp[][3])
{
	float clen,slen;
	int i;

	clen=0.;

	for(i=0;i<n-1;i++){
		slen= sqrt( (dp[i+1][0]-dp[i][0])*(dp[i+1][0]-dp[i][0]) 
			+ (dp[i+1][1]-dp[i][1])*(dp[i+1][1]-dp[i][1]));
	
		clen+=slen;
	} //i
	return clen;
}

//----------------------------------------------------------------------*
void invertB2(int n, float B2x1[], float B2z1[], float B2temp1[][3])
{
	for (int i=1; i<=n; i++){
		B2temp1[i-1][0] = (float)B2x1[i];
		B2temp1[i-1][1] = (float)B2z1[i];
	}
}
//----------------------------------------------------------------------*
void curvatLine(int npcs,float xv[],float cpts[][3],int nel,float por[][3][2],float *cmax)
{
	float ps,pe,dt,tp;
	float cmid;
	int i,npts,ierr;
	float pi,del[mzmax+6],ai[mzmax-1][4][4];

	float pt[3],tv[3],sd[3],pn[3],cm;
	float v1[3],v2[3];
	float angle,dd[3];
	
    pi=acos(-1.0f);
    npts=npcs;
    xv2del(4,npts,xv,del);
    bsmat (npts,del,ai);
	ierr=dptPlaneq(npts,cpts,pn);

	ps=xv[3];
	pe=xv[3+npcs];

	dt=(pe-ps)/(float)(nel-1);

	*cmax=-1.0e12f;

	cmid=0.;
// case planar curve
	for(i=2;i<=nel-1;i++){
		tp=ps+(float)(i-1)*dt;
		bmatpd03c(4,npcs,xv,cpts,del,ai,tp,pt,tv,sd,&cm) ;
		crossp(pn,tv,v1);
		crossp(tv,pn,v2);
// Check angle between (v1,sd)
		refang2(sd,v1,&angle);

		if(angle<pi*0.5) {
			v1[0]=v2[0];
			v1[1]=v2[1];
			v1[2]=v2[2];
		} 

		unitvc(v1,dd);

		por[i-1][0][0]=pt[0];
		por[i-1][1][0]=pt[1];

		por[i-1][0][1]=v1[0]* cm;
		por[i-1][1][1]=v1[1]* cm;

		*cmax=max(*cmax,fabs(cm) );
		cmid+=fabs(cm);

	} //i
	*cmax=cmid/nel;
}
//----------------------------------------------------------------------*
void xv2del(int norder,int npts,float xv[],float del[])
{
	int i,j;
	for(i=0;i<npts-1;i++){
        j=i+norder; 
        del[i]=xv[j]-xv[j-1];
	} //i
}
void bsegmat (int n,float del[],int nseg,float smat[4][4])
{
	int k,j;

	float del00,del01,del02,del03;
	float del10,del11,del12,del13;
	float del20,del21,del22,del23;
	float del30,del31,del32,del33;

	float a11,a12,a13,a14;
	float a21,a22,a23,a24;
	float a31,a32,a33,a34;
	float a41,a42,a43,a44;

	float aa,bb,cc,dd;

	k=nseg-1;

    del12=0.0; 
	for(j=k;j<=k+1;j++){

       if (j<1 || j>n){
          aa=0.0; 
	   } else {
          aa=del[j-1];
	   } 
       del12=del12+aa;
	} //j

	if(k+2>n) {
		aa=0.0; 
	}else {
        aa=del[k+1];
	}

    if (k-1<1) {
		bb=0.0 ;
	} else {
        bb=del[k-2];
	}

	if((k+3)>n) {
        cc=0.0; 
	}else {
        cc=del[k+2];
    }

    if (k<1) {
        dd=0.0 ;
	} else {
        dd=del[k-1];
    }
   
    del13=del12+aa;
    del23=del12+bb;
    del02=del13-dd;
    del03=del02+cc;

    a11=del[k]*del[k]/del12/del23;

    if (k==0) { 
        a13=0.0;
        a23=0.0; 
    } else {
        a13=del[k-1]*del[k-1]/del12/del13;
        a23=3.0*del[k]*del[k-1]/del12/del13;
	} 

    a33=3.0*del[k]*del[k]/del12/del13;
    a44=del[k]*del[k]/del02/del03;
    a43=del[k]*del[k]/del02/del13;


    smat[0][0]=a11;
    smat[1][0]=1.0-a11-a13;
    smat[2][0]=a13;
    smat[3][0]=0.0;

    smat[0][1]=-3.0*a11;
    smat[1][1]=3.0*a11-a23;
    smat[2][1]=a23;
    smat[3][1]=0.0;

    smat[0][2]=3.0*a11;
    smat[1][2]=-(3.0*a11+a33);
    smat[2][2]=a33;
    smat[3][2]=0.0;

    smat[0][3]=-a11;
    smat[2][3]=-(a33/3.0+a44+a43);
    smat[1][3]=(a11-smat[2][3]-a44);
    smat[3][3]=a44;
}
//----------------------------------------------------------------------*			
//----------------------------------------------------------------------*
void bsmat(int npts,float del[],float aitemp1[][4][4])
{

	float smat[4][4];
	int k,ik,ij,n;

	n=npts-1;
	for(k=0;k<n;k++){
		bsegmat (n,del,k+1,smat);
		for(ik=0;ik<4;ik++){
			for(ij=0;ij<4;ij++){
				aitemp1[k][ij][ik]=smat[ij][ik];
			} //ij
		} //ik
	} //k
}
				
//----------------------------------------------------------------------*
int bmatpd03c(int kord,int ncps,float xv[],float vtx[][3],float del[],float aitemp2[][4][4],float tpa,float pt[3],float d1[3],float d2[3],float *cm)
{
//  zero 1st,2st derivative and curvature at parameter (global) 
//
	float x0[3],t0[4],x1[3],t1[4],x2[3],t2[3];
	float a[3],a1,a2,a3;
	int istat;  
	int k;
	int npts,ns;
	float tl,tiny;
	tiny=1.0e-12f;

    istat=0;
 
    npts=ncps-kord+2;  
    gpa2lpa (kord,npts,xv,tpa,&ns,&tl); 
    if (ns==0) goto i_999;

    t0[0]=1.0; 
    t0[1]=tl;  
    t0[2]=tl*tl;
    t0[3]=tl*tl*tl; 

    bmatcb (t0,ns,aitemp2,vtx,x0); 
	for(k=0;k<3;k++){
		pt[k]=x0[k];
	} // k 
	
    t1[0]=0.0; 
    t1[1]=1.0;  
    t1[2]=2.0*tl;
    t1[3]=3.0*tl*tl;