missile_png.cpp

本程序包为导弹比例导引仿真计算程序

//
//    导弹比例导引仿真 Visual C++ 源程序部分，可由MATLAB通过MEX进行调用。
//
//
//	   COPYRIGHT (C) vxLand , 2002-2008
//

#include<stdio.h>
#include<math.h>
#include<string.h>
#include "mex.h"
#define A 3.1415926
/* Output Arguments */

#define	YP_OUT	plhs[0]
#define NUM_OUT plhs[1]

    double *yp; 
    double *num;

struct point
{
    float x;
	float y;
};
void png()
{
    struct point
	{
	  float x;
	  float y;
	}T,M,points[200];
	T.x =0.0;
	T.y= 2000.0;
	M.x = 0.0;
	M.y = 0.0;
	int i = 0;
	float VT = 100.0;
	float VM = 200.0;
    float dt = (float)0.1;
    float aT0 = (float)A/2;
	float aM0 = (float)A/2;
	float aT1,aM1;
	float daT,daM;
    float ddT,ddM;
	float k = (float)3.0;
    float dlT,dlM;
	float dx,dy;
        int flag = 0;
	FILE *fpout1,*fpout2;

	fpout1 = fopen("Missile_Png1.dat","w");
	fprintf(fpout1,"t(s),x(m),y(m),q(deg),Theta(deg)\n");
    fpout2 = fopen("Missile_Png2.dat","w");
    fprintf(fpout2,"x(t),y(t)\n");
    dlT = VT*dt;
	dlM = VM*dt;
	aT0 = (float)atan( (T.y-M.y)/(T.x-M.x) ); 
	aM0 = aT0;
    for(i=0;i<200;i++)
    {   
	//printf("%7.1f,%7.1f,%7.1f,%7.1f,%7.1f \n",i*dt,T.x,T.y,M.x,M.y);
	fprintf(fpout1,"%8.3f , %7.1f , %7.1f , %7.1f , %7.1f\n",\
			(float)i*dt,(float)M.x,(float)M.y,aT1*57.3,aM1*57.3);
	fprintf(fpout2,"%7.1f , %7.1f\n",(float)T.x,(float)T.y);
		T.x += dlT;
		dx = dlM*(float)cos(aM0);
		dy = dlM*(float)sin(aM0);
	    M.x += dx;
	    M.y += dy;

        points[i].x = M.x;
	points[i].y = M.y;
	//used by matlab
        yp[2*i] = (double)M.x;
        yp[2*i+1] = (double)M.y;
	aT1 = (float)atan( (T.y-M.y)/(T.x-M.x) ); 
        daT = aT1-aT0;

        ddT = daT/dt;
        ddM = ddT * k;
	    daM = ddM*dt;
        aM1 = daM+aM0;

//        printf("  x = %7.1f   y = %7.1f \n",yp[i*2],yp[i*2+1]);
        printf("  x = %7.1f   y = %7.1f \n",M.x,M.y);
		 if(sqrt(pow((M.x-T.x),2)+pow((M.y-T.y),2))<10 || M.y>2000)
		 {
			 if(M.y>2000)
				printf(" \nFAIL\n");
			 else
				printf("\nSUCCESS , i=%d \n",i);
			 flag = 1;
		 }

		aT0 = aT1;
		aM0 = aM1;
        if(flag)
          break;
        
	}
	//how many points used
	num[0] = i; 
//for (i=0;i<num[0];i++) {yp[2*i+0]=i;yp[2*i+1]=i*10;}
	
	fclose(fpout1);
    fclose(fpout2);
    
}

void mexFunction( int nlhs, mxArray *plhs[], 
		  int nrhs, const mxArray*prhs[] )
     
{ 
  
    /* Create a matrix for the return argument */ 
    YP_OUT = mxCreateDoubleMatrix(2, 200, mxREAL); 
    NUM_OUT = mxCreateDoubleMatrix(1,1, mxREAL); 
    
    /* Assign pointers to the various parameters */ 
    yp  = mxGetPr(YP_OUT);
    num = mxGetPr(NUM_OUT);
  
    /* Do the actual computations in a subroutine */
    png();
    
    return;
  
}