Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
0
// 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;
}
Ejemplo n.º 3
0
    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;
	}
    }
Ejemplo n.º 4
0
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));
}
Ejemplo n.º 6
0
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++;
    }
}
Ejemplo n.º 7
0
    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;
        }
    }
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 11
0
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;
}
Ejemplo n.º 12
0
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;
}
Ejemplo n.º 13
0
//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);
}
Ejemplo n.º 14
0
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);
}
Ejemplo n.º 15
0
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]);
}
Ejemplo n.º 16
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;

    }
}
Ejemplo n.º 17
0
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;
    }
}
Ejemplo n.º 18
0
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;
		
	
}
Ejemplo n.º 19
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;
		
	
}
Ejemplo n.º 20
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;
}