void inverse(float **M, int size)
{
float **P, **M_inv, **res, pivot, **B;
int c_size = 0, i, j;
res = (float **)malloc(size * sizeof(float *));
M_inv = (float **)malloc(size * sizeof(float *));
P = (float **)malloc(sizeof(float *));
iden(M_inv, size);
while (c_size < size)
{
B = (float **)malloc(size * sizeof(float *));
iden(B, size);
extract_col_vector(M, P, c_size, size);
matrix_multiplication(M_inv, P, res, size, size, 1);
pivot = res[c_size][0];
for(i=0; i<size; i++)
{
if(i==c_size)
res[i][0] = 1 / pivot;
else
res[i][0] = -(res[i][0]/pivot);
}
replace_col_vector(B, res, c_size, size);
matrix_multiplication(B, M_inv, res, size, size, size);
copy_mat(res, M_inv, 0, size-1, 0, size-1);
c_size++;
}
copy_mat(M_inv, M, 0, size-1, 0, size-1);
}
// model of motor subsystem in physics-based model of wheelchair(Quantum6000z)
void motor_model_c(double *motor_state, double *friction_out, double motor_input,
double delta_t, double param_mu, double param_beta, double param_gamma, double param_alpha){
double friction_threshold;
mxArray *mtx_A, *mtx_Bf, *mtx_Bd, *mtx_tmp1, *mtx_tmp2, *mtx_tmp3;
double *mtx_A_ptr, *mtx_Bf_ptr, *mtx_Bd_ptr, *mtx_tmp1_ptr, *mtx_tmp2_ptr, *mtx_tmp3_ptr;
// parameter matrix set-up
mtx_A = mxCreateDoubleMatrix(2, 2, mxREAL);
mtx_A_ptr = mxGetPr(mtx_A);
mtx_A_ptr[0] = 1;
mtx_A_ptr[1] = -param_beta * delta_t;
mtx_A_ptr[2] = delta_t;
mtx_A_ptr[3] = 1 - param_gamma * delta_t;
mtx_Bf = mxCreateDoubleMatrix(2, 1, mxREAL);
mtx_Bf_ptr = mxGetPr(mtx_Bf);
mtx_Bf_ptr[0] = delta_t;
mtx_Bf_ptr[1] = 0;
mtx_Bd = mxCreateDoubleMatrix(2, 1, mxREAL);
mtx_Bd_ptr = mxGetPr(mtx_Bd);
mtx_Bd_ptr[0] = 0;
mtx_Bd_ptr[1] = param_alpha * delta_t;
// temporary variable set-up
mtx_tmp1 = mxCreateDoubleMatrix(2, 1, mxREAL);
mtx_tmp1_ptr = mxGetPr(mtx_tmp1);
mtx_tmp2 = mxCreateDoubleMatrix(2, 1, mxREAL);
mtx_tmp2_ptr = mxGetPr(mtx_tmp2);
mtx_tmp3 = mxCreateDoubleMatrix(2, 1, mxREAL);
mtx_tmp3_ptr = mxGetPr(mtx_tmp3);
// friction threshold set-up
friction_threshold = -motor_state[0]/delta_t - motor_state[1];
if(abs(friction_threshold) <= param_mu)
friction_out[0] = friction_threshold;
else
friction_out[0] = sign_manual(friction_threshold) * param_mu;
// update motor state(motor_state = A*motor_state + B_f*friction_out + B_d*motor_input)
matrix_multiplication(mtx_A_ptr, motor_state, mtx_tmp1_ptr, 2, 1, 2);
//matrix_scalar_multiplication(mtx_Bf_ptr, friction_out, mtx_tmp2_ptr, 2, 1);
matrix_multiplication(mtx_Bf_ptr, friction_out, mtx_tmp2_ptr, 2, 1, 1);
matrix_addition(mtx_tmp1_ptr, mtx_tmp2_ptr, mtx_tmp3_ptr, 2, 1);
matrix_scalar_multiplication(mtx_Bd_ptr, motor_input, mtx_tmp2_ptr, 2, 1);
matrix_addition(mtx_tmp2_ptr, mtx_tmp3_ptr, mtx_tmp1_ptr, 2, 1);
matrix_assignment(mtx_tmp1_ptr, motor_state, 2, 1);
return;
}
void
test_cases()
{
double start_time = 0.0f, end_time = 0.0f;
for (int i = 0; i < TestFrameWork::size; i++)
{
std::cout<<"\n"<<"Test Case "<<i+1;
result = new float[ TestFrameWork::matrix_dim[i] * TestFrameWork::matrix_dim[i] ];
sq_matrix_1 = new float[matrix_dim[i] * matrix_dim[i]];
sq_matrix_2 = new float[matrix_dim[i] * matrix_dim[i]];
answers = new float[matrix_dim[i] * matrix_dim[i]];
randomize(sq_matrix_1, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]);
randomize(sq_matrix_2, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]);
start_time = wtime();
matrix_multiplication(TestFrameWork::sq_matrix_1, TestFrameWork::sq_matrix_2, result, TestFrameWork::matrix_dim[i]);
end_time = wtime();
std::cout<<"\t"<< (end_time - start_time)<<" milliseconds\n";
tripleloop(answers, sq_matrix_1, sq_matrix_2, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]);
CPPUNIT_ASSERT(TestFrameWork::diff(answers, result, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]) < EPS);
delete sq_matrix_1;
delete sq_matrix_2;
delete answers;
delete result;
}
}
MATRIX * minimacs_fft_shurgenerator(uint lmsg, uint lcode, byte root) {
MATRIX * res = 0;
MATRIX * small = build_nth_matrix(lmsg,lmsg,root);
MATRIX * smallinv = LUInverse(small);
MATRIX * big = build_nth_matrix(lcode, lmsg, root);
res = matrix_multiplication( big, smallinv);
return res;
}
/* This function checks if a matrix is orthogonal
* returns -1 if it can't be checked
* returns 0 if it isn't orthogonal
* returns 1 if it is orthogonal */
int is_orthogonal(struct matrix m)
{
if(m.rows!=m.columns)
{
printf("This matrix isn't square\n");
return -1;
}
struct matrix c=traspose(m);
c=matrix_multiplication(m,c);
return compare_matrix(c,identity_matrix(c.rows));
}
void iterate(float **M, int size)
{
float **P, **M_inv, **res, pivot, **B;
int c_size = 0, i, j;
res = (float **)malloc(size * sizeof(float *));
M_inv = (float **)malloc(size * sizeof(float *));
P = (float **)malloc(sizeof(float *));
iden(M_inv, size);
while (c_size < size)
{
printf("iteration number: %d\n", c_size+1);
B = (float **)malloc(size * sizeof(float *));
iden(B, size);
extract_col_vector(M, P, c_size, size);
matrix_multiplication(M_inv, P, res, size, size, 1);
pivot = res[c_size][0];
for(i=0; i<size; i++)
{
if(i==c_size)
res[i][0] = 1 / pivot;
else
res[i][0] = -(res[i][0]/pivot);
}
printf("c^\n");
display(res, size, 1);
replace_col_vector(B, res, c_size, size);
//res is getting changed over here
matrix_multiplication(B, M_inv, res, size, size, size);
copy_mat(res, M_inv, size);
printf("B\n");
display(B, size, size);
printf("M_inv\n");
display(M_inv, size, size);
c_size++;
}
}
void
test_cases()
{
double start_time = 0.0f, end_time = 0.0f;
for (int i = 0; i < TestFrameWork::size; i++)
{
std::cout<<"\n"<<"Test Case "<<i+1;
result = new float[ TestFrameWork::matrix_dim[i] * TestFrameWork::matrix_dim[i] ];
sq_matrix_1 = new float[matrix_dim[i] * matrix_dim[i]];
sq_matrix_2 = new float[matrix_dim[i] * matrix_dim[i]];
answers = new float[matrix_dim[i] * matrix_dim[i]];
randomize(sq_matrix_1, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]);
randomize(sq_matrix_2, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]);
matrix_multiplication(TestFrameWork::sq_matrix_1, TestFrameWork::sq_matrix_2, result, TestFrameWork::matrix_dim[i]);
tripleloop(answers, sq_matrix_1, sq_matrix_2, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]);
CPPUNIT_ASSERT(TestFrameWork::diff(answers, result, TestFrameWork::matrix_dim[i], TestFrameWork::matrix_dim[i]) < EPS);
start_time = wtime();
for (int j = 0; j < NUM_REPS; ++j) {
matrix_multiplication(TestFrameWork::sq_matrix_1, TestFrameWork::sq_matrix_2, result, TestFrameWork::matrix_dim[i]);
}
end_time = wtime();
double t = (double)(end_time - start_time)/(double)(1000.0*NUM_REPS);
double gflops = (double)((2*TestFrameWork::matrix_dim[i]-1)*TestFrameWork::matrix_dim[i]*TestFrameWork::matrix_dim[i]*1e-9/t);
std::cout<<"\t"<< gflops <<" Gflop/s\n";
delete sq_matrix_1;
delete sq_matrix_2;
delete answers;
delete result;
}
}
static Aes
shift_row_mix_cols(OE oe, Aes aes) {
MATRIX * state = load_matrix(oe,aes->state,16,1);
//MATRIX * SRMC = load_matrix(oe, (byte*)srmc, 16, 16 );
MATRIX * SR = load_matrix(oe, (byte*)tbl_shift_rows, 16, 16);
MATRIX * MC = load_matrix(oe, (byte*)tbl_mix_columns, 16, 16);
MATRIX * SRMC = matrix_multiplication(MC, SR);
MATRIX * res = matrix_multiplication(SRMC,state);
uint i = 0;
Aes aesres = 0;
destroy_matrix(SR);
destroy_matrix(MC);
destroy_matrix(SRMC);
destroy_matrix(state);
aesres = Aes_new(oe);
for(i = 0;i < 16;++i) {
aesres->state[i] = matrix_getentry(res,i,0);
}
destroy_matrix(res);
return aesres;
}
int main(int argc,char* argv[])
{
//order of matrix1 is argv[1]xargv[2]
//order of matrix2 is argv[2]xargv[3]
int rows_mat_a=atoi(argv[1]);
int rows_mat_b=atoi(argv[2]);
int columns_mat_b=atoi(argv[3]);
input_matrixA("matrixA",rows_mat_a,rows_mat_b);
input_matrixB("matrixB",rows_mat_b,columns_mat_b);
input_matrixA("matrixC",rows_mat_a,columns_mat_b);
printf("ready for multiplication\n");
struct timeval t1;
gettimeofday(&t1,NULL);
printf("going to multiply\n");
matrix_multiplication(rows_mat_a,rows_mat_b,columns_mat_b);
printf("multiplication completed\n");
struct timeval t2;
gettimeofday(&t2,NULL);
if(t2.tv_usec<t1.tv_usec)
{
//printf("time manipuation\n");
t2.tv_sec-=1;
t2.tv_usec=1000000+t2.tv_usec;
}
//display of matrix C
/*
fdc=open("matC",O_RDONLY,0666);
int x,y,s;
for(x=0;x<rowsa;x++)
{
for(y=0;y<cols;y++)
{
read(fdc,&s,sizeof(int));
printf("%d ",s);
}
printf("\n");
}
close(fdc);
*/
printf("time taken=%ld.%.6ldsec\n",(long)(t2.tv_sec-t1.tv_sec),(long)(t2.tv_usec-t1.tv_usec));
return 0;
}
/* This function returns the matrix m elevated to the nth
* power by multiplying */
struct matrix pow_matrix(struct matrix m, unsigned int n)
{
if(m.rows!=m.columns)
{
printf("This matrix can't be powered\n");
printf("It isn't square\n");
return create_matrix(0,0);
}
unsigned int i;
struct matrix c=copy_matrix(m);
for (i = 0; i < n; i++)
matrix_multiplication(c,m);
return c;
}
static Aes
mix_columns(OE oe, Aes aes) {
Aes res = Aes_new(oe);
MATRIX * MC = load_matrix(oe,(byte*)tbl_mix_columns,16,16);
MATRIX * V = load_matrix(oe,aes->state,16,1);
MATRIX * R = matrix_multiplication(MC,V);
uint i = 0;
destroy_matrix(MC);
destroy_matrix(V);
for(i = 0;i < 16;++i) {
res->state[i] = matrix_getentry(R,i,0);
}
destroy_matrix(R);
return res;
}
static Aes
shift_rows(OE oe, Aes aes) {
Aes res = Aes_new(oe);
MATRIX * SR = load_matrix(oe,(byte*)tbl_shift_rows,16,16);
MATRIX * V = load_matrix(oe,aes->state,16,1);
MATRIX * R = matrix_multiplication(SR,V);
uint i = 0;
destroy_matrix(SR);
destroy_matrix(V);
for(i = 0;i < 16;++i) {
res->state[i] = matrix_getentry(R,i,0);
}
destroy_matrix(R);
return res;
}
//Forward pass
void LinearLayer::forward() {
assert(has_bottom_layer());
Data* out_bottom = get_bottom_layer()->get_output();
// std::cout << "bottom begin" << std::endl;
// out_bottom->print();
// std::cout << "bottom end" << std::endl;
//TODO: do multiplication in one GEMM step?
//flatten any data dimensions to a single dimension
std::unique_ptr<Data> out_bottom_flat = out_bottom->flatten_to_matrix();
std::unique_ptr<Data> output_flat = m_output->flatten_to_matrix();
//linear combination of the inputs
MatrixMultiplicationMKL matrix_multiplication(
out_bottom_flat.get(),
m_weights,
output_flat.get());
matrix_multiplication.execute();
//add bias to each batch
PlusEqualRow().execute(output_flat.get(), m_bias);
}
int main()
{
matrix_t mat_a, mat_b;
matrix_t mat_c;
struct timeval start_time, end_time;
random_matrix(&mat_a, 4);
random_matrix(&mat_b, 4);
null_matrix(&mat_c, 4);
print_matrix(mat_a);
printf("\n");
print_matrix(mat_b);
printf("\n");
print_matrix(mat_c);
gettimeofday(&start_time, 0);
matrix_multiplication(mat_a, mat_b, mat_c);
gettimeofday(&end_time, 0);
printf("Normal Multiplication\n");
print_matrix(mat_c);
print_time_taken(start_time, end_time);
mat_c = set_zero(mat_c);
mat_c = matrix_multiplication_strassen(mat_a, mat_b, mat_c, 2);
printf("Strassen Multiplication\n");
print_matrix(mat_c);
}
void iterate(float **M, int m, int n)
{
int e_var, l_var, count=1, *track, i, j, t;
float **B, **b, **Y, **P, **Cb, **YP, yp, **A, **Xb, **Z, min_zc, min_A, temp=0;
//basis matrix
B = (float **)malloc(m * sizeof(float *));
//constraint rhs matrix
b = (float **)malloc(m * sizeof(float *));
copy_mat(M, b, 0, m-1, n+m, n+m);
//supporting matrices
track = (int*)malloc((n+m) * sizeof(int));
Y = (float **)malloc(sizeof(float *));
Cb = (float **)malloc(sizeof(float *));
P = (float **)malloc(m * sizeof(float *));
YP = (float **)malloc(sizeof(float *));
A = (float **)malloc(m * sizeof(float *));
Xb = (float **)malloc(m * sizeof(float *));
Z = (float **)malloc(sizeof(float *));
//forming the tracking matrix
for(i=0; i<n+m; i++)
track[i] = i;
do
{
min_zc=0;
min_A=999;
printf("\n\niteration number: %d\n",count++);
//computing inverse of the basis matrix
copy_mat(M, B, 0, m-1, n, n+m-1);
inverse(B, m);
printf("inverse of the basis matrix\n");
display(B, m, m);
//forming the Cb matrix
copy_mat(M, Cb, m, m, n, n+m-1);
for(j=0; j<m; j++)
Cb[0][j] = -Cb[0][j];
//computing the Xb matrix
matrix_multiplication(B, b, Xb, m, m, 1);
//computing the Z matrix
matrix_multiplication(Cb, Xb, Z, 1, m, 1);
//replacing Xb and Z into M
for(i=0; i<m+1; i++)
{
if(i<m)
M[i][n+m] = Xb[i][0];
if(i==m)
M[i][n+m] = Z[0][0];
}
printf("extended revised simplex table\n");
display(M, m+1, n+m+1);
//computing the Y matrix
matrix_multiplication(Cb, B, Y, 1, m, m);
//determining the entering variable
for(j=0; j<m; j++)
{
copy_mat(M, P, 0, m-1, j, j);
matrix_multiplication(Y, P, YP, 1, m, 1);
YP[0][0] = YP[0][0] + M[m][j];
if(YP[0][0] < min_zc)
{
min_zc = YP[0][0];
e_var = j;
}
}
if(min_zc < 0)
{
//determining the leaving variable
copy_mat(M, P, 0, m-1, e_var, e_var);
matrix_multiplication(B, P, A, m, m, 1);
for(j=0; j<m; j++)
{
if(A[j][0] > 0)
{
temp = Xb[j][0] / A[j][0];
if(temp > 0 && temp < min_A)
{
min_A = temp;
l_var = j+n;
}
}
}
if(min_A == 0)
{
printf("the problem is unbounded\n");
exit(0);
}
printf("entering variable = x%d\nleaving variable = x%d\n", track[e_var]+1, track[l_var]+1);
for(i=0; i<m+1; i++)
{
temp = M[i][e_var];
M[i][e_var] = M[i][l_var];
M[i][l_var] = temp;
}
//updating the hash table
t = track[e_var];
track[e_var] = track[l_var];
track[l_var] = t;
}
}while(min_zc < 0);
for(i=m; i<n+m; i++)
{
if(track[i] < m)
printf("x%d = %7.2f\n", track[i]+1, Xb[i-m][0]);
}
printf("all the other variables in the objective function have zero value.\noptimal value of the objective function = %7.2f\n\n",Z[0][0]);
}
matrix_t matrix_multiplication_strassen(matrix_t mat_a, matrix_t mat_b,
matrix_t mat_c, int min_thres)
{
int len = mat_a.row_end - mat_a.row_start + 1;
matrix_t p1, p2, p3, p4, p5, p6, p7;
matrix_t c_copy = mat_c;
/* if (len != pow(2, log2(len))) { */
/* printf("Strassen Algorithm not applicable\n"); */
/* return mat_c; */
/* } */
if (len <= min_thres) {
return matrix_multiplication(mat_a, mat_b, mat_c);
} else {
printf("Divide and Conquer\n");
matrix_init(&p1, &p2, &p3, &p4, &p5, &p6, &p7, len/2);
matrix_multiplication_strassen(partition_matrix(mat_a, 1, 1),
matrix_subtraction(partition_matrix(mat_b, 1, 2),
partition_matrix(mat_b, 2, 2), p1),
p1, min_thres);
matrix_multiplication_strassen(partition_matrix(mat_b, 2, 2),
matrix_addition(partition_matrix(mat_a, 1, 1),
partition_matrix(mat_a, 1, 2), p2),
p2, min_thres);
matrix_multiplication_strassen(partition_matrix(mat_b, 1, 1),
matrix_addition(partition_matrix(mat_a, 2, 1),
partition_matrix(mat_a, 2, 2), p3),
p3, min_thres);
matrix_multiplication_strassen(partition_matrix(mat_a, 2, 2),
matrix_subtraction(partition_matrix(mat_b, 2, 1),
partition_matrix(mat_b, 1, 1), p3),
p3, min_thres);
matrix_addition(matrix_multiplication_strassen(partition_matrix(mat_a, 1, 1),
matrix_addition(partition_matrix(mat_b, 1, 1),
partition_matrix(mat_b, 2, 2),
p5), p5, min_thres),
matrix_multiplication_strassen(partition_matrix(mat_a, 2, 2),
matrix_addition(partition_matrix(mat_b, 1, 1),
partition_matrix(mat_b, 2, 2),
p5), p5, min_thres),
p5);
matrix_subtraction(matrix_multiplication_strassen(partition_matrix(mat_a, 1, 2),
matrix_addition(partition_matrix(mat_b, 2, 1),
partition_matrix(mat_b, 2, 2),
p6), p6, min_thres),
matrix_multiplication_strassen(partition_matrix(mat_a, 2, 2),
matrix_addition(partition_matrix(mat_b, 2, 1),
partition_matrix(mat_b, 2, 2),
p6), p6, min_thres),
p6);
matrix_subtraction(matrix_multiplication_strassen(partition_matrix(mat_a, 1, 1),
matrix_addition(partition_matrix(mat_b, 1, 1),
partition_matrix(mat_b, 1, 2),
p7), p7, min_thres),
matrix_multiplication_strassen(partition_matrix(mat_a, 2, 1),
matrix_addition(partition_matrix(mat_b, 1, 1),
partition_matrix(mat_b, 1, 2),
p7), p7, min_thres),
p7);
matrix_addition(matrix_addition(p5, p4, partition_matrix(mat_c, 1, 1)),
matrix_subtraction(p6, p2, partition_matrix(mat_c, 1, 1)),
partition_matrix(mat_c, 1, 1));
matrix_addition(p1, p2, partition_matrix(mat_c, 1, 2));
matrix_addition(p3, p4, partition_matrix(mat_c, 2, 1));
matrix_subtraction(matrix_addition(p5, p1, partition_matrix(mat_c, 2, 2)),
matrix_addition(p3, p7, partition_matrix(mat_c, 2, 2)),
partition_matrix(mat_c, 2, 2));
mat_c.row_start = c_copy.row_start;
mat_c.row_end = c_copy.row_end;
mat_c.column_start = c_copy.column_start;
mat_c.column_end = c_copy.column_end;
print_matrix(mat_c);
return mat_c;
}
}
void ram(void) {
int i;
frame = 0;
struct vertex2d projected[NUM_VERTICES];
float matrix_projection[16];
init_matrix_projection_fov(matrix_projection, DEG2RAD(2), 0.1, 50, 1);
while (true) {
lcdFill(COLOR_BACKGROUND);
float mat_a[16];
float mat_b[16];
float mat_c[16];
float matrix_worldviewprojection[16];
init_vertices(vertices);
// Calculate worldviewprojection matrix by concatenating the
// following operations:
// - rotate y axis
// - rotate x axis
// - translate
// - project
float rad = (float)frame * 0.015f;
init_matrix_rotation_y(mat_a, rad);
init_matrix_rotation_x(mat_b, -rad*0.8);
matrix_multiplication(mat_a, mat_b, mat_c);
init_matrix_translation(mat_a, 0, -2, 11.5f);
matrix_multiplication(mat_c, mat_a, mat_b);
matrix_multiplication(mat_b, matrix_projection, matrix_worldviewprojection);
// Transform vertices
for (i=0; i<NUM_VERTICES; i++) {
struct vertex transformed;
transform_vertex(&vertices[i], &transformed, matrix_worldviewprojection);
if (transformed.w > -0.0001)
projected[i].x = VERTEX_BEHIND_CAMERA;
else {
projected[i].x = (int)(transformed.x / transformed.w) + SCREEN_WIDTH / 2;
projected[i].y = (int)(transformed.y / transformed.w) + SCREEN_HEIGHT / 2;
}
}
// Draw line grid
struct vertex2d from, to;
for (int x=0; x<TESSELATION; x++)
for (int y=0; y<TESSELATION; y++)
{
if (x < (TESSELATION - 1)) {
from = projected[x + y * TESSELATION];
to = projected[x + y * TESSELATION + 1];
line(from.x, from.y, to.x, to.y, COLOR_LINE);
}
if (y < (TESSELATION - 1)) {
from = projected[x + y * TESSELATION];
to = projected[x + y * TESSELATION + TESSELATION];
line(from.x, from.y, to.x, to.y, COLOR_LINE);
}
}
lcdDisplay();
rad += 0.015;
frame++;
int key = getInputRaw();
if (key == BTN_ENTER)
break;
}
}
int main()
{
int A[m][m]={0},B[m][m]={0},C[m][m]={0};
int TA[m][m]={0}; // TA = traspose(A)
int r,c; //r:row index, c:column index
int i,j;
// Ask the user to enter the matrix
printf("The dimension of matrix is 3\n");
printf("A=\n");
// --- write your codes here to get matrix A---
for(i=0; i<m; i++)
for(j=0; j<m; j++)
scanf(" %d", &A[i][j]);
printf("B=\n");
// --- write your codes here to get matrix B---
for(i=0; i<m; i++)
for(j=0; j<m; j++)
scanf(" %d", &B[i][j]);
//copy A to TA
for (r=0; r<m;r++){
for (c=0; c<m;c++){
TA[r][c]=A[r][c];
}
}
// call the two functions for transpose(A) and C = transpose(A)*B
matrix_transpose(&TA, m);
matrix_multiplication(&TA, &B, &C, m);
system("cls");
printf("A=\n");
int CNT=0;
// --- write your codes here to print out A ---
for(i=0; i<m; i++){
for(j=0; j<m; j++){
printf("%d ", A[i][j]);
CNT++;
if(CNT == m){
printf("\n");
CNT=0;
}
}
}
printf("\n");
printf("B=\n");
// --- write your codes here to print out B ---
for(i=0; i<m; i++){
for(j=0; j<m; j++){
printf("%d ", B[i][j]);
CNT++;
if(CNT == m){
printf("\n");
CNT=0;
}
}
}
printf("\n");
printf("C=\n");
// --- write your codes here to print out C ---
for(i=0; i<m; i++){
for(j=0; j<m; j++){
printf("%d ", C[i][j]);
CNT++;
if(CNT == m){
printf("\n");
CNT=0;
}
}
}
system("PAUSE");
return 0;
}
int main()
{
int n; // size of square matrix
int **A=NULL,**B=NULL,**C=NULL;
int **TA=NULL; // TA = traspose(A)
printf("Please input the size of matrix:");
scanf("%d",&n);
_flushall();
// Call the function - AllocIntMatrix to get the memory for matrix A and B
// --- write your codes here ---
A=AllocIntMatrix(n);
B=AllocIntMatrix(n);
int i,j;
// Ask the user to enter the matrix
printf("A=\n");
// --- write your codes here to get matrix A---
for(i=0; i<n; i++)
for(j=0; j<n; j++)
scanf(" %d", &A[i][j]);
printf("B=\n");
// --- write your codes here to get matrix B---
for(i=0; i<n; i++)
for(j=0; j<n; j++)
scanf(" %d", &B[i][j]);
// call the two functions for transpose(A) and C = transpose(A)*B
TA=matrix_transpose(&A[0], n); // share the ownership of the heap memory (i.e., A) with matrix_transpose() by call by reference (pointer)
// obtain the ownership of the heap memory (i.e., TA) requested in matrix_transpose()
//multiplication
C=matrix_multiplication(&TA[0], &B[0], n);// share the ownership of the heap memory (i.e., A, B) with matrix_multiplication() by call by reference (pointer)
// obtain the ownership of the heap memory (i.e., C) requested in matrix_multiplication()
system("cls");
printf("A=\n");
// --- write your codes here to print out A ---
int CNT=0;
for(i=0; i<n; i++){
for(j=0; j<n; j++){
printf("%d ", A[i][j]);
CNT++;
if(CNT == n){
printf("\n");
CNT=0;
}
}
}
printf("\n");
printf("TA=\n");
// --- write your codes here to print out TA ---
for(i=0; i<n; i++){
for(j=0; j<n; j++){
printf("%d ", TA[i][j]);
CNT++;
if(CNT == n){
printf("\n");
CNT=0;
}
}
}
printf("\n");
printf("B=\n");
// --- write your codes here to print out B ---
for(i=0; i<n; i++){
for(j=0; j<n; j++){
printf("%d ", B[i][j]);
CNT++;
if(CNT == n){
printf("\n");
CNT=0;
}
}
}
printf("\n");
printf("C=\n");
// --- write your codes here to print out C ---
for(i=0; i<n; i++){
for(j=0; j<n; j++){
printf("%d ", C[i][j]);
CNT++;
if(CNT == n){
printf("\n");
CNT=0;
}
}
}
// release heap memory explicitly preventing memory leakage
FreeIntMatrix(A ,n); // release A
FreeIntMatrix(B ,n); // relase B
FreeIntMatrix(C ,n); // relase C
FreeIntMatrix(TA ,n); // release TA
A = B = C = TA = NULL;
system("PAUSE");
return 0;
}
// Function to make desired output of this c file from inputs
void predict_phys(mxArray *plhs[], mxArray *prhs[]){
double muR, muL, betaR, betaL, gammaR, gammaL, alphaR, alphaL, turnRate, turnRateInPlace, turnReductionRate, wheelbase;
double dt, turnRateModified;
double *param_array_ptr, *data_input_ptr; // pointer for input matrices
double *vs_ptr, *omegas_ptr, *us_joystick_ptr, *qs_ptr, *us_motor_ptr, *fs_ptr; // pointer for output matrices
double *q_l_ptr, *q_r_ptr, *u_joystick_ptr, *u_motor_ptr, *input_trans_mtx_ptr, *u_friction_r_ptr, *u_friction_l_ptr;
mxArray *q_l, *q_r, *u_joystick, *u_motor, *input_trans_mtx, *u_friction_r, *u_friction_l;
int num_data_cases, num_pred_step, data_cnt, pred_cnt;
// model parameter set-up
param_array_ptr = mxGetPr(prhs[0]);
muR = param_array_ptr[0];
betaR = param_array_ptr[1];
gammaR = param_array_ptr[2];
alphaR = param_array_ptr[3];
muL = param_array_ptr[4];
betaL = param_array_ptr[5];
gammaL = param_array_ptr[6];
alphaL = param_array_ptr[7];
turnRate = param_array_ptr[8];
turnRateInPlace = param_array_ptr[8];
turnReductionRate = param_array_ptr[9];
wheelbase = param_array_ptr[10];
// dealing with other input arguments
num_data_cases = mxGetM(prhs[1]);
data_input_ptr = mxGetPr(prhs[1]);
num_pred_step = mxGetScalar(prhs[2]);
// output variable set-up
plhs[0] = mxCreateDoubleMatrix(num_data_cases, num_pred_step, mxREAL);
vs_ptr = mxGetPr(plhs[0]);
plhs[1] = mxCreateDoubleMatrix(num_data_cases, num_pred_step, mxREAL);
omegas_ptr = mxGetPr(plhs[1]);
plhs[2] = mxCreateDoubleMatrix(num_data_cases, 2*num_pred_step, mxREAL);
us_joystick_ptr = mxGetPr(plhs[2]);
plhs[3] = mxCreateDoubleMatrix(num_data_cases, 2*num_pred_step, mxREAL);
qs_ptr = mxGetPr(plhs[3]);
plhs[4] = mxCreateDoubleMatrix(num_data_cases, 2*num_pred_step, mxREAL);
us_motor_ptr = mxGetPr(plhs[4]);
plhs[5] = mxCreateDoubleMatrix(num_data_cases, 2*num_pred_step, mxREAL);
fs_ptr = mxGetPr(plhs[5]);
// intermediate variable set-up
dt = (double)1/25;
q_l = mxCreateDoubleMatrix(2, 1, mxREAL);
q_l_ptr = mxGetPr(q_l);
q_r = mxCreateDoubleMatrix(2, 1, mxREAL);
q_r_ptr = mxGetPr(q_r);
u_joystick = mxCreateDoubleMatrix(2, 1, mxREAL);
u_joystick_ptr = mxGetPr(u_joystick);
u_motor = mxCreateDoubleMatrix(2, 1, mxREAL);
u_motor_ptr = mxGetPr(u_motor);
u_friction_r = mxCreateDoubleMatrix(1, 1, mxREAL);
u_friction_r_ptr = mxGetPr(u_friction_r);
u_friction_l = mxCreateDoubleMatrix(1, 1, mxREAL);
u_friction_l_ptr = mxGetPr(u_friction_l);
input_trans_mtx = mxCreateDoubleMatrix(2, 2, mxREAL);
input_trans_mtx_ptr = mxGetPr(input_trans_mtx);
input_trans_mtx_ptr[0] = 1;
input_trans_mtx_ptr[1] = 1;
for(data_cnt=0; data_cnt<num_data_cases; data_cnt++){
// get initial state from input data matrix
q_l_ptr[0] = data_input_ptr[data_cnt];
q_l_ptr[1] = data_input_ptr[data_cnt+num_data_cases];
q_r_ptr[0] = data_input_ptr[data_cnt+2*num_data_cases];
q_r_ptr[1] = data_input_ptr[data_cnt+3*num_data_cases];
for(pred_cnt=0; pred_cnt<num_pred_step; pred_cnt++){
u_joystick_ptr[0] = data_input_ptr[data_cnt+(2*pred_cnt+4)*num_data_cases];
u_joystick_ptr[1] = data_input_ptr[data_cnt+(2*pred_cnt+5)*num_data_cases];
turnRateModified = turnRate*(1-turnReductionRate*u_joystick_ptr[0]);
if(u_joystick_ptr[0]<0.00001 && u_joystick_ptr[0]>-0.00001){
input_trans_mtx_ptr[2] = turnRateInPlace;
input_trans_mtx_ptr[3] = -turnRateInPlace;
}
else{
input_trans_mtx_ptr[2] = turnRateModified;
input_trans_mtx_ptr[3] = -turnRateModified;
}
matrix_multiplication(input_trans_mtx_ptr, u_joystick_ptr, u_motor_ptr, 2, 1, 2);
// use motor model to update motor states
motor_model_c(q_r_ptr, u_friction_r_ptr, u_motor_ptr[0], dt, muR, betaR, gammaR, alphaR);
motor_model_c(q_l_ptr, u_friction_l_ptr, u_motor_ptr[1], dt, muL, betaL, gammaL, alphaL);
// save result
vs_ptr[data_cnt+pred_cnt*num_data_cases] = (q_r_ptr[0] + q_l_ptr[0])*0.5;
omegas_ptr[data_cnt+pred_cnt*num_data_cases] = (q_r_ptr[0] - q_l_ptr[0])/wheelbase;
us_joystick_ptr[data_cnt+2*pred_cnt*num_data_cases] = u_joystick_ptr[0];
us_joystick_ptr[data_cnt+(2*pred_cnt+1)*num_data_cases] = u_joystick_ptr[1];
qs_ptr[data_cnt+2*pred_cnt*num_data_cases] = q_l_ptr[0];
qs_ptr[data_cnt+(2*pred_cnt+1)*num_data_cases] = q_r_ptr[0];
us_motor_ptr[data_cnt+2*pred_cnt*num_data_cases] = u_motor_ptr[0];
us_motor_ptr[data_cnt+(2*pred_cnt+1)*num_data_cases] = u_motor_ptr[1];
fs_ptr[data_cnt+2*pred_cnt*num_data_cases] = u_friction_l_ptr[0];
fs_ptr[data_cnt+(2*pred_cnt+1)*num_data_cases] = u_friction_r_ptr[0];
}
}
return;
}