102 Dalmatians: Puppies to the Rescue
Decompilation Source Reference | GitLab repository
Loading...
Searching...
No Matches
quaternion.c
Go to the documentation of this file.
1/* Recovered game implementation; original addresses identify provenance. */
7#include "pcdogs/math/types.h"
8#include <string.h>
9
10// Original pcdogs.exe: no direct counterpart (decomp helper)
11// Multiplies signed components and returns the low 32 bits after shifting the product right by 14 bits.
12static int32_t Quaternion_MultiplyQ14(int32_t left_factor, int32_t right_factor)
13{
14 return (int32_t)((uint64_t)((int64_t)left_factor * right_factor) >> 14);
15}
16
17// Original pcdogs.exe: no direct counterpart (decomp helper)
18// Blends two quaternion components with Q14 weights. Each product is shifted separately before wrapping addition and narrowing to int16.
19static int16_t Quaternion_BlendComponent(int16_t from_component, int16_t to_component,
20 int32_t from_weight, int32_t to_weight)
21{
22 /* Each product is shifted separately before the wrapping ADD and word store. */
23 uint32_t weighted_from_component = (uint32_t)Quaternion_MultiplyQ14(from_component, from_weight);
24 uint32_t weighted_to_component = (uint32_t)Quaternion_MultiplyQ14(to_component, to_weight);
25
26 return (int16_t)(weighted_from_component + weighted_to_component);
27}
28
29/* Original endpoint copies alternate one dword load and one dword store. */
30static void copy_packed_quaternion(struct Math_QuaternionI16 *out, const struct Math_QuaternionI16 *source)
31{
32 for (unsigned i = 0; i < 4; i += 2)
33 {
34 uint32_t pair;
35 memcpy(&pair, &source->components[i], sizeof(pair));
36 memcpy(&out->components[i], &pair, sizeof(pair));
37 }
38}
39
40static int32_t quaternion_dot_q14(const struct Math_QuaternionI16 *a, const struct Math_QuaternionI16 *b)
41{
42 const int32_t dot = a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
43 return dot >> 14;
44}
45
46// Original pcdogs.exe: 0x40c540
47// Blend toward the sampled orientation, selecting the shorter hemisphere.
48void Animation_InterpolateKeyframeQuatBlend(uint32_t frame_time, int32_t blend_weight_q14, struct Animation_SplineChannelQuat *quat_track, struct Math_QuaternionI16 *inout_quat)
49{
50 struct Math_QuaternionI16 sampled;
51 Animation_InterpolateQuat(frame_time, quat_track, &sampled);
52
53 int32_t dot = quaternion_dot_q14(inout_quat, &sampled);
54 int32_t from_weight = MATH_Q14_ONE - blend_weight_q14;
55 int32_t to_weight = blend_weight_q14;
56
57 /* Native JB includes -0x3ff0 in the spherical interval and excludes +0x3ff0. */
58 if ((uint32_t)dot + 0x3ff0u < 0x7fe0u)
59 {
60 int32_t angle = Math_AcosRatioFP12(dot < 0 ? -dot : dot, MATH_Q14_ONE);
61 int32_t phase = ((int16_t)angle * blend_weight_q14) >> 14;
62 int32_t reciprocal = (MATH_Q14_ONE * MATH_Q14_ONE) / Math_SinQ14(angle);
63 from_weight = Quaternion_MultiplyQ14(Math_SinQ14(angle - phase), reciprocal);
64 to_weight = Quaternion_MultiplyQ14(Math_SinQ14(phase), reciprocal);
65 }
66
67 if (dot < 0)
68 to_weight = -to_weight;
69 for (unsigned i = 0; i < 4; ++i)
70 inout_quat->components[i] = Quaternion_BlendComponent(inout_quat->components[i], sampled.components[i], from_weight, to_weight);
71}
72
73static uint32_t key_time_q6(uint32_t packed_timing)
74{
75 return (packed_timing >> 14) & ANIMATION_SPLINE_TIME_Q6_MASK;
76}
77
78/* Select the first key strictly later than the sample. The caller has already
79 established that the final key lies after the sample. */
80static int next_rotation_key(const struct Animation_RotationKeyframe *keys, int last, uint32_t sample_time)
81{
82 int low = 1;
83 if (last <= 3)
84 {
85 while (sample_time >= key_time_q6(keys[low].timing.packed))
86 ++low;
87 return low;
88 }
89
90 int high = last;
91 while (low <= high)
92 {
93 int middle = (low + high) / 2;
94 if (sample_time < key_time_q6(keys[middle].timing.packed))
95 high = middle - 1;
96 else
97 low = middle + 1;
98 }
99 return low;
100}
101
102/* Hamilton product. Each individual product is shifted before the sum.
103 An overlapping base must see preceding output components in native order. */
104static void compose_spin(struct Math_QuaternionI16 *out, const struct Math_QuaternionI16 *base, const struct Math_QuaternionI16 *spin)
105{
106 if (base->w == MATH_Q14_ONE)
107 {
108 *out = *spin;
109 return;
110 }
111 if (spin->w == MATH_Q14_ONE)
112 {
113 copy_packed_quaternion(out, base);
114 return;
115 }
116
117 out->w = Quaternion_MultiplyQ14(base->w, spin->w) - Quaternion_MultiplyQ14(base->x, spin->x) - Quaternion_MultiplyQ14(base->y, spin->y) - Quaternion_MultiplyQ14(base->z, spin->z);
118 out->x = Quaternion_MultiplyQ14(base->w, spin->x) + Quaternion_MultiplyQ14(base->x, spin->w) + Quaternion_MultiplyQ14(base->y, spin->z) - Quaternion_MultiplyQ14(base->z, spin->y);
119 out->y = Quaternion_MultiplyQ14(base->w, spin->y) + Quaternion_MultiplyQ14(base->y, spin->w) + Quaternion_MultiplyQ14(base->z, spin->x) - Quaternion_MultiplyQ14(base->x, spin->z);
120 out->z = Quaternion_MultiplyQ14(base->w, spin->z) + Quaternion_MultiplyQ14(base->z, spin->w) + Quaternion_MultiplyQ14(base->x, spin->y) - Quaternion_MultiplyQ14(base->y, spin->x);
121}
122
123// Original pcdogs.exe: 0x40c790
124// Sample a keyed or constant quaternion channel. The final segment wraps to the first key.
125void Animation_InterpolateQuat(uint32_t frame_time, struct Animation_SplineChannelQuat *quat_track, struct Math_QuaternionI16 *out_quat)
126{
128 if ((uintptr_t)keys == UINT32_MAX)
129 {
130 const struct Animation_SplineChannelQuatConstantView *constant = (const void *)quat_track;
131 copy_packed_quaternion(out_quat, &constant->value_q14);
132 return;
133 }
134
135 uint32_t timing = (uint32_t)quat_track->packed_time_bias_and_end_time;
136 uint32_t sample_time = frame_time + ((timing & ANIMATION_TIME_BIAS_MASK) << 6);
137 int last = (int)quat_track->keyframe_count - 1;
138 uint32_t last_time = key_time_q6(keys[last].timing.packed);
139 struct Animation_RotationKeyframe *previous = &keys[last];
140 struct Animation_RotationKeyframe *next = keys;
141 uint32_t end_time = key_time_q6(timing);
142
143 if (sample_time == last_time)
144 {
145 copy_packed_quaternion(out_quat, &previous->rotation);
146 return;
147 }
148 if (sample_time < last_time)
149 {
150 next = &keys[next_rotation_key(keys, last, sample_time)];
151 previous = next - 1;
152 end_time = key_time_q6(next->timing.packed);
153 }
154 else if (sample_time >= end_time)
155 {
157 if ((quat_track->channel_flags & wrap_flags) == wrap_flags)
158 previous = keys;
159 copy_packed_quaternion(out_quat, &previous->rotation);
160 return;
161 }
162
163 int32_t t = Animation_CalculateSplineParameter(sample_time, end_time, previous->timing.packed,
164 (uint8_t)next->timing.packed, (uint8_t *)keys) >>
165 6;
166 uint32_t flags = next->segment_flags;
167 int32_t turns = (uint8_t)flags;
168 int32_t rotation_angle = (flags & ANIMATION_QUATERNION_SPIN_ANGLE_MASK) >> 8;
169 int32_t tangent_angle = flags >> 20;
170 struct Math_QuaternionI16 rotation, tangent;
171
172 if (turns && rotation_angle == 0)
173 {
174 int32_t phase = (Quaternion_MultiplyQ14(t, turns * MATH_Q12_ONE) >> 1) & MATH_ANGLE_HALF_MASK;
175 int32_t cosine = Math_SinQ14(MATH_ANGLE_QUARTER_TURN - phase);
176 int32_t sine = Math_SinQ14(phase);
177 struct Math_QuaternionI16 spin = {
178 .w = (int16_t)cosine,
179 .x = (int16_t)Quaternion_MultiplyQ14(previous->tangent_out.x, sine),
180 .y = (int16_t)Quaternion_MultiplyQ14(previous->tangent_out.y, sine),
181 .z = (int16_t)Quaternion_MultiplyQ14(previous->tangent_out.z, sine)};
182 if (tangent_angle == 0)
183 {
184 compose_spin(out_quat, &previous->rotation, &spin);
185 return;
186 }
187 compose_spin(&rotation, &previous->rotation, &spin);
188 Animation_InterpolateQuaternionSlerp(&tangent, &previous->rotation, &next->tangent_in, t, tangent_angle, turns);
189 }
190 else
191 {
192 Animation_InterpolateQuaternionSlerp(&rotation, &previous->rotation, &next->rotation, t, rotation_angle, turns);
193 Animation_InterpolateQuaternionSlerp(&tangent, &previous->tangent_out, &next->tangent_in, t, tangent_angle, turns);
194 }
195
196 int32_t curve_weight = 2 * (t - Quaternion_MultiplyQ14(t, t));
197 int32_t curve_angle = Math_AcosRatioFP12(quaternion_dot_q14(&rotation, &tangent), MATH_Q14_ONE);
198 Animation_InterpolateQuaternionSlerp(out_quat, &rotation, &tangent, curve_weight, curve_angle, 0);
199}
200
201// Original pcdogs.exe: 0x40ce70
202// Interpolates quaternion components with angle and spin information, using linear interpolation for zero angle or sine and conditional normalization otherwise. It performs no hemisphere correction, preserves ordered word stores for overlapping inputs, and retains the divide exception when the normalization magnitude is zero.
204 struct Math_QuaternionI16 *from_quat, struct Math_QuaternionI16 *to_quat,
205 int32_t blend_weight_q14, int32_t angle_fp12, int32_t spin_phase)
206{
207 int32_t reciprocal = 0;
208 int32_t from_weight = (int32_t)(0x4000u - (uint32_t)blend_weight_q14);
209 int32_t to_weight = blend_weight_q14;
210
211 if ((uint16_t)angle_fp12 != 0)
212 {
213 int32_t sine = Math_SinQ14(angle_fp12);
214
215 if (sine != 0)
216 {
217 reciprocal = 0x10000000 / sine;
218
219 int32_t phase_sum = (int16_t)((uint32_t)spin_phase << 11) + (int16_t)angle_fp12;
220 /* Native two-operand IMUL discards the high word before SAR. */
221 int32_t phase_product = (int32_t)((uint32_t)phase_sum * (uint32_t)blend_weight_q14);
222 int32_t phase = phase_product >> 14;
223 int32_t remaining_angle = (int32_t)((uint32_t)angle_fp12 - (uint32_t)phase);
224
225 from_weight = Quaternion_MultiplyQ14(Math_SinQ14(remaining_angle), reciprocal);
226 to_weight = Quaternion_MultiplyQ14(Math_SinQ14(phase), reciprocal);
227 }
228 }
229
230 /* These must remain four ordered word stores. In-place endpoints and
231 * partially overlapping packed records observe each preceding write. */
232 volatile struct Math_QuaternionI16 *out = out_quat;
233
234 out->w = Quaternion_BlendComponent(from_quat->w, to_quat->w, from_weight, to_weight);
235 out->x = Quaternion_BlendComponent(from_quat->x, to_quat->x, from_weight, to_weight);
236 out->y = Quaternion_BlendComponent(from_quat->y, to_quat->y, from_weight, to_weight);
237 out->z = Quaternion_BlendComponent(from_quat->z, to_quat->z, from_weight, to_weight);
238
239 if (reciprocal <= 0x50000)
240 return;
241
242 int32_t w = out->w, x = out->x, y = out->y, z = out->z;
243 uint32_t norm_bits = (uint32_t)(w * w) + (uint32_t)(x * x) + (uint32_t)(y * y) + (uint32_t)(z * z);
244 int32_t norm_q14 = (int32_t)norm_bits >> 14;
245
246 /* 40cfdf is JAE: the addition wraps and the comparison is unsigned. */
247 if ((uint32_t)norm_q14 + 0x3ff0u >= 0x7fe0u)
248 return;
249
250 /* Native DIV is unsigned. A zero magnitude deliberately retains its native
251 * divide exception; introducing a zero-quaternion fallback changes behavior. */
252 uint32_t scale = 0x10000000u / (uint32_t)Math_SqrtFP14((uint32_t)norm_q14);
253
254 out->w = (int16_t)Quaternion_MultiplyQ14(out->w, (int32_t)scale);
255 out->x = (int16_t)Quaternion_MultiplyQ14(out->x, (int32_t)scale);
256 out->y = (int16_t)Quaternion_MultiplyQ14(out->y, (int32_t)scale);
257 out->z = (int16_t)Quaternion_MultiplyQ14(out->z, (int32_t)scale);
258}
259
260// Original pcdogs.exe: 0x40d120
261// Samples a Q14 quaternion channel and converts it into a Q12 rotation matrix.
262void Animation_ConvertQuatToRotMatrix(uint32_t frame_time, struct Animation_SplineChannelQuat *quat_track, struct Math_Matrix3x3I16 *out_matrix)
263{
264 struct Math_QuaternionI16 sampled_quat;
265
266 Animation_InterpolateQuat(frame_time, quat_track, &sampled_quat);
267
268 int32_t w = sampled_quat.w, x = sampled_quat.x, y = sampled_quat.y, z = sampled_quat.z;
269 const int32_t half_q28 = (MATH_Q14_ONE * MATH_Q14_ONE) / 2;
270
271 /* Twice a Q28 product becomes Q12 after a shift of 15. */
272 *out_matrix = (struct Math_Matrix3x3I16){
273 .m00 = (half_q28 - y * y - z * z) >> 15,
274 .m01 = (x * y + w * z) >> 15,
275 .m02 = (x * z - w * y) >> 15,
276 .m10 = (x * y - w * z) >> 15,
277 .m11 = (half_q28 - x * x - z * z) >> 15,
278 .m12 = (y * z + w * x) >> 15,
279 .m20 = (x * z + w * y) >> 15,
280 .m21 = (y * z - w * x) >> 15,
281 .m22 = (half_q28 - x * x - y * y) >> 15};
282}
#define ANIMATION_QUATERNION_SPIN_ANGLE_MASK
Definition constants.h:40
#define ANIMATION_SPLINE_REPEAT_ENDPOINT
Definition constants.h:31
#define ANIMATION_SPLINE_TIME_Q6_MASK
Definition constants.h:37
#define ANIMATION_TIME_BIAS_MASK
Definition constants.h:39
#define ANIMATION_SPLINE_WRAP_SAMPLE_TIME
Definition enums.h:55
int32_t Animation_CalculateSplineParameter(uint32_t sample_time, uint32_t next_key_time, uint32_t prev_key_packed, uint8_t next_ease_index, uint8_t *keyframe_data)
#define MATH_Q14_ONE
Definition constants.h:11
#define MATH_ANGLE_QUARTER_TURN
Definition constants.h:15
#define MATH_ANGLE_HALF_MASK
Definition constants.h:17
#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_AcosRatioFP12(int32_t numerator, int32_t denominator)
static void compose_spin(struct Math_QuaternionI16 *out, const struct Math_QuaternionI16 *base, const struct Math_QuaternionI16 *spin)
Definition quaternion.c:104
void Animation_InterpolateQuat(uint32_t frame_time, struct Animation_SplineChannelQuat *quat_track, struct Math_QuaternionI16 *out_quat)
Definition quaternion.c:125
static int next_rotation_key(const struct Animation_RotationKeyframe *keys, int last, uint32_t sample_time)
Definition quaternion.c:80
static int16_t Quaternion_BlendComponent(int16_t from_component, int16_t to_component, int32_t from_weight, int32_t to_weight)
Definition quaternion.c:19
void Animation_InterpolateKeyframeQuatBlend(uint32_t frame_time, int32_t blend_weight_q14, struct Animation_SplineChannelQuat *quat_track, struct Math_QuaternionI16 *inout_quat)
Definition quaternion.c:48
void Animation_ConvertQuatToRotMatrix(uint32_t frame_time, struct Animation_SplineChannelQuat *quat_track, struct Math_Matrix3x3I16 *out_matrix)
Definition quaternion.c:262
static int32_t quaternion_dot_q14(const struct Math_QuaternionI16 *a, const struct Math_QuaternionI16 *b)
Definition quaternion.c:40
static int32_t Quaternion_MultiplyQ14(int32_t left_factor, int32_t right_factor)
Definition quaternion.c:12
void Animation_InterpolateQuaternionSlerp(struct Math_QuaternionI16 *out_quat, struct Math_QuaternionI16 *from_quat, struct Math_QuaternionI16 *to_quat, int32_t blend_weight_q14, int32_t angle_fp12, int32_t spin_phase)
Definition quaternion.c:203
static void copy_packed_quaternion(struct Math_QuaternionI16 *out, const struct Math_QuaternionI16 *source)
Definition quaternion.c:30
static uint32_t key_time_q6(uint32_t packed_timing)
Definition quaternion.c:73
struct Math_QuaternionI16 tangent_out
Definition types.h:1165
union Animation_KeyframeTiming timing
Definition types.h:1162
struct Math_QuaternionI16 tangent_in
Definition types.h:1166
struct Math_QuaternionI16 rotation
Definition types.h:1163
struct Math_QuaternionI16 value_q14
Definition types.h:832
struct Animation_RotationKeyframe * keyframes_or_minus_one_sentinel
Definition types.h:323
int32_t packed_time_bias_and_end_time
Definition types.h:324
Animation_SplineChannelFlags channel_flags
Definition types.h:328
int16_t m00
Definition types.h:162
int16_t components[4]
Definition types.h:214