33 #ifndef __small_linalg_h__
34 #define __small_linalg_h__
48 return m[0]*(m[4]*m[8] - m[5]*m[7])
49 - m[1]*(m[3]*m[8] - m[5]*m[6])
50 + m[2]*(m[3]*m[7] - m[4]*m[6]);
60 if (det == 0)
return -1;
61 double det_inv = 1.0 / det;
62 inverse[0] = det_inv * (m[4]*m[8] - m[5]*m[7]);
63 inverse[1] = det_inv * (m[2]*m[7] - m[1]*m[8]);
64 inverse[2] = det_inv * (m[1]*m[5] - m[2]*m[4]);
65 inverse[3] = det_inv * (m[5]*m[6] - m[3]*m[8]);
66 inverse[4] = det_inv * (m[0]*m[8] - m[2]*m[6]);
67 inverse[5] = det_inv * (m[2]*m[3] - m[0]*m[5]);
68 inverse[6] = det_inv * (m[3]*m[7] - m[4]*m[6]);
69 inverse[7] = det_inv * (m[1]*m[6] - m[0]*m[7]);
70 inverse[8] = det_inv * (m[0]*m[4] - m[1]*m[3]);
78 double det = m[0]*m[3] - m[1]*m[2];
79 if (det == 0)
return -1;
81 inverse[0] = m[3] / det;
82 inverse[1] = -m[1] / det;
83 inverse[2] = -m[2] / det;
84 inverse[3] = m[0] / det;
92 result[0] = m[0]*v[0] + m[1]*v[1];
93 result[1] = m[2]*v[0] + m[3]*v[1];
100 result[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
101 result[1] = m[3]*v[0] + m[4]*v[1] + m[5]*v[2];
102 result[2] = m[6]*v[0] + m[7]*v[1] + m[8]*v[2];
109 result[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2] + m[3]*v[3];
110 result[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2] + m[7]*v[3];
111 result[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2] + m[11]*v[3];
112 result[3] = m[12]*v[0] + m[13]*v[1] + m[14]*v[2] + m[15]*v[3];
118 result[0] = v1[0] + v2[0];
119 result[1] = v1[1] + v2[1];
120 result[2] = v1[2] + v2[2];
124 vector_add_nd (
const double * v1,
const double * v2,
int N,
double * result)
127 for (i = 0; i < N; i++)
128 result[i] = v1[i] + v2[i];
132 vector_sub_nd (
const double * v1,
const double * v2,
int N,
double * result)
135 for (i = 0; i < N; i++)
136 result[i] = v1[i] - v2[i];
147 result[0] = v1[0] - v2[0];
148 result[1] = v1[1] - v2[1];
149 result[2] = v1[2] - v2[2];
155 return v1[0]*v2[0] + v1[1]*v2[1];
161 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
167 result[0] = v1[1]*v2[2] - v1[2]*v2[1];
168 result[1] = v1[2]*v2[0] - v1[0]*v2[2];
169 result[2] = v1[0]*v2[1] - v1[1]*v2[0];
175 return v[0]*v[0] + v[1]*v[1];
181 return sqrt(v[0]*v[0] + v[1]*v[1]);
187 return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
193 return v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
256 double t[3] = { a * x[0] + y[0], a * x[1] + y[1], a * x[2] + y[2] };
257 z[0] = t[0]; z[1] = t[1]; z[2] = t[2];
267 double costheta = dot / ( mag1 * mag2);
268 if (costheta > 1)
return 0;
269 double theta = acos (costheta);
271 if ( (v1[0]*v2[1] - v1[1]*v2[0]) < 0) {
284 double costheta = dot / (mag1*mag2);
285 if (costheta > 1)
return 0;
286 return acos (costheta);
328 for (i = 0; i < 4; i++) {
329 for (j = 0; j < 4; j++) {
332 for (k = 0; k < 4; k++)
333 acc += a[4*i + k] * b[j + 4*k];
343 for (i = 0; i < 3; i++) {
344 for (j = 0; j < 3; j++) {
347 for (k = 0; k < 3; k++)
348 acc += a[3*i + k] * b[j + 3*k];
356 const double *b,
int b_nrows,
int b_ncols,
360 for (i=0; i<a_nrows; i++) {
361 for (j=0; j<b_ncols; j++) {
364 for (r=0; r<a_ncols; r++) {
365 acc += a[i*a_ncols + r] * b[r*b_ncols + j];
367 result[i*b_ncols + j] = acc;
377 double rot_inv_1[3] = { m[0], m[4], m[8] };
378 double rot_inv_2[3] = { m[1], m[5], m[9] };
379 double rot_inv_3[3] = { m[2], m[6], m[10] };
380 double t_inv[3] = { -m[3], -m[7], -m[11] };
394 inv[12] = inv[13] = inv[14] = 0;
403 rot[0] = m[0]; rot[1] = m[1]; rot[2] = m[2]; rot[3] = 0;
404 rot[4] = m[4]; rot[5] = m[5]; rot[6] = m[6]; rot[7] = 0;
405 rot[8] = m[8]; rot[9] = m[9]; rot[10] = m[10]; rot[11] = 0;
406 rot[12] = rot[13] = rot[14] = 0; rot[15] = 1;
413 t[0] = 1; t[1] = 0; t[2] = 0; t[3] = m[3];
414 t[4] = 0; t[5] = 1; t[6] = 0; t[7] = m[7];
415 t[8] = 0; t[9] = 0; t[10] = 1; t[11] = m[11];
416 t[12] = t[13] = t[14] = 0; t[15] = 1;
423 for (r = 0; r < rows; r++) {
424 for (c = 0; c < cols; c++) {
425 printf(
"%10f ", a[r*cols + c]);
435 for (i = 0; i < 3; i++)
436 printf(
"%15f\n", a[i]);
444 result[0] = v1[0] * v2[0];
445 result[1] = v1[1] * v2[0];
446 result[2] = v1[2] * v2[0];
447 result[3] = result[1];
448 result[4] = v1[1] * v2[1];
449 result[5] = v1[2] * v2[1];
450 result[6] = result[2];
451 result[7] = result[5];
452 result[8] = v1[2] * v2[2];
static int matrix_rigid_body_transform_inverse_4x4d(const double m[16], double inv[16])
static void matrix_rigid_body_transform_get_rotation_matrix_4x4d(const double m[16], double rot[16])
static void matrix_rigid_body_transform_get_translation_matrix_4x4d(const double m[16], double t[16])
static void vector_saxpy_3d(double a, const double x[3], const double y[3], double z[3])
static double vector_angle_3d(const double v1[3], const double v2[3])
static int matrix_inverse_3x3d(const double m[9], double inverse[9])
static double vector_magnitude_squared_2d(const double v[2])
static double vector_magnitude_3d(const double v[3])
static void vector_normalize_2d(double v[2])
static double vector_dist_squared_3d(const double v1[3], const double v2[3])
static int matrix_inverse_2x2d(const double m[4], double inverse[4])
static void vector_sub_nd(const double *v1, const double *v2, int N, double *result)
static void matrix_transpose_4x4d(const double m[16], double result[16])
static void matrix_vector_multiply_2x2_2d(const double m[4], const double v[2], double result[2])
static void matrix_multiply(const double *a, int a_nrows, int a_ncols, const double *b, int b_nrows, int b_ncols, double *result)
static void matrix_print(const double *a, int rows, int cols)
static void matrix_vector_multiply_4x4_4d(const double m[16], const double v[4], double result[4])
static void vector_vector_outer_product_3d(const double v1[3], const double v2[3], double result[9])
static void vector_scale_2d(double v[2], double s)
static void vector_add_3d(const double v1[3], const double v2[3], double result[3])
static void matrix_transpose_3x3d(const double m[9], double result[9])
static double vector_dist_2d(const double v1[2], const double v2[2])
int matrix_inverse_4x4d(const double m[16], double inv[16])
static void matrix_multiply_3x3_3x3(const double a[9], const double b[9], double r[9])
static double vector_angle_2d(const double v1[2], const double v2[2])
static void vector_print_3d(const double a[3])
static void vector_add_nd(const double *v1, const double *v2, int N, double *result)
static void vector_normalize_3d(double v[3])
static void vector_cross_3d(const double v1[3], const double v2[3], double result[3])
static void vector_subtract_3d(const double v1[3], const double v2[3], double result[3])
static double matrix_determinant_3x3d(const double m[9])
static double vector_dot_3d(const double v1[3], const double v2[3])
static double vector_magnitude_squared_3d(const double v[3])
static double vector_dist_3d(const double v1[3], const double v2[3])
static void matrix_multiply_4x4_4x4(const double a[16], const double b[16], double r[16])
static double vector_magnitude_2d(const double v[2])
static double vector_dot_2d(const double v1[2], const double v2[2])
static void vector_scale_3d(double v[2], double s)
static void matrix_vector_multiply_3x3_3d(const double m[9], const double v[3], double result[3])