15 if (out_matrix == left_factor)
20 else if (out_matrix == right_factor)
22 saved = *right_factor;
23 right_factor = &saved;
27 for (
int col = 0; col < 3; ++col)
29 for (
int row = 0; row < 3; ++row)
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];
181 if (!(uint16_t)angle_z)
206 int32_t sin_x_extended = sin_x_q12;
220 int32_t sin_y_extended = sin_y_q12;
227 if (!(uint16_t)angle_z)
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;
261 output_quat->
w = (int16_t)(trace_root >> 1);
264 int64_t quat_x_product = (int64_t)(((int32_t)matrix->
m12 - (int32_t)matrix->
m21) * 4) * trace_reciprocal;
266 output_quat->
x = (int16_t)(quat_x_product >> 14);
268 int64_t quat_y_product = (int64_t)(((int32_t)matrix->
m20 - (int32_t)matrix->
m02) * 4) * trace_reciprocal;
270 output_quat->
y = (int16_t)(quat_y_product >> 14);
272 int64_t quat_z_product = (int64_t)(((int32_t)matrix->
m01 - (int32_t)matrix->
m10) * 4) * trace_reciprocal;
274 output_quat->
z = (int16_t)(quat_z_product >> 14);
279 int32_t largest_axis_index = 0;
281 if (trace_m11 > trace_m00)
283 largest_axis_index = 1;
286 if (trace_m22 > matrix->
elements[largest_axis_index * 4])
288 largest_axis_index = 2;
291 int32_t next_axis = largest_axis_index + 1;
296 int32_t other_axis = next_axis + 1;
297 int32_t other_axis_saved = other_axis;
303 (int32_t)matrix->
elements[largest_axis_index * 4] - (int32_t)matrix->
elements[other_axis * 4] - (int32_t)matrix->
elements[next_axis * 4])
307 output_quat->
components[largest_axis_index + 1] = (int16_t)(sqrt_term >> 1);
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;
313 output_quat->
w = (int16_t)(quat_w_product >> 14);
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;
317 output_quat->
components[other_axis_saved] = (int16_t)(next_component_product >> 14);
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;
321 output_quat->
components[other_axis + 1] = (int16_t)(other_component_product >> 14);
335 int32_t axis_z = (int16_t)-y;
340 axis_x = (int16_t)-z;
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);
354 axis_x = axis_y = axis_z = 0;
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)
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)