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);
}
