void check_matrix(const dim_type dim[],ndim_type ndim) { if(ndim<=0 || ndim>TZ_MATRIX_MAX_DIM) { TZ_WARN(ERROR_DATA_VALUE); matrix_error("Invalid dimension number",NULL); } ndim_type i; for(i=0; i<ndim; i++) if(dim[i]<=0) matrix_error("Invalid dimension",NULL); }
/** * Convert index to subscripts */ void ind2sub(const dim_type dim[],ndim_type ndim,size_t index,dim_type sub[]) { dim_type length = 1; ndim_type i; for(i=0; i<ndim; i++) length *= dim[i]; if(index>=length) matrix_error("Index out of range: %s","ind2sub"); for (i = 0; i < ndim; i++) { sub[i] = index % dim[i]; index /= dim[i]; } /* for(i=ndim-1;i>=1;i--) { length /= dim[i]; sub[i] = I/length; I %= length; } sub[0] = I; */ }
/** * Convert subscripts to index */ size_t sub2ind(const dim_type dim[],ndim_type ndim,const dim_type sub[]) { ndim_type i; size_t idx; if(sub[ndim-1]+1 > dim[ndim-1] || sub[ndim-1]<0) matrix_error("Subscript out of range",NULL); idx = sub[ndim-1]; for(i=ndim-1; i>0; i--) { if(sub[i-1]>dim[i-1] || sub[i-1]<0) matrix_error("Subscript out of range",NULL); idx = idx*dim[i-1]+sub[i-1]; } return idx; }
int altitude_estimator::matrix_mul(float *out, const float *m1, int row1, int column1, const float *m2, int row2, int column2) { if (column1 != row2) matrix_error("invalid matrix_mul"); for(int x1 = 0; x1<column2; x1++) { for(int y1 = 0; y1<row1; y1++) { out[y1*column2+x1] = 0; for(int k = 0; k<column1; k++) out[y1*column2+x1] += m1[y1*column1+k] * m2[k*column2+x1]; } } return 0; }
MATRIX_STATUS matrix_error_int(MATRIX_STATUS err) { const char *msgerr; switch(err) { case MATRIX_OK: return err; case MATRIX_MEMORY_ALLOCATION: msgerr = "Memory allocation error"; break; case MATRIX_BAD_PARAMETER: msgerr = "Bad parameter"; break; case MATRIX_WRONG_DIMENSION: msgerr = "Wrong dimension(s)"; break; case MATRIX_OUT_OF_RANGE: msgerr = "Index out of range"; break; case MATRIX_NO_CONVERGENCE: msgerr = "No convergence after iterations"; break; case MATRIX_DIVISION_ZERO: msgerr = "Division by zero"; break; case MATRIX_NOT_SQUARE: msgerr = "Not square matrix"; break; case MATRIX_SINGULARITY: msgerr = "Singular matrix"; break; default: msgerr = "Undefined error"; break; } matrix_error(err,msgerr); return(err); }