controller.c

一个飞机控制器的控制器源码

    return Matrix_new(1, 1, 1, token, token.type);
}
Token Matrix_toString(Token thisToken, ...) {
    int i, j;
    int currentSize, allocatedSize;
    char* string;
    Token elementString;
    allocatedSize = 512;
    string = (char*) malloc(allocatedSize);
    string[0] = '[';
    string[1] = '\0';
    currentSize = 2;
    for (i = 0; i < thisToken.payload.Matrix->column; i++) {
        if (i != 0) {
            strcat(string, "; ");
        }
        for (j = 0; j < thisToken.payload.Matrix->row; j++) {
            if (j != 0) {
                strcat(string, ", ");
            }
            elementString = functionTable[(int) Matrix_get(thisToken, j, i).type][FUNC_toString](Matrix_get(thisToken, j, i));
            currentSize += strlen(elementString.payload.String);
            if (currentSize > allocatedSize) {
                allocatedSize *= 2;
                string = (char*) realloc(string, allocatedSize);
            }
            strcat(string, elementString.payload.String);
            free(elementString.payload.String);
        }
    }
    strcat(string, "]");
    return String_new(string);
}
Token Matrix_isCloseTo(Token thisToken, ...) {
    int i, j;
    va_list argp;
    Token otherToken;
    Token tolerance;
    va_start(argp, thisToken);
    otherToken = va_arg(argp, Token);
    tolerance = va_arg(argp, Token);
    if (( thisToken.payload.Matrix->row != otherToken.payload.Matrix->row ) ||
    ( thisToken.payload.Matrix->column != otherToken.payload.Matrix->column )) {
        return Boolean_new(false);
    }
    for (i = 0; i < thisToken.payload.Matrix->column; i++) {
        for (j = 0; j < thisToken.payload.Matrix->row; j++) {
            if (!functionTable[(int) Matrix_get(thisToken, j, i).type][FUNC_isCloseTo](Matrix_get(thisToken, j, i), Matrix_get(otherToken, j, i), tolerance).payload.Boolean) {
                return Boolean_new(false);
            }
        }
    }
    va_end(argp);
    return Boolean_new(true);
}
// Assume the given otherToken is array type.
// Return a new Array token.
Token Matrix_multiply(Token thisToken, ...) {
    int i, j;
    va_list argp;
    Token result;
    Token element, otherToken;
    va_start(argp, thisToken);
    otherToken = va_arg(argp, Token);
    if (otherToken.type == TYPE_Matrix
    && otherToken.payload.Matrix->row == 1
    && otherToken.payload.Matrix->column == 1) {
        // Handle simple scaling by a 1x1 matrix
        result = Matrix_new(thisToken.payload.Matrix->row, thisToken.payload.Matrix->column, 0);
    } else {
        result = Matrix_new(thisToken.payload.Matrix->row, thisToken.payload.Matrix->row, 0);
    }
    switch (otherToken.type) {
        case TYPE_Matrix:
        for (i = 0; i < thisToken.payload.Matrix->column; i++) {
            for (j = 0; j < thisToken.payload.Matrix->row; j++) {
                element = Matrix_get(thisToken, j, i);
                if (otherToken.payload.Matrix->row == 1
                && otherToken.payload.Matrix->column == 1) {
                    Matrix_set(result, j, i, functionTable[(int)element.type][FUNC_multiply](element, Matrix_get(otherToken, 0, 0)));
                }
            }
        }
        break;
        #ifdef TYPE_Array
        case TYPE_Array:
        element = Array_new(thisToken.payload.Matrix->column *
        thisToken.payload.Matrix->row, 0);
        for (i = 0; i < thisToken.payload.Matrix->column; i++) {
            for (j = 0; j < thisToken.payload.Matrix->row; j++) {
                Array_set(element,
                i + thisToken.payload.Matrix->row * j,
                Matrix_get(thisToken, j, i));
            }
        }
        break;
        #endif
        default:
        for (i = 0; i < thisToken.payload.Matrix->column; i++) {
            for (j = 0; j < thisToken.payload.Matrix->row; j++) {
                element = Matrix_get(thisToken, j, i);
                result.payload.Matrix->elements[i] = functionTable[(int)element.type][FUNC_multiply](element, otherToken);
            }
        }
    }
    va_end(argp);
    return result;
}
Token Matrix_delete(Token token, ...) {
    int i, j;
    Token element, emptyToken;
    // Delete each elements.
    for (i = 0; i < token.payload.Matrix->column; i++) {
        for (j = 0; j < token.payload.Matrix->row; j++) {
            element = Matrix_get(token, j, i);
            functionTable[(int) element.type][FUNC_delete](element);
        }
    }
    free(token.payload.Matrix->elements);
    free(token.payload.Matrix);
    return emptyToken;
}
Token transpose(Token mytoken) //矩阵转置
{
	int 	i,j;
	Token	result;
	int		row,column;
	row = mytoken.payload.Matrix->row;
	column = mytoken.payload.Matrix->column;
	result = Matrix_new(column, row, 0);
	Token tempToken[row*column];
	Token tranToken[column][row];
	Token thisToken[row][column];	
    for (i = 0; i < column; i++) {
    	for(j = 0; j< row; j++)
    	{    	
    		tempToken[i*row+j]= Matrix_get(mytoken,j,i);		
		}
	}
	for(i=0;i<row;i++)
		for(j=0;j<column;j++){		
			thisToken[i][j]=tempToken[i*column+j];
		}	
	for(i=0;i<column;i++)
		for(j=0;j<row;j++){		
			tranToken[i][j]=thisToken[j][i];			
		}	
	for(i=0;i<column;i++)
		for(j=0;j<row;j++){		
			tempToken[i*row+j]=tranToken[i][j];		
		}		
	for(i=0;i<row;i++)
		for(j=0;j<column;j++){	
			Matrix_set(result,j,i,tempToken[i*column+j]);
		}	
	return result;	
}

Token inverse(Token thistoken) 
{
	int 	row;
	int 	column;
	int 	i,j;
	Token	result;
	Token	tempToken;
	
	row = thistoken.payload.Matrix->row;
	column = thistoken.payload.Matrix->column;
	if(row != column)
	{
		printf("this is not a squre matrix!\n");
		exit(0);
	}
	double *a = calloc(row * row, sizeof(double));
	
	double *c = calloc(row * row, sizeof(double));

    for (i = 0; i < column; i++) {
    	for(j = 0; j< row; j++)
    	{
    		tempToken=Matrix_get(thistoken,j,i);
    		switch(tempToken.type)
    		{
    			#ifdef TYPE_Double
    			case TYPE_Double:
    				a[j*column+i]=(real)tempToken.payload.Double;
    				break;
    			#endif
    			#ifdef TYPE_Int
    			case	TYPE_Int:
    				a[j*column+i]=(real)tempToken.payload.Int;
    				break;
    			#endif
    			default:
    				printf("unknow type in inverse function");
    				exit(1);		
    		}       	
    	}
    }	
	integer M = row;
	integer lda = M;
    integer lwork = row;
    integer INFO; 
    integer * ipiv = calloc(row*row,sizeof(integer)); 
	dgetrf_(&M,&M,a,&M,ipiv,&INFO);

 	dgetri_(&M,a,&lda,ipiv,c,&lwork,&INFO);
 	result = Matrix_new(row, row, 0);
    for (i = 0; i < column; i++)
    {
		for(j = 0; j< row; j++)
		{	
    		Matrix_set(result,j,i,Double_new((double)a[j*column+i])); 
		}
	}

	return result;
	
}

Token add(Token thisToken, ...) {
    int i, j;
    char tempChar;
    double tempDouble=0.0;
    va_list argp;
    Token result;
    Token tempToken;
    Token otherToken;

    va_start(argp, thisToken);
    otherToken = va_arg(argp, Token);
	
	switch(otherToken.type)
	{
		#ifdef TYPE_Matrix
		case TYPE_Matrix:
	    	if(thisToken.payload.Matrix->row != otherToken.payload.Matrix->row || thisToken.payload.Matrix->column != otherToken.payload.Matrix->column )	
		    {
				printf("These matrix can not sum!");
				exit(-1);
		    }
		    result = Matrix_new(thisToken.payload.Matrix->row, thisToken.payload.Matrix->column, 0);
		    for (i = 0; i < thisToken.payload.Matrix->column; i++) {
		        for (j = 0; j < thisToken.payload.Matrix->row; j++) {
		        	tempToken = Matrix_get(thisToken, j, i);
		        	tempChar = tempToken.type;
	 				switch(tempToken.type) 
					{
						#ifdef TYPE_Int
						case TYPE_Int:
							tempDouble = (double)tempToken.payload.Int;
							break;
						#endif
						#ifdef TYPE_Double
						case TYPE_Double:
							tempDouble = tempToken.payload.Double;
							break;
						#endif
					}			        
			        tempToken = Matrix_get(otherToken, j, i);
	 				switch(tempToken.type) 
					{
						#ifdef TYPE_Int
						case TYPE_Int:
							tempDouble += (double)tempToken.payload.Int;
							break;
						#endif
						#ifdef TYPE_Double
						case TYPE_Double:
							tempDouble += tempToken.payload.Double;
							break;
						#endif
					}
					#ifndef TYPE_Int
				    #define TYPE_Int 9
				    #endif    
				    #ifndef TYPE_Double
				    #define TYPE_Double 9
				    #endif
			        if(tempChar == TYPE_Int && tempToken.type == TYPE_Int)		        
		            	Matrix_set(result, j, i,Int_new((int)tempDouble));
					else      
		            	Matrix_set(result, j, i,Double_new((double)tempDouble));          
		        }
		    }
	    	break;
	    #endif
	    
	    #ifdef TYPE_Int
        case TYPE_Int:
        	result = Int_new(thisToken.payload.Int + otherToken.payload.Int);
        	break;
        #endif
        #ifdef TYPE_Double