102 Dalmatians: Puppies to the Rescue
Decompilation Source Reference | GitLab repository
Loading...
Searching...
No Matches
joint_tracking.c
Go to the documentation of this file.
6#include "pcdogs/math/types.h"
8
9static int32_t project_sine(int32_t angle, int32_t coordinate)
10{
11 int32_t sine_q12 = Math_SinQ14(angle) >> 2;
12 return (int32_t)(((int64_t)sine_q12 * coordinate) >> MATH_Q12_SHIFT);
13}
14
15static int32_t project_pair(int32_t first_angle, int32_t first, int32_t second_angle, int32_t second)
16{
17 /* The two products lose their fractional bits before the 32-bit add. */
18 return (int32_t)((uint32_t)project_sine(first_angle, first) +
19 (uint32_t)project_sine(second_angle, second));
20}
21
22static bool inside_cone(int16_t span, int16_t center, int32_t lateral, int32_t forward, int32_t radius)
23{
24 int32_t half_span = span >> 1;
25 if (span == 0 || half_span >= MATH_ANGLE_HALF_TURN)
26 return true;
27
28 int32_t center_distance = project_pair(center, lateral, MATH_ANGLE_QUARTER_TURN - center, forward);
29 int32_t upper = center + half_span;
30 int32_t lower = center - half_span;
31 if (half_span > MATH_ANGLE_QUARTER_TURN)
32 {
33 return center_distance >= -radius ||
34 project_pair(upper + MATH_ANGLE_QUARTER_TURN, lateral, -upper, forward) <= radius ||
35 project_pair(lower - MATH_ANGLE_QUARTER_TURN, lateral, MATH_ANGLE_HALF_TURN - lower, forward) <= radius;
36 }
37
38 return center_distance >= -radius &&
39 project_pair(upper - MATH_ANGLE_QUARTER_TURN, lateral, MATH_ANGLE_HALF_TURN - upper, forward) >= -radius &&
40 project_pair(lower + MATH_ANGLE_QUARTER_TURN, lateral, -lower, forward) >= -radius;
41}
42
43static int16_t step_tracking_angle(int16_t target, int16_t current, int16_t span, int16_t step)
44{
45 int16_t next;
46 if (target != 0 && span < MATH_ANGLE_FULL_TURN && current != 0 &&
47 ((MATH_ANGLE_HALF_TURN - target) ^ current) < 0)
48 {
49 /* Reverse direction through neutral on a mechanically limited joint. */
50 next = current < 0 ? current + (step >> 1) : current - (step >> 1);
51 }
52 else
53 {
54 next = Math_ClampAngleDelta(target, current, step);
55 if (next > MATH_ANGLE_HALF_TURN)
57 }
58
59 int16_t half_span = span >> 1;
60 if (((half_span + next) & MATH_ANGLE_WRAP_MASK) > span)
61 return next < 0 ? -half_span : half_span;
62 return next;
63}
64
65static int32_t transform_component(const int16_t *row, const struct Math_Vec3I32 *delta)
66{
67 int64_t dot = (int64_t)row[0] * delta->x + (int64_t)row[1] * delta->y + (int64_t)row[2] * delta->z;
68 return (int32_t)(dot >> MATH_Q12_SHIFT);
69}
70
71// Original pcdogs.exe: 0x40e650
72void Bone_UpdateJointTracking(struct Bone_JointTrackState *track, struct Actor_State *target_actor,
73 int32_t explicit_reference_joint_index)
74{
75 struct Bone_JointTrackStateView *state = (struct Bone_JointTrackStateView *)track;
76 struct Math_Matrix3x3I16 basis, inverse;
77 Math_MulMat3x3Q12(&basis, &state->base_rotation, &state->node->world_rot_matrix);
78 for (int row = 0; row < 3; ++row)
79 for (int col = 0; col < 3; ++col)
80 inverse.elements[3 * row + col] = basis.elements[3 * col + row];
81
82 int16_t yaw = 0;
83 int16_t pitch = 0;
84 if (target_actor)
85 {
86 const struct Math_Vec3I32 *target = &target_actor->world_render_pos;
87 const struct Math_Vec3I32 *origin = &state->node->world_pos;
88 struct Math_Vec3I32 delta = {
89 .x = (int32_t)((uint32_t)target->x - origin->x),
90 .y = (int32_t)((uint32_t)target->y - origin->y),
91 .z = (int32_t)((uint32_t)target->z - origin->z)};
92 int32_t x = transform_component(&inverse.elements[0], &delta);
93 int32_t y = transform_component(&inverse.elements[3], &delta);
94 int32_t z = transform_component(&inverse.elements[6], &delta);
95 uint64_t squared = (uint64_t)((int64_t)x * x) + (uint64_t)((int64_t)y * y) + (uint64_t)((int64_t)z * z);
96 int32_t distance = Math_SqrtFP12((int32_t)(squared >> MATH_Q12_SHIFT));
97 int32_t radius = target_actor->cull_radius * 32;
98 if (explicit_reference_joint_index ||
99 (inside_cone(state->yaw_span_fp12, state->yaw_center_fp12, x, z, radius) &&
101 {
103 pitch = (Math_AsinRatioFP12(y, distance) - state->pitch_center_fp12) & MATH_ANGLE_WRAP_MASK;
104 }
105 }
106
107 state->current_pitch = step_tracking_angle(pitch, state->current_pitch, state->pitch_span_fp12, state->max_angle_step);
108 state->current_yaw = step_tracking_angle(yaw, state->current_yaw, state->yaw_span_fp12, state->max_angle_step);
109 if (state->current_yaw || state->current_pitch)
110 {
111 struct Math_Matrix3x3I16 rotation;
112 Math_MulMat3x3Q12(&inverse, &state->node->world_rot_matrix, &inverse);
113 Math_BuildMatrixRotationXY(&rotation, &basis, -state->current_pitch, state->current_yaw);
114 Math_MulMat3x3Q12(&state->node->world_rot_matrix, &inverse, &rotation);
115 }
116}
static int32_t project_pair(int32_t first_angle, int32_t first, int32_t second_angle, int32_t second)
void Bone_UpdateJointTracking(struct Bone_JointTrackState *track, struct Actor_State *target_actor, int32_t explicit_reference_joint_index)
static int32_t project_sine(int32_t angle, int32_t coordinate)
static int32_t transform_component(const int16_t *row, const struct Math_Vec3I32 *delta)
static int16_t step_tracking_angle(int16_t target, int16_t current, int16_t span, int16_t step)
static bool inside_cone(int16_t span, int16_t center, int32_t lateral, int32_t forward, int32_t radius)
#define MATH_Q12_SHIFT
Definition constants.h:8
#define MATH_ANGLE_QUARTER_TURN
Definition constants.h:15
#define MATH_ANGLE_WRAP_MASK
Definition constants.h:16
#define MATH_ANGLE_HALF_TURN
Definition constants.h:14
#define MATH_ANGLE_FULL_TURN
Definition constants.h:13
int32_t Math_SinQ14(int32_t angle)
int32_t Math_SqrtFP12(uint32_t value)
uint16_t Math_ClampAngleDelta(int16_t target_angle, int16_t current_angle, int16_t max_delta)
void Math_MulMat3x3Q12(struct Math_Matrix3x3I16 *out_matrix, struct Math_Matrix3x3I16 const *right_factor, struct Math_Matrix3x3I16 const *left_factor)
Definition matrices.c:11
int16_t Math_CalculateAtan2FP12(int32_t y, int32_t x)
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
int32_t Math_AsinRatioFP12(int32_t numerator, int32_t denominator)
int16_t cull_radius
Definition types.h:424
struct Math_Vec3I32 world_render_pos
Definition types.h:420
int16_t pitch_center_fp12
Definition types.h:910
struct Scene_Node * node
Definition types.h:917
struct Math_Matrix3x3I16 base_rotation
Definition types.h:915
int16_t elements[9]
Definition types.h:173
int32_t y
Definition types.h:471
int32_t x
Definition types.h:470
int32_t z
Definition types.h:472
struct Math_Matrix3x3I16 world_rot_matrix
Definition types.h:118
struct Math_Vec3I32 world_pos
Definition types.h:120