102 Dalmatians: Puppies to the Rescue
Decompilation Source Reference | GitLab repository
Loading...
Searching...
No Matches
matrices.c
Go to the documentation of this file.
1/* Recovered game implementation; original addresses identify provenance. */
6#include "pcdogs/math/types.h"
7#include <string.h>
8
9// Original pcdogs.exe: 0x42df40
10// Multiplies two Q12 matrices with signed 16-bit outputs and a temporary copy when the destination aliases a factor. If all three pointers coincide, only the factor selected by the original branch is copied.
11void Math_MulMat3x3Q12(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *right_factor, struct Math_Matrix3x3I16 const *left_factor)
12{
13 struct Math_Matrix3x3I16 saved;
14
15 if (out_matrix == left_factor)
16 {
17 saved = *left_factor;
18 left_factor = &saved;
19 }
20 else if (out_matrix == right_factor)
21 {
22 saved = *right_factor;
23 right_factor = &saved;
24 }
25
26 /* Column-first output order matters when both factors alias the result. */
27 for (int col = 0; col < 3; ++col)
28 {
29 for (int row = 0; row < 3; ++row)
30 {
31 const int16_t *left_row = &left_factor->elements[row * 3];
32 const int16_t *right_col = &right_factor->elements[col];
33 int32_t dot = left_row[0] * right_col[0] + left_row[1] * right_col[3] + left_row[2] * right_col[6];
34 out_matrix->elements[row * 3 + col] = (int16_t)(dot >> MATH_Q12_SHIFT);
35 }
36 }
37}
38
39/* Axis builders copy four dwords and one word, in order, from live scratch. */
41 const union Math_RotationScratch *scratch)
42{
43 for (unsigned pair = 0; pair < 4; ++pair)
44 {
45 uint32_t values;
46 memcpy(&values, &scratch->matrix.elements[2 * pair], sizeof(values));
47 memcpy(&out->elements[2 * pair], &values, sizeof(values));
48 }
49 out->m22 = scratch->matrix.m22;
50}
51
52// Original pcdogs.exe: 0x42e140
53// Builds an X-axis Q12 rotation in shared scratch storage and optionally composes it with the supplied matrix before writing the result.
54void Math_BuildMatrixRotationX(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle)
55{
56 int16_t cosine_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle) >> 2);
57 int32_t sine_q12 = Math_SinQ14(angle) >> 2;
58
59 g_rotationXScratch.matrix.m21 = (int16_t)sine_q12;
60 g_rotationXScratch.matrix.m12 = (int16_t)-(sine_q12);
61 g_rotationXScratch.matrix.m11 = cosine_q12;
62 g_rotationXScratch.matrix.m22 = cosine_q12;
63
64 if (left_factor)
65 {
66 Math_MulMat3x3Q12(out_matrix, &g_rotationXScratch.matrix, left_factor);
67
68 return;
69 }
70
72}
73
74// Original pcdogs.exe: 0x42e1d0
75// Builds a Y-axis Q12 rotation in shared scratch storage and optionally composes it with the supplied matrix before writing the result.
76void Math_BuildMatrixRotationY(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle)
77{
78 int16_t cosine_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle) >> 2);
79 int32_t sine_q12 = Math_SinQ14(angle) >> 2;
80
81 g_rotationYScratch.matrix.m02 = (uint16_t)sine_q12;
82 g_rotationYScratch.matrix.m00 = cosine_q12;
83 g_rotationYScratch.matrix.m22 = cosine_q12;
84 g_rotationYScratch.matrix.m20 = (int16_t)-(sine_q12);
85
86 if (left_factor)
87 {
88 Math_MulMat3x3Q12(out_matrix, &g_rotationYScratch.matrix, left_factor);
89
90 return;
91 }
92
94}
95
96// Original pcdogs.exe: 0x42e270
97// Builds a Z-axis Q12 rotation in shared scratch storage and optionally composes it with the supplied matrix before writing the result.
98void Math_BuildMatrixRotationZ(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle)
99{
100 int16_t cosine_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle) >> 2);
101 int32_t sine_q12 = Math_SinQ14(angle) >> 2;
102
103 g_rotationZScratch.matrix.m10 = (int16_t)sine_q12;
104 g_rotationZScratch.matrix.m01 = (int16_t)-(sine_q12);
105 g_rotationZScratch.matrix.m00 = cosine_q12;
106 g_rotationZScratch.matrix.m11 = cosine_q12;
107
108 if (left_factor)
109 {
110 Math_MulMat3x3Q12(out_matrix, &g_rotationZScratch.matrix, left_factor);
111
112 return;
113 }
114
116}
117
118// Original pcdogs.exe: 0x42e300
119// Builds a combined X/Y Q12 rotation, optionally composing it with the supplied matrix through shared axis scratch storage.
120void Math_BuildMatrixRotationXY(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle_x, int32_t angle_y)
121{
122 int16_t cos_y_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle_y) >> 2);
123 int32_t sin_y_q12 = Math_SinQ14(angle_y) >> 2;
124
125 g_rotationYScratch.matrix.m00 = cos_y_q12;
126 g_rotationYScratch.matrix.m20 = (int16_t)-(sin_y_q12);
127 g_rotationYScratch.matrix.m02 = (uint16_t)sin_y_q12;
128 g_rotationYScratch.matrix.m22 = cos_y_q12;
129
130 int16_t cos_x_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle_x) >> 2);
131 int32_t sin_x_q12 = Math_SinQ14(angle_x) >> 2;
132
133 g_rotationXScratch.matrix.m21 = (uint16_t)sin_x_q12;
134 g_rotationXScratch.matrix.m12 = (int16_t)-(sin_x_q12);
135 g_rotationXScratch.matrix.m11 = cos_x_q12;
136 g_rotationXScratch.matrix.m22 = cos_x_q12;
137
138 if (!left_factor)
139 {
140 Math_MulMat3x3Q12(out_matrix, &g_rotationXScratch.matrix,
141 &g_rotationYScratch.matrix);
142
143 return;
144 }
145
146 struct Math_Matrix3x3I16 temp_y_post_matrix;
147
148 Math_MulMat3x3Q12(&temp_y_post_matrix, &g_rotationYScratch.matrix, left_factor);
149 Math_MulMat3x3Q12(out_matrix, &g_rotationXScratch.matrix, &temp_y_post_matrix);
150}
151
152// Original pcdogs.exe: 0x42e3e0
153// Builds a Q12 rotation from three angles and an optional input matrix, retaining the original axis composition order and zero-angle branches.
154void Math_BuildRotationMatrix(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle_x, int32_t angle_y, int32_t angle_z)
155{
156 int16_t cos_x_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle_x) >> 2);
157 int32_t sin_x_q12 = Math_SinQ14(angle_x) >> 2;
158
159 // LookupSinCosTable(0x400 - angle) gives cos, LookupSinCosTable(angle) gives sin
160 // >>2 converts from lookup table format to 12-bit fixed point
161 g_rotationXScratch.matrix.m21 = (uint16_t)sin_x_q12;
162 g_rotationXScratch.matrix.m12 = (int16_t)-(sin_x_q12);
163 g_rotationXScratch.matrix.m11 = cos_x_q12;
164 g_rotationXScratch.matrix.m22 = cos_x_q12;
165
166 struct Math_Matrix3x3I16 temp_x_matrix;
167
168 if (!left_factor)
169 temp_x_matrix = g_rotationXScratch.matrix;
170 else
171 Math_MulMat3x3Q12(&temp_x_matrix, &g_rotationXScratch.matrix, left_factor);
172
173 int16_t cos_y_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle_y) >> 2);
174 int32_t sin_y_q12 = Math_SinQ14(angle_y) >> 2;
175
176 g_rotationYScratch.matrix.m00 = cos_y_q12;
177 g_rotationYScratch.matrix.m20 = (int16_t)-(sin_y_q12);
178 g_rotationYScratch.matrix.m02 = (uint16_t)sin_y_q12;
179 g_rotationYScratch.matrix.m22 = cos_y_q12;
180
181 if (!(uint16_t)angle_z)
182 {
183 Math_MulMat3x3Q12(out_matrix, &g_rotationYScratch.matrix, &temp_x_matrix);
184
185 return;
186 }
187
188 struct Math_Matrix3x3I16 temp_xy_matrix;
189
190 Math_MulMat3x3Q12(&temp_xy_matrix, &g_rotationYScratch.matrix, &temp_x_matrix);
191
192 int16_t cos_z_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle_z) >> 2);
193 int32_t sin_z_q12 = Math_SinQ14(angle_z) >> 2;
194
195 g_rotationZScratch.matrix.m10 = (uint16_t)sin_z_q12;
196 g_rotationZScratch.matrix.m00 = cos_z_q12;
197 g_rotationZScratch.matrix.m01 = (int16_t)-(sin_z_q12);
198 g_rotationZScratch.matrix.m11 = cos_z_q12;
199 Math_MulMat3x3Q12(out_matrix, &temp_xy_matrix, &g_rotationZScratch.matrix);
200}
201
202// Original pcdogs.exe: 0x42e540
203// Builds a Q12 rotation using supplied X/Y sine and cosine values, an optional input matrix, and an optional Z rotation. Uses shared axis scratch matrices and preserves the original composition order.
204void Math_BuildRotationMatrixFromXYSinCos(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int16_t sin_x_q12, int16_t cos_x_q12, int16_t sin_y_q12, int16_t cos_y_q12, int32_t angle_z)
205{
206 int32_t sin_x_extended = sin_x_q12;
207
208 g_rotationXScratch.matrix.m21 = (uint16_t)sin_x_extended;
209 g_rotationXScratch.matrix.m12 = (int16_t)-(sin_x_extended);
210 g_rotationXScratch.matrix.m11 = cos_x_q12;
211 g_rotationXScratch.matrix.m22 = cos_x_q12;
212
213 struct Math_Matrix3x3I16 temp_x_matrix;
214
215 if (!left_factor)
216 temp_x_matrix = g_rotationXScratch.matrix;
217 else
218 Math_MulMat3x3Q12(&temp_x_matrix, &g_rotationXScratch.matrix, left_factor);
219
220 int32_t sin_y_extended = sin_y_q12;
221
222 g_rotationYScratch.matrix.m00 = cos_y_q12;
223 g_rotationYScratch.matrix.m20 = (int16_t)-(sin_y_extended);
224 g_rotationYScratch.matrix.m02 = (uint16_t)sin_y_extended;
225 g_rotationYScratch.matrix.m22 = cos_y_q12;
226
227 if (!(uint16_t)angle_z)
228 {
229 Math_MulMat3x3Q12(out_matrix, &g_rotationYScratch.matrix, &temp_x_matrix);
230
231 return;
232 }
233
234 struct Math_Matrix3x3I16 temp_xy_matrix;
235
236 Math_MulMat3x3Q12(&temp_xy_matrix, &g_rotationYScratch.matrix, &temp_x_matrix);
237
238 int16_t cos_z_q12 = (int16_t)(Math_SinQ14(MATH_ANGLE_QUARTER_TURN - angle_z) >> 2);
239 int32_t sin_z_q12 = Math_SinQ14(angle_z) >> 2;
240
241 g_rotationZScratch.matrix.m10 = (uint16_t)sin_z_q12;
242 g_rotationZScratch.matrix.m01 = (int16_t)-(sin_z_q12);
243 g_rotationZScratch.matrix.m00 = cos_z_q12;
244 g_rotationZScratch.matrix.m11 = cos_z_q12;
245 Math_MulMat3x3Q12(out_matrix, &temp_xy_matrix, &g_rotationZScratch.matrix);
246}
247
248// Original pcdogs.exe: 0x440910
249// Converts a Q12 rotation matrix to a signed 16-bit quaternion using either the positive-trace or largest-diagonal branch. Square-root reciprocals are not guarded against zero.
251{
252 int16_t trace_m11 = matrix->m11;
253 int16_t trace_m00 = matrix->m00;
254 int16_t trace_m22 = matrix->m22;
255 int32_t trace = (int32_t)trace_m00 + (int32_t)trace_m11 + (int32_t)trace_m22;
256
257 if (trace > 0)
258 {
259 int32_t trace_root = Math_SqrtFP14((trace << 2) + MATH_Q14_ONE);
260
261 output_quat->w = (int16_t)(trace_root >> 1);
262
263 int32_t trace_reciprocal = (int64_t)MATH_QUATERNION_RECIPROCAL_SCALE_Q27 / trace_root;
264 int64_t quat_x_product = (int64_t)(((int32_t)matrix->m12 - (int32_t)matrix->m21) * 4) * trace_reciprocal;
265
266 output_quat->x = (int16_t)(quat_x_product >> 14);
267
268 int64_t quat_y_product = (int64_t)(((int32_t)matrix->m20 - (int32_t)matrix->m02) * 4) * trace_reciprocal;
269
270 output_quat->y = (int16_t)(quat_y_product >> 14);
271
272 int64_t quat_z_product = (int64_t)(((int32_t)matrix->m01 - (int32_t)matrix->m10) * 4) * trace_reciprocal;
273
274 output_quat->z = (int16_t)(quat_z_product >> 14);
275
276 return;
277 }
278
279 int32_t largest_axis_index = 0;
280
281 if (trace_m11 > trace_m00)
282 {
283 largest_axis_index = 1;
284 }
285
286 if (trace_m22 > matrix->elements[largest_axis_index * 4])
287 {
288 largest_axis_index = 2;
289 }
290
291 int32_t next_axis = largest_axis_index + 1;
292
293 if (next_axis == 3)
294 next_axis = 0;
295
296 int32_t other_axis = next_axis + 1;
297 int32_t other_axis_saved = other_axis;
298
299 if (other_axis == 3)
300 other_axis = 0;
301
302 int32_t sqrt_term = Math_SqrtFP14(((
303 (int32_t)matrix->elements[largest_axis_index * 4] - (int32_t)matrix->elements[other_axis * 4] - (int32_t)matrix->elements[next_axis * 4])
304 << 2) +
306
307 output_quat->components[largest_axis_index + 1] = (int16_t)(sqrt_term >> 1);
308
309 int32_t axis_reciprocal = (int64_t)MATH_QUATERNION_RECIPROCAL_SCALE_Q27 / sqrt_term;
310 int32_t next_axis_row_offset = next_axis * 3;
311 int64_t quat_w_product = (int64_t)(((int32_t)matrix->elements[next_axis_row_offset + other_axis] - (int32_t)matrix->elements[other_axis * 3 + next_axis]) * 4) * axis_reciprocal;
312
313 output_quat->w = (int16_t)(quat_w_product >> 14);
314
315 int64_t next_component_product = (int64_t)(((int32_t)matrix->elements[largest_axis_index * 3 + next_axis] + (int32_t)matrix->elements[next_axis_row_offset + largest_axis_index]) * 4) * axis_reciprocal;
316
317 output_quat->components[other_axis_saved] = (int16_t)(next_component_product >> 14);
318
319 int64_t other_component_product = (int64_t)(((int32_t)matrix->elements[largest_axis_index * 3 + other_axis] + (int32_t)matrix->elements[other_axis * 3 + largest_axis_index]) * 4) * axis_reciprocal;
320
321 output_quat->components[other_axis + 1] = (int16_t)(other_component_product >> 14);
322}
323
324/* The native IMUL keeps the low 32 bits before SAR. */
325static int32_t mul_q12(int32_t a, int32_t b)
326{
327 return (a * b) >> MATH_Q12_SHIFT;
328}
329
330/* Construct the half-turn 2uu^T - I about a perpendicular unit vector u. */
331static void build_opposite_rotation(struct Math_Matrix3x3I16 *out, int16_t x, int16_t y, int16_t z)
332{
333 int32_t axis_x = 0;
334 int32_t axis_y = z;
335 int32_t axis_z = (int16_t)-y;
336
337 /* Signed comparison after the wrapping addition at 440b61..440b6d. */
338 if ((axis_y * axis_y + axis_z * axis_z) < MATH_Q14_ONE)
339 {
340 axis_x = (int16_t)-z;
341 axis_y = 0;
342 axis_z = x;
343 }
344
345 int32_t length = Math_SqrtFP12((axis_x * axis_x + axis_y * axis_y + axis_z * axis_z) >> MATH_Q12_SHIFT);
346 if (length)
347 {
348 int32_t scale = INT64_C(0x100000000) / length;
349 axis_x = (int16_t)(((int64_t)axis_x * scale) >> 20);
350 axis_y = (int16_t)(((int64_t)axis_y * scale) >> 20);
351 axis_z = (int16_t)(((int64_t)axis_z * scale) >> 20);
352 }
353 else
354 axis_x = axis_y = axis_z = 0;
355
356 out->m00 = 2 * mul_q12(axis_x, axis_x) - MATH_Q12_ONE;
357 out->m01 = out->m10 = 2 * mul_q12(axis_x, axis_y);
358 out->m02 = out->m20 = 2 * mul_q12(axis_x, axis_z);
359 out->m11 = 2 * mul_q12(axis_y, axis_y) - MATH_Q12_ONE;
360 out->m12 = out->m21 = 2 * mul_q12(axis_y, axis_z);
361 out->m22 = 2 * mul_q12(axis_z, axis_z) - MATH_Q12_ONE;
362}
363
364// Original pcdogs.exe: 0x440ae0
365// Builds a Q12 rotation between two normalized vectors.
366void Math_BuildRotationFromVectors(struct Math_Vec3I16 *from_vec, struct Math_Vec3I16 *to_vec, struct Math_Matrix3x3I16 *out_matrix)
367{
368 int32_t x = from_vec->x, y = from_vec->y, z = from_vec->z;
369 int32_t tx = to_vec->x, ty = to_vec->y, tz = to_vec->z;
370 int32_t dot = (x * tx + y * ty + z * tz) >> MATH_Q12_SHIFT;
372 {
373 build_opposite_rotation(out_matrix, tx, ty, tz);
374 return;
375 }
376
378 {
379 /* The cross product rounds each product before subtraction and
380 narrows each component to a signed word before normalization. */
381 int32_t cx = (int16_t)(mul_q12(y, tz) - mul_q12(z, ty));
382 int32_t cy = (int16_t)(mul_q12(z, tx) - mul_q12(x, tz));
383 int32_t cz = (int16_t)(mul_q12(x, ty) - mul_q12(y, tx));
384 int32_t length_sq = (cx * cx + cy * cy + cz * cz) >> MATH_Q12_SHIFT;
385 if (length_sq > 4)
386 {
387 int32_t scale = ((int64_t)(MATH_Q12_ONE - dot) * MATH_Q12_ONE) / length_sq;
388 int32_t xy = mul_q12(mul_q12(cx, cy), scale);
389 int32_t xz = mul_q12(mul_q12(cx, cz), scale);
390 int32_t yz = mul_q12(mul_q12(cy, cz), scale);
391
392 out_matrix->m00 = dot + mul_q12(mul_q12(cx, cx), scale);
393 out_matrix->m01 = xy - cz;
394 out_matrix->m02 = xz + cy;
395 out_matrix->m10 = xy + cz;
396 out_matrix->m11 = dot + mul_q12(mul_q12(cy, cy), scale);
397 out_matrix->m12 = yz - cx;
398 out_matrix->m20 = xz - cy;
399 out_matrix->m21 = yz + cx;
400 out_matrix->m22 = dot + mul_q12(mul_q12(cz, cz), scale);
401 return;
402 }
403 }
404
405 *out_matrix = (struct Math_Matrix3x3I16){
406 .m00 = MATH_Q12_ONE, .m11 = MATH_Q12_ONE, .m22 = MATH_Q12_ONE};
407}
#define MATH_Q14_ONE
Definition constants.h:11
#define MATH_Q12_SHIFT
Definition constants.h:8
#define MATH_ROTATION_PARALLEL_DOT_Q12
Definition constants.h:32
#define MATH_ANGLE_QUARTER_TURN
Definition constants.h:15
#define MATH_QUATERNION_RECIPROCAL_SCALE_Q27
Definition constants.h:33
#define MATH_Q12_ONE
Definition constants.h:9
int32_t Math_SinQ14(int32_t angle)
int32_t Math_SqrtFP14(uint32_t value)
int32_t Math_SqrtFP12(uint32_t value)
#define g_rotationXScratch
Definition globals.h:29
#define g_rotationZScratch
Definition globals.h:31
#define g_rotationYScratch
Definition globals.h:30
static void build_opposite_rotation(struct Math_Matrix3x3I16 *out, int16_t x, int16_t y, int16_t z)
Definition matrices.c:331
void Math_BuildMatrixRotationY(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle)
Definition matrices.c:76
static int32_t mul_q12(int32_t a, int32_t b)
Definition matrices.c:325
void Math_BuildRotationFromVectors(struct Math_Vec3I16 *from_vec, struct Math_Vec3I16 *to_vec, struct Math_Matrix3x3I16 *out_matrix)
Definition matrices.c:366
void Math_BuildQuaternionFromMatrix(struct Math_Matrix3x3I16 *matrix, struct Math_QuaternionI16 *output_quat)
Definition matrices.c:250
void Math_BuildMatrixRotationX(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle)
Definition matrices.c:54
void Math_BuildRotationMatrixFromXYSinCos(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int16_t sin_x_q12, int16_t cos_x_q12, int16_t sin_y_q12, int16_t cos_y_q12, int32_t angle_z)
Definition matrices.c:204
void Math_BuildMatrixRotationZ(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle)
Definition matrices.c:98
void Math_MulMat3x3Q12(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *right_factor, struct Math_Matrix3x3I16 const *left_factor)
Definition matrices.c:11
static void copy_rotation_matrix(struct Math_Matrix3x3I16 *out, const union Math_RotationScratch *scratch)
Definition matrices.c:40
void Math_BuildMatrixRotationXY(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle_x, int32_t angle_y)
Definition matrices.c:120
void Math_BuildRotationMatrix(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *left_factor, int32_t angle_x, int32_t angle_y, int32_t angle_z)
Definition matrices.c:154
int16_t m21
Definition types.h:169
int16_t m00
Definition types.h:162
int16_t m01
Definition types.h:163
int16_t elements[9]
Definition types.h:173
int16_t m12
Definition types.h:167
int16_t m11
Definition types.h:166
int16_t m20
Definition types.h:168
int16_t m22
Definition types.h:170
int16_t m02
Definition types.h:164
int16_t m10
Definition types.h:165
int16_t components[4]
Definition types.h:214
int16_t z
Definition types.h:460
int16_t x
Definition types.h:458
int16_t y
Definition types.h:459
struct Math_Matrix3x3I16 matrix
Definition types.h:573