1 /* matrix.c -- Fixed-point 2D linear algebra operations with matrices
2
3 Copyright 2001 Carl Worth
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2, or (at your option)
8 any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14 */
15
16 #include <stdio.h>
17 #include <stdlib.h>
18 #include <math.h>
19
20 #include "matrix.h"
21 #include "fixed.h"
22
23 #ifdef DMALLOC
24 #include "dmalloc.h"
25 #endif
26
matrix_init(matrix_t * matrix,int precision)27 int matrix_init(matrix_t *matrix, int precision)
28 {
29 matrix->precision_bits = precision;
30 matrix_set_identity(matrix);
31
32 return 0;
33 }
34
matrix_set_explicit(matrix_t * matrix,double m00,double m01,double m02,double m10,double m11,double m12,double m20,double m21,double m22)35 void matrix_set_explicit(matrix_t *matrix,
36 double m00, double m01, double m02,
37 double m10, double m11, double m12,
38 double m20, double m21, double m22)
39 {
40 matrix->m[0][0] = f_to_fixed(m00, matrix->precision_bits);
41 matrix->m[0][1] = f_to_fixed(m01, matrix->precision_bits);
42 matrix->m[0][2] = f_to_fixed(m02, matrix->precision_bits);
43
44 matrix->m[1][0] = f_to_fixed(m10, matrix->precision_bits);
45 matrix->m[1][1] = f_to_fixed(m11, matrix->precision_bits);
46 matrix->m[1][2] = f_to_fixed(m12, matrix->precision_bits);
47
48 matrix->m[2][0] = f_to_fixed(m20, matrix->precision_bits);
49 matrix->m[2][1] = f_to_fixed(m21, matrix->precision_bits);
50 matrix->m[2][2] = f_to_fixed(m22, matrix->precision_bits);
51 }
52
matrix_set_identity(matrix_t * matrix)53 void matrix_set_identity(matrix_t *matrix)
54 {
55 matrix_set_explicit(matrix,
56 1.0, 0.0, 0.0,
57 0.0, 1.0, 0.0,
58 0.0, 0.0, 1.0);
59 }
60
matrix_set_translate(matrix_t * matrix,double x_off,double y_off)61 void matrix_set_translate(matrix_t *matrix, double x_off, double y_off)
62 {
63 matrix_set_explicit(matrix,
64 1.0, 0.0, x_off,
65 0.0, 1.0, y_off,
66 0.0, 0.0, 1.0);
67 }
68
matrix_set_scale(matrix_t * matrix,double x_scale,double y_scale)69 void matrix_set_scale(matrix_t *matrix, double x_scale, double y_scale)
70 {
71 matrix_set_explicit(matrix,
72 x_scale, 0.0, 0.0,
73 0.0, y_scale, 0.0,
74 0.0, 0.0, 1.0);
75 }
76
matrix_set_rotate(matrix_t * matrix,double theta)77 void matrix_set_rotate(matrix_t *matrix, double theta)
78 {
79 matrix_set_explicit(matrix,
80 cos(theta), -sin(theta), 0.0,
81 sin(theta), cos(theta), 0.0,
82 0.0, 0.0, 1.0);
83 }
84
matrix_set_rotate_about(matrix_t * matrix,double theta,double x,double y)85 void matrix_set_rotate_about(matrix_t *matrix, double theta, double x, double y)
86 {
87 matrix_t tmp, to_origin, rot, from_origin;
88
89 /* XXX: This only works if (int) x and (int) y fit in 16 bits. It
90 would be better to dynamically compute the maximum allowable
91 precision for the rotation and intermediate matrices. */
92 matrix_init(&to_origin, 0);
93 matrix_init(&rot, 16);
94 matrix_init(&tmp, 16);
95 matrix_init(&from_origin, 0);
96
97 matrix_set_translate(&to_origin, -x, -y);
98 matrix_set_rotate(&rot, theta);
99 matrix_multiply(&tmp, &rot, &to_origin);
100
101 matrix_set_translate(&from_origin, x, y);
102 matrix_multiply(matrix, &from_origin, &tmp);
103 }
104
matrix_multiply(matrix_t * result,matrix_t * a,matrix_t * b)105 void matrix_multiply(matrix_t *result, matrix_t *a, matrix_t *b)
106 {
107 int i, j, k;
108 int intermediate_precision;
109 long int val;
110
111 intermediate_precision = a->precision_bits + b->precision_bits;
112 for (j=0; j<3; j++) {
113 for (i=0; i<3; i++) {
114 val = 0;
115 for (k=0; k<3; k++) {
116 val += fixed_adjust_precision(a->m[j][k] * b->m[k][i],
117 intermediate_precision,
118 result->precision_bits);
119 }
120 result->m[j][i] = val;
121 }
122 }
123 }
124
matrix_copy(matrix_t * dest,matrix_t * src)125 void matrix_copy(matrix_t *dest, matrix_t *src)
126 {
127 int i, j;
128
129 for (j=0; j<3; j++) {
130 for (i=0; i<3; i++) {
131 dest->m[j][i] = src->m[j][i];
132 }
133 }
134 }
135