hexapod_ng/legacy/linux/liblicks/linalg/linalg.c

#include <math.h>

#include "linalg.h"

void
copy_vector3d(vector3d *v1, vector3d *v2) {
   v2->x=v1->x;
   v2->y=v1->y;
   v2->z=v1->z;
}

void
add_vector3d(vector3d *v1, vector3d *v2, vector3d *result) {
   result->x = v1->x + v2->x;
   result->y = v1->y + v2->y;
   result->z = v1->z + v2->z;
}

void
sub_vector3d(vector3d *v1, vector3d *v2, vector3d *result) {
   result->x = v1->x - v2->x;
   result->y = v1->y - v2->y;
   result->z = v1->z - v2->z;
}

void
scalar_mult_vector3d(vector3d *v1, double s, vector3d *result) {
   result->x = v1->x * s;
   result->y = v1->y * s;
   result->z = v1->z * s;
}

void
cross_mult_vector3d(vector3d *v1, vector3d *v2, vector3d *result) {
   vector3d t;
   t.x = v1->y * v2->z - v1->z * v2->y;
   t.y = v1->z * v2->x - v1->x * v2->z;
   t.z = v1->x * v2->y - v1->y * v2->z;
   copy_vector3d(&t,result);
}

void
matrix_mult_vector3d(vector3d *v1, matrix3x3d *M, vector3d *result) {
   vector3d t;
   t.x = v1->x * M->x[0] + v1->y * M->y[0] + v1->z * M->z[0];
   t.y = v1->x * M->x[1] + v1->y * M->y[1] + v1->z * M->z[1];
   t.z = v1->x * M->x[2] + v1->y * M->y[2] + v1->z * M->z[2];
   copy_vector3d(&t,result);
}

void
rot_x_vector3d(vector3d *v, double a, vector3d *result) {
   vector3d t;
   t.x = v->x;
   t.y = v->y * cos(a) + v->z * sin(a);
   t.z =-v->y * sin(a) + v->z * cos(a);
   copy_vector3d(&t,result);
}

void
rot_y_vector3d(vector3d *v, double a, vector3d *result) {
   vector3d t;
   t.x =  v->x * cos(a) - v->z * sin(a);
   t.y =  v->y;
   t.z =  v->x * sin(a) + v->z * cos(a);  
   copy_vector3d(&t, result);
}

void
rot_z_vector3d(vector3d *v, double a, vector3d *result) {
   vector3d t;
   t.x = v->x * cos(a) + v->y * sin(a);
   t.y =-v->x * sin(a) + v->y * cos(a);
   t.z = v->z;
   copy_vector3d(&t, result);
}