uint64_t fibonacci( uint64_t n, uint64_t module ) {
uint64_t power[] = { 0, 1, 1, 1 };
uint64_t result[] = { 1, 0, 0, 1 };
while ( n ) {
if ( n & 1 ) {
mult_matrix( result, power, module );
}
mult_matrix( power, power, module );
n >>= 1;
}
return result[1];
}
void translate_matrix(double (*im)[4], double (*om)[4], struct vector3 *translate)
{
double tm[4][4];
unsigned short l,m,n;
tm[0][0]=1;
tm[0][1]=0;
tm[0][2]=0;
tm[0][3]=0;
tm[1][0]=0;
tm[1][1]=1;
tm[1][2]=0;
tm[1][3]=0;
tm[2][0]=0;
tm[2][1]=0;
tm[2][2]=1;
tm[2][3]=0;
tm[3][0]=translate->x;
tm[3][1]=translate->y;
tm[3][2]=translate->z;
tm[3][3]=1;
l=4;
m=4;
n=4;
mult_matrix(im,tm,om,l,m,n);
}
void f_rotate_z (void)
{
array_t *matrix;
double angle;
Matrix current_matrix;
Matrix rot_matrix;
Matrix final_matrix;
int i;
/*
* get arguments from stack.
*/
matrix = (sp - 1)->u.arr;
angle = (sp--)->u.real;
/*
* convert vec matrix to float matrix.
*/
for (i = 0; i < 16; i++) {
current_matrix[i] = matrix->item[i].u.real;
}
/*
* create z rotation matrix.
*/
rotate_z_matrix(angle, rot_matrix);
/*
* compute transformed matrix.
*/
mult_matrix(current_matrix, rot_matrix, final_matrix);
/*
* convert float matrix to vec matrix.
*/
for (i = 0; i < 16; i++) {
matrix->item[i].u.real = final_matrix[i];
}
}
/**
* Scales the rows of a matrix according to scaling vector3.
* Scales row0 of im by scale.x
* Scales row1 of im by scale.y
* Scales row2 of im by scale.z
* Scales row3 of im by 1 (no scaling)
* Result is placed in om.
*/
void scale_matrix(double (*im)[4], double (*om)[4], struct vector3 *scale)
{
double sc[4][4];
unsigned short l,m,n;
sc[0][0]=scale->x;
sc[0][1]=0;
sc[0][2]=0;
sc[0][3]=0;
sc[1][0]=0;
sc[1][1]=scale->y;
sc[1][2]=0;
sc[1][3]=0;
sc[2][0]=0;
sc[2][1]=0;
sc[2][2]=scale->z;
sc[2][3]=0;
sc[3][0]=0;
sc[3][1]=0;
sc[3][2]=0;
sc[3][3]=1;
l=4;
m=4;
n=4;
mult_matrix(im,sc,om,l,m,n);
}
void calculate_view(t_3dview *view)
{
#define SMALL 1e-6
mat4 To,Te,T1,T2,T3,T4,T5,N1,D1,D2,D3,D4,D5;
real dx,dy,dz,l,r;
/* eye center */
dx=view->eye[XX];
dy=view->eye[YY];
dz=view->eye[ZZ];
l = sqrt(dx*dx+dy*dy+dz*dz);
r = sqrt(dx*dx+dy*dy);
#ifdef DEBUG
print_v4(debug,"eye",N,view->eye);
printf("del: %10.5f%10.5f%10.5f l: %10.5f, r: %10.5f\n",dx,dy,dz,l,r);
#endif
if (l < SMALL)
fatal_error(0,"Error: Zero Length Vector - No View Specified");
translate((real)(-view->origin[XX]),
(real)(-view->origin[YY]),(real)(-view->origin[ZZ]),To);
translate((real)(-view->eye[XX]),
(real)(-view->eye[YY]),(real)(-view->eye[ZZ]),Te);
unity_m4(T2);
T2[YY][YY]=0, T2[YY][ZZ]=-1, T2[ZZ][YY]=1, T2[ZZ][ZZ]=0;
unity_m4(T3);
if (r > 0)
T3[XX][XX]=-dy/r, T3[XX][ZZ]=dx/r, T3[ZZ][XX]=-dx/r, T3[ZZ][ZZ]=-dy/r;
unity_m4(T4);
T4[YY][YY]=r/l, T4[YY][ZZ]=dz/l, T4[ZZ][YY]=-dz/l, T4[ZZ][ZZ]=r/l;
unity_m4(T5);
T5[ZZ][ZZ]=-1;
unity_m4(N1);
/* N1[XX][XX]=4,N1[YY][YY]=4; */
mult_matrix(T1,To,view->Rot);
mult_matrix(D1,Te,T2);
mult_matrix(D2,T3,T4);
mult_matrix(D3,T5,N1);
mult_matrix(D4,T1,D1);
mult_matrix(D5,D2,D3);
mult_matrix(view->proj,D4,D5);
#ifdef DEBUG
print_m4(debug,"T1",T1);
print_m4(debug,"T2",T2);
print_m4(debug,"T3",T3);
print_m4(debug,"T4",T4);
print_m4(debug,"T5",T5);
print_m4(debug,"N1",N1);
print_m4(debug,"Rot",view->Rot);
print_m4(debug,"Proj",view->proj);
#endif
}
static void rand_rot(int natoms, rvec x[], rvec v[], vec4 xrot[], vec4 vrot[],
int *seed, rvec max_rot)
{
mat4 mt1, mt2, mr[DIM], mtemp1, mtemp2, mtemp3, mxtot, mvtot;
rvec xcm;
real phi;
int i, m;
clear_rvec(xcm);
for (i = 0; (i < natoms); i++)
{
for (m = 0; (m < DIM); m++)
{
xcm[m] += x[i][m]/natoms; /* get center of mass of one molecule */
}
}
fprintf(stderr, "center of geometry: %f, %f, %f\n", xcm[0], xcm[1], xcm[2]);
translate(-xcm[XX], -xcm[YY], -xcm[ZZ], mt1); /* move c.o.ma to origin */
for (m = 0; (m < DIM); m++)
{
phi = M_PI*max_rot[m]*(2*rando(seed) - 1)/180;
rotate(m, phi, mr[m]);
}
translate(xcm[XX], xcm[YY], xcm[ZZ], mt2);
/* For mult_matrix we need to multiply in the opposite order
* compared to normal mathematical notation.
*/
mult_matrix(mtemp1, mt1, mr[XX]);
mult_matrix(mtemp2, mr[YY], mr[ZZ]);
mult_matrix(mtemp3, mtemp1, mtemp2);
mult_matrix(mxtot, mtemp3, mt2);
mult_matrix(mvtot, mr[XX], mtemp2);
for (i = 0; (i < natoms); i++)
{
m4_op(mxtot, x[i], xrot[i]);
m4_op(mvtot, v[i], vrot[i]);
}
}
void rotate_3d(t_3dview *view,int axis,bool bPositive)
{
static bool bFirst=TRUE;
static mat4 RotP[DIM];
static mat4 RotM[DIM];
int i,j;
mat4 m4;
if (bFirst) {
real rot=DEG2RAD*15;
for(i=0; (i<DIM); i++) {
rotate(i, rot ,RotP[i]);
rotate(i,(real)(-rot),RotM[i]);
#ifdef DEBUG
print_m4(debug,"RotP",RotP[i]);
print_m4(debug,"RotM",RotM[i]);
#endif
}
}
/*
if (bPositive)
m4_op(RotP[axis],view->eye,v4);
else
m4_op(RotM[axis],view->eye,v4);
for(i=0; (i<DIM); i++)
view->eye[i]=v4[i];
*/
if (bPositive)
mult_matrix(m4,view->Rot,RotP[axis]);
else
mult_matrix(m4,view->Rot,RotM[axis]);
for(i=0; (i<N); i++)
for(j=0; (j<N); j++)
view->Rot[i][j]=m4[i][j];
calculate_view(view);
}
//! \brief Calculate the Graph Potentials of a molecule
//!
//! based on
//! V.E. and Rozenblit, A.B. Golender
//! <em>Logical and Combinatorial Algorithms for Drug Design</em>. \n
//! For an example see:
//! Walters, W. P., Yalkowsky, S. H., \em JCICS, 1996, 36(5), 1015-1017.
//! <a href="http://dx.doi.org/10.1021/ci950278o">DOI: 10.1021/ci950278o</a>
void GraphPotentials(OBMol &mol, std::vector<double> &pot)
{
double det;
vector<vector<double> > g,c,h;
construct_g_matrix(mol,g);
invert_matrix(g,det);
construct_c_matrix(mol,c);
mult_matrix(h,g,c);
pot.resize(mol.NumAtoms()+1);
for (unsigned int i = 0; i < mol.NumAtoms();++i)
pot[i+1] = h[i][0];
}
void rotate_3d(t_3dview *view, int axis, gmx_bool bPositive)
{
int i, j;
mat4 m4;
if (bPositive)
{
mult_matrix(m4, view->Rot, view->RotP[axis]);
}
else
{
mult_matrix(m4, view->Rot, view->RotM[axis]);
}
for (i = 0; (i < N); i++)
{
for (j = 0; (j < N); j++)
{
view->Rot[i][j] = m4[i][j];
}
}
calculate_view(view);
}
int main(){
const uint64_t N = 1000;
clock_t start_t = 0;
clock_t end_t = 0;
double *A_matrix, *B_matrix, *A_matrix_dev, *B_matrix_dev, *result_cpu, *result_gpu;
TIMEIT_START;
// allocate memmory on HOST
A_matrix = (double*)malloc(N*N*sizeof(double));
B_matrix = (double*)malloc(N*N*sizeof(double));
result_cpu = (double*)malloc(N*N*sizeof(double));
result_gpu = (double*)malloc(N*N*sizeof(double));
TIMEIT_END(clock_t host_alloc_t);
print_timeit(host_alloc_t,"allocating memory on host:");
fill_matrix(A_matrix,N);
fill_matrix(B_matrix,N);
//print_matrix(A_matrix,N);
printf("\n");
TIMEIT_START;
mult_matrix(A_matrix,B_matrix,result_cpu,N);
TIMEIT_END(clock_t cpu_mult_t);
print_timeit(cpu_mult_t,"multiplying matrix by a matrix with into another one:");
//print_matrix(result_cpu,N);
TIMEIT_START;
cudaMalloc((void **) &A_matrix_dev,N*N*sizeof(double));
cudaMalloc((void **) &B_matrix_dev,N*N*sizeof(double));
cudaMalloc((void **) &result_gpu,N*N*sizeof(double));
TIMEIT_END(clock_t gpu_alloc_t);
print_timeit(gpu_alloc_t,"allocation memory on device:");
TIMEIT_START;
cudaMemcpy(A_matrix_dev, A_matrix, N*N*sizeof(double), cudaMemcpyHostToDevice);
cudaMemcpy(B_matrix_dev, B_matrix, N*N*sizeof(double), cudaMemcpyHostToDevice);
TIMEIT_END(clock_t gpu_copy_t);
print_timeit(gpu_copy_t,"allocation memory on device:");
free(A_matrix);
free(B_matrix);
free(result_cpu);
free(result_gpu);
return EXIT_SUCCESS;
}
void f_translate (void)
{
array_t *matrix;
double x, y, z;
Matrix current_matrix;
Matrix trans_matrix;
Matrix final_matrix;
int i;
if ((sp - 1)->type != T_REAL) {
bad_arg(3, F_TRANSLATE);
}
if (sp->type != T_REAL) {
bad_arg(4, F_TRANSLATE);
}
/*
* get arguments from stack.
*/
matrix = (sp - 3)->u.arr;
x = (sp - 2)->u.real;
y = (sp - 1)->u.real;
z = sp->u.real;
sp -= 3;
/*
* convert vec matrix to float matrix.
*/
for (i = 0; i < 16; i++) {
current_matrix[i] = matrix->item[i].u.real;
}
/*
* create translation matrix.
*/
translate_matrix(x, y, z, trans_matrix);
/*
* compute transformed matrix.
*/
mult_matrix(current_matrix, trans_matrix, final_matrix);
/*
* convert float matrix to vec matrix.
*/
for (i = 0; i < 16; i++) {
matrix->item[i].u.real = final_matrix[i];
}
}
void f_scale (void)
{
array_t *matrix;
LPC_FLOAT x, y, z;
Matrix current_matrix;
Matrix scaling_matrix;
Matrix final_matrix;
int i;
if ((sp - 1)->type != T_REAL) {
bad_arg(3, F_SCALE);
}
if (sp->type != T_REAL) {
bad_arg(4, F_SCALE);
}
/*
* get arguments from stack.
*/
matrix = (sp - 3)->u.arr;
x = (sp - 2)->u.real;
y = (sp - 1)->u.real;
z = sp->u.real;
sp -= 3;
/*
* convert vec matrix to float matrix.
*/
for (i = 0; i < 16; i++) {
current_matrix[i] = matrix->item[i].u.real;
}
/*
* create scaling matrix.
*/
scale_matrix(x, y, z, scaling_matrix);
/*
* compute transformed matrix.
*/
mult_matrix(current_matrix, scaling_matrix, final_matrix);
/*
* convert float matrix to vec matrix.
*/
for (i = 0; i < 16; i++) {
matrix->item[i].u.real = final_matrix[i];
}
}
void get_rotation(float angle, float ax, float ay, float az, gtMatrix mat)
{
gtMatrix r1,r2,r3;
float theta;
gtVector n;
gtVector a,b,c;
a[0] = ax;
a[1] = ay;
a[2] = az;
normalize_vector(a);
/* create vector not parallel to "a" */
ax = fabs(a[0]);
ay = fabs(a[1]);
az = fabs(a[2]);
n[0] = n[1] = n[2] = 0;
if (ax > ay) {
if (ay > az)
n[2] = 1; /* z is smallest */
else
n[1] = 1; /* y is smallest */
}
else {
if (ax > az)
n[2] = 1; /* z is smallest */
else
n[0] = 1; /* x is smallest */
}
/* create "b" orthogonal to "a" */
cross (b, a, n);
normalize_vector(b);
/* create "c" orthogonal to "a" and "b" */
cross (c, a, b);
/* make matrix that rotates a,b,c into x,y,z axes */
identity_matrix (r1);
r1[0][0] = a[0];
r1[1][0] = a[1];
r1[2][0] = a[2];
r1[0][1] = b[0];
r1[1][1] = b[1];
r1[2][1] = b[2];
r1[0][2] = c[0];
r1[1][2] = c[1];
r1[2][2] = c[2];
/* make matrix for rotation by theta degrees around x-axis */
theta = angle * 3.1415926535 / 180.0;
identity_matrix (r2);
r2[1][1] = cos(theta);
r2[2][2] = cos(theta);
r2[1][2] = sin(theta);
r2[2][1] = -sin(theta);
/* make matrix that is inverse of r1 */
copy_matrix (r3, r1);
transpose_matrix (r3);
/* compose these matrices for final matrix mat = r3 * r2 * r1 */
mult_matrix (mat, r3, r2);
mult_matrix (mat, mat, r1);
}
// Performs matrix multiplication and returns a new matrix.
Matrix *mult_matrix_func(Matrix *A, Matrix *B) {
Matrix *C = make_matrix(A->rows, B->cols);
mult_matrix(A, B, C);
return C;
}
void rotate_matrix(double (*im)[4], double (*om)[4], struct vector3 *axis, double angle)
{
double r1[4][4],r2[4][4],r3[4][4],rm[4][4];
double a,b,c,v;
double rad;
unsigned short l,m,n;
normalize(axis);
a=axis->x;
b=axis->y;
c=axis->z;
v=sqrt(b*b+c*c);
r1[0][0]=1;
r1[0][1]=0;
r1[0][2]=0;
r1[0][3]=0;
r1[1][0]=0;
r1[1][1]=1;
r1[1][2]=0;
r1[1][3]=0;
r1[2][0]=0;
r1[2][1]=0;
r1[2][2]=1;
r1[2][3]=0;
r1[3][0]=0;
r1[3][1]=0;
r1[3][2]=0;
r1[3][3]=1;
if (v!=0.0) {
r1[1][1]=c/v;
r1[1][2]=b/v;
r1[2][1]=-b/v;
r1[2][2]=c/v;
}
r2[0][0]=v;
r2[0][1]=0;
r2[0][2]=a;
r2[0][3]=0;
r2[1][0]=0;
r2[1][1]=1;
r2[1][2]=0;
r2[1][3]=0;
r2[2][0]=-a;
r2[2][1]=0;
r2[2][2]=v;
r2[2][3]=0;
r2[3][0]=0;
r2[3][1]=0;
r2[3][2]=0;
r2[3][3]=1;
rad=MY_PI/180.0;
r3[0][0]=cos(angle*rad);
r3[0][1]=sin(angle*rad);
r3[0][2]=0;
r3[0][3]=0;
r3[1][0]=-sin(angle*rad);
r3[1][1]=cos(angle*rad);
r3[1][2]=0;
r3[1][3]=0;
r3[2][0]=0;
r3[2][1]=0;
r3[2][2]=1;
r3[2][3]=0;
r3[3][0]=0;
r3[3][1]=0;
r3[3][2]=0;
r3[3][3]=1;
l=4;
m=4;
n=4;
mult_matrix(r1,r2,rm,l,m,n);
mult_matrix(rm,r3,rm,l,m,n);
r2[0][2]=-a;
r2[2][0]=a;
if (v!=0.0) {
r1[1][2]=-b/v;
r1[2][1]=b/v;
}
mult_matrix(rm,r2,rm,l,m,n);
mult_matrix(rm,r1,rm,l,m,n);
mult_matrix(im,rm,om,l,m,n);
}
static void csc(enum v4l2_colorspace colorspace, double *r, double *g, double *b)
{
/* Convert the primaries of Rec. 709 Linear RGB */
switch (colorspace) {
case V4L2_COLORSPACE_SMPTE240M:
*r = transfer_srgb_to_rgb(*r);
*g = transfer_srgb_to_rgb(*g);
*b = transfer_srgb_to_rgb(*b);
mult_matrix(r, g, b, rec709_to_240m);
break;
case V4L2_COLORSPACE_SMPTE170M:
*r = transfer_srgb_to_rgb(*r);
*g = transfer_srgb_to_rgb(*g);
*b = transfer_srgb_to_rgb(*b);
mult_matrix(r, g, b, rec709_to_170m);
break;
case V4L2_COLORSPACE_470_SYSTEM_BG:
*r = transfer_srgb_to_rgb(*r);
*g = transfer_srgb_to_rgb(*g);
*b = transfer_srgb_to_rgb(*b);
mult_matrix(r, g, b, rec709_to_ebu);
break;
case V4L2_COLORSPACE_470_SYSTEM_M:
*r = transfer_srgb_to_rgb(*r);
*g = transfer_srgb_to_rgb(*g);
*b = transfer_srgb_to_rgb(*b);
mult_matrix(r, g, b, rec709_to_ntsc1953);
break;
case V4L2_COLORSPACE_SRGB:
case V4L2_COLORSPACE_REC709:
default:
break;
}
*r = ((*r) < 0) ? 0 : (((*r) > 1) ? 1 : (*r));
*g = ((*g) < 0) ? 0 : (((*g) > 1) ? 1 : (*g));
*b = ((*b) < 0) ? 0 : (((*b) > 1) ? 1 : (*b));
/* Encode to gamma corrected colorspace */
switch (colorspace) {
case V4L2_COLORSPACE_SMPTE240M:
*r = transfer_rgb_to_smpte240m(*r);
*g = transfer_rgb_to_smpte240m(*g);
*b = transfer_rgb_to_smpte240m(*b);
break;
case V4L2_COLORSPACE_SMPTE170M:
case V4L2_COLORSPACE_470_SYSTEM_M:
case V4L2_COLORSPACE_470_SYSTEM_BG:
*r = transfer_rgb_to_rec709(*r);
*g = transfer_rgb_to_rec709(*g);
*b = transfer_rgb_to_rec709(*b);
break;
case V4L2_COLORSPACE_SRGB:
break;
case V4L2_COLORSPACE_REC709:
default:
*r = transfer_srgb_to_rec709(*r);
*g = transfer_srgb_to_rec709(*g);
*b = transfer_srgb_to_rec709(*b);
break;
}
}
static void rot_conf(t_atoms *atoms, rvec x[], rvec v[], real trans, real angle,
rvec head, rvec tail, matrix box, int isize, atom_id index[],
rvec xout[], rvec vout[])
{
rvec arrow, center, xcm;
real theta, phi, arrow_len;
mat4 Rx, Ry, Rz, Rinvy, Rinvz, Mtot, Tcm, Tinvcm, Tx;
mat4 temp1, temp2, temp3, temp4, temp21, temp43;
vec4 xv;
int i, j, ai;
rvec_sub(tail, head, arrow);
arrow_len = norm(arrow);
if (debug)
{
fprintf(debug, "Arrow vector: %10.4f %10.4f %10.4f\n",
arrow[XX], arrow[YY], arrow[ZZ]);
fprintf(debug, "Effective translation %g nm\n", trans);
}
if (arrow_len == 0.0)
{
gmx_fatal(FARGS, "Arrow vector not given");
}
/* Copy all aoms to output */
for (i = 0; (i < atoms->nr); i++)
{
copy_rvec(x[i], xout[i]);
copy_rvec(v[i], vout[i]);
}
/* Compute center of mass and move atoms there */
clear_rvec(xcm);
for (i = 0; (i < isize); i++)
{
rvec_inc(xcm, x[index[i]]);
}
for (i = 0; (i < DIM); i++)
{
xcm[i] /= isize;
}
if (debug)
{
fprintf(debug, "Center of mass: %10.4f %10.4f %10.4f\n",
xcm[XX], xcm[YY], xcm[ZZ]);
}
for (i = 0; (i < isize); i++)
{
rvec_sub(x[index[i]], xcm, xout[index[i]]);
}
/* Compute theta and phi that describe the arrow */
theta = acos(arrow[ZZ]/arrow_len);
phi = atan2(arrow[YY]/arrow_len, arrow[XX]/arrow_len);
if (debug)
{
fprintf(debug, "Phi = %.1f, Theta = %.1f\n", RAD2DEG*phi, RAD2DEG*theta);
}
/* Now the total rotation matrix: */
/* Rotate a couple of times */
rotate(ZZ, -phi, Rz);
rotate(YY, M_PI/2-theta, Ry);
rotate(XX, angle*DEG2RAD, Rx);
Rx[WW][XX] = trans;
rotate(YY, theta-M_PI/2, Rinvy);
rotate(ZZ, phi, Rinvz);
mult_matrix(temp1, Ry, Rz);
mult_matrix(temp2, Rinvy, Rx);
mult_matrix(temp3, temp2, temp1);
mult_matrix(Mtot, Rinvz, temp3);
print_m4(debug, "Rz", Rz);
print_m4(debug, "Ry", Ry);
print_m4(debug, "Rx", Rx);
print_m4(debug, "Rinvy", Rinvy);
print_m4(debug, "Rinvz", Rinvz);
print_m4(debug, "Mtot", Mtot);
for (i = 0; (i < isize); i++)
{
ai = index[i];
m4_op(Mtot, xout[ai], xv);
rvec_add(xv, xcm, xout[ai]);
m4_op(Mtot, v[ai], xv);
copy_rvec(xv, vout[ai]);
}
}
void tform_matrix(struct vector3 *scale, struct vector3 *translate, struct vector3 *axis, double angle, double (*om)[4])
{
double sc[4][4];
double tm[4][4];
double r1[4][4],r2[4][4],r3[4][4];
double a,b,c,v;
double rad;
unsigned short l,m,n;
init_matrix(om);
sc[0][0]=scale->x;
sc[0][1]=0;
sc[0][2]=0;
sc[0][3]=0;
sc[1][0]=0;
sc[1][1]=scale->y;
sc[1][2]=0;
sc[1][3]=0;
sc[2][0]=0;
sc[2][1]=0;
sc[2][2]=scale->z;
sc[2][3]=0;
sc[3][0]=0;
sc[3][1]=0;
sc[3][2]=0;
sc[3][3]=1;
tm[0][0]=1;
tm[0][1]=0;
tm[0][2]=0;
tm[0][3]=0;
tm[1][0]=0;
tm[1][1]=1;
tm[1][2]=0;
tm[1][3]=0;
tm[2][0]=0;
tm[2][1]=0;
tm[2][2]=1;
tm[2][3]=0;
tm[3][0]=translate->x;
tm[3][1]=translate->y;
tm[3][2]=translate->z;
tm[3][3]=1;
normalize(axis);
a=axis->x;
b=axis->y;
c=axis->z;
v=sqrt(b*b+c*c);
r1[0][0]=1;
r1[0][1]=0;
r1[0][2]=0;
r1[0][3]=0;
r1[1][0]=0;
r1[1][1]=1;
r1[1][2]=0;
r1[1][3]=0;
r1[2][0]=0;
r1[2][1]=0;
r1[2][2]=1;
r1[2][3]=0;
r1[3][0]=0;
r1[3][1]=0;
r1[3][2]=0;
r1[3][3]=1;
if (v!=0.0) {
r1[1][1]=c/v;
r1[1][2]=b/v;
r1[2][1]=-b/v;
r1[2][2]=c/v;
}
r2[0][0]=v;
r2[0][1]=0;
r2[0][2]=a;
r2[0][3]=0;
r2[1][0]=0;
r2[1][1]=1;
r2[1][2]=0;
r2[1][3]=0;
r2[2][0]=-a;
r2[2][1]=0;
r2[2][2]=v;
r2[2][3]=0;
r2[3][0]=0;
r2[3][1]=0;
r2[3][2]=0;
r2[3][3]=1;
rad=MY_PI/180.0;
r3[0][0]=cos(angle*rad);
r3[0][1]=sin(angle*rad);
r3[0][2]=0;
r3[0][3]=0;
r3[1][0]=-sin(angle*rad);
r3[1][1]=cos(angle*rad);
r3[1][2]=0;
r3[1][3]=0;
r3[2][0]=0;
r3[2][1]=0;
r3[2][2]=1;
r3[2][3]=0;
r3[3][0]=0;
r3[3][1]=0;
r3[3][2]=0;
r3[3][3]=1;
l=4;
m=4;
n=4;
mult_matrix(r1,r2,om,l,m,n);
mult_matrix(om,r3,om,l,m,n);
r2[0][2]=-a;
r2[2][0]=a;
if (v!=0.0) {
r1[1][2]=-b/v;
r1[2][1]=b/v;
}
mult_matrix(om,r2,om,l,m,n);
mult_matrix(om,r1,om,l,m,n);
mult_matrix(om,sc,om,l,m,n);
mult_matrix(om,tm,om,l,m,n);
}
// Performs matrix multiplication and returns a new matrix.
Matrix *mult_matrix_func(Matrix *A, Matrix *B) {
Matrix *Ret = make_matrix(A->rows, B->cols);
mult_matrix(A,B,Ret);
return Ret;
}
