1 #include <stdio.h>
2 #include <string.h>
3
4 #include <math.h>
5
6 #include "matrixmath.h"
7
8 #ifdef MEMWATCH
9 # include "memwatch.h"
10 #endif
11
12
13 void MatrixMulti3Rotate33(double *a, double *b, double *r);
14
15 void MatrixRotateHeading3(double *a, double theta, double *r);
16 void MatrixRotatePitch3(double *a, double theta, double *r);
17 void MatrixRotateBank3(double *a, double theta, double *r);
18
19 void MatrixGetHeading33(double theta, double *r);
20 void MatrixGetPitch33(double theta, double *r);
21 void MatrixGetBank33(double theta, double *r);
22
23
24 /*
25 * Multiplies matrix a (3 by 1) with rotational matrix b (3 by 3)
26 * and stores results into matrix r (3 by 1).
27 */
MatrixMulti3Rotate33(double * a,double * b,double * r)28 void MatrixMulti3Rotate33(double *a, double *b, double *r)
29 {
30 int i, j;
31 int m = 3, n = 3;
32
33 for(j = 0; j < n; j++)
34 {
35 r[j] = 0.0;
36 for(i = 0; i < m; i++)
37 r[j] += a[i] * b[(i * m) + j];
38 }
39 }
40
41 /*
42 * Rotates position matrix a (3 * 1) about z axis clockwise.
43 *
44 * Theta is in radians.
45 */
MatrixRotateHeading3(double * a,double theta,double * r)46 void MatrixRotateHeading3(double *a, double theta, double *r)
47 {
48 double b[3 * 3];
49
50 b[0] = cos(theta);
51 b[1] = -sin(theta);
52 b[2] = 0.0;
53
54 b[3] = -b[1]; /* -sin(theta) */
55 b[4] = b[0]; /* cos(theta) */
56 b[5] = 0.0;
57
58 b[6] = 0.0;
59 b[7] = 0.0;
60 b[8] = 1.0;
61
62 MatrixMulti3Rotate33(a, b, r);
63 }
64
65 /*
66 * Rotates position matrix a (3 * 1) about x axis clockwise.
67 *
68 * Theta is in radians.
69 */
MatrixRotatePitch3(double * a,double theta,double * r)70 void MatrixRotatePitch3(double *a, double theta, double *r)
71 {
72 double b[3 * 3];
73
74 b[0] = 1.0;
75 b[1] = 0.0;
76 b[2] = 0.0;
77
78 b[3] = 0.0;
79 b[4] = cos(theta);
80 b[5] = -sin(theta);
81
82 b[6] = 0.0;
83 b[7] = -b[5]; /* sin(theta) */
84 b[8] = b[4]; /* cos(theta) */
85
86 MatrixMulti3Rotate33(a, b, r);
87 }
88
89 /*
90 * Rotates position matrix a (3 * 1) about y axis clockwise.
91 *
92 * Theta is in radians.
93 */
MatrixRotateBank3(double * a,double theta,double * r)94 void MatrixRotateBank3(double *a, double theta, double *r)
95 {
96 double b[3 * 3];
97
98 b[0] = cos(theta);
99 b[1] = 0.0;
100 b[2] = sin(theta);
101
102 b[3] = 0.0;
103 b[4] = 1.0;
104 b[5] = 0.0;
105
106 b[6] = -b[2]; /* -sin(theta) */
107 b[7] = 0.0;
108 b[8] = b[0]; /* cos(theta) */
109
110 MatrixMulti3Rotate33(a, b, r);
111 }
112
113
114 /*
115 * Generates a rotation matrix r (3 * 3) about the z axis clockwise.
116 *
117 * Theta is in radians.
118 */
MatrixGetHeading33(double theta,double * r)119 void MatrixGetHeading33(double theta, double *r)
120 {
121 r[0] = cos(theta);
122 r[1] = -sin(theta);
123 r[2] = 0.0;
124
125 r[3] = -r[1]; /* -sin(theta) */
126 r[4] = r[0]; /* cos(theta) */
127 r[5] = 0.0;
128
129 r[6] = 0.0;
130 r[7] = 0.0;
131 r[8] = 1.0;
132 }
133
134 /*
135 * Generates a rotation matrix r (3 * 3) about the x axis clockwise.
136 *
137 * Theta is in radians.
138 */
MatrixGetPitch33(double theta,double * r)139 void MatrixGetPitch33(double theta, double *r)
140 {
141 r[0] = 1.0;
142 r[1] = 0.0;
143 r[2] = 0.0;
144
145 r[3] = 0.0;
146 r[4] = cos(theta);
147 r[5] = -sin(theta);
148
149 r[6] = 0.0;
150 r[7] = -r[5]; /* sin(theta) */
151 r[8] = r[4]; /* cos(theta) */
152 }
153
154 /*
155 * Generates a rotation matrix r (3 * 3) about the y axis clockwise.
156 *
157 * Theta is in radians.
158 */
MatrixGetBank33(double theta,double * r)159 void MatrixGetBank33(double theta, double *r)
160 {
161 r[0] = cos(theta);
162 r[1] = 0.0;
163 r[2] = sin(theta);
164
165 r[3] = 0.0;
166 r[4] = 1.0;
167 r[5] = 0.0;
168
169 r[6] = -r[2]; /* -sin(theta) */
170 r[7] = 0.0;
171 r[8] = r[0]; /* cos(theta) */
172 }
173