1use super::random::random_vec3;
7use super::types::{BoneId, Human, BONE_COUNT};
8use crate::body::{
9 body_apply_angular_impulse, body_get_transform, body_set_bullet, body_set_linear_velocity,
10 create_body, destroy_body,
11};
12use crate::id::{NULL_BODY_ID, NULL_JOINT_ID};
13use crate::joint::{
14 create_motor_joint, create_parallel_joint, destroy_joint, revolute_joint_set_max_motor_torque,
15 revolute_joint_set_spring_damping_ratio, revolute_joint_set_spring_hertz,
16 spherical_joint_set_max_motor_torque, spherical_joint_set_spring_damping_ratio,
17 spherical_joint_set_spring_hertz, JointType,
18};
19use crate::math_functions::{
20 compute_quat_between_unit_vectors, inv_mul_quat, neg, Vec3, VEC3_AXIS_Y, VEC3_AXIS_Z,
21};
22use crate::types::{
23 default_body_def, default_motor_joint_def, default_parallel_joint_def, BodyType,
24};
25use crate::world::World;
26use crate::BodyId;
27
28pub fn destroy_human(human: &mut Human, world: &mut World) {
31 debug_assert!(human.is_spawned);
32
33 for i in 0..human.filter_joint_count as usize {
34 destroy_joint(world, human.filter_joints[i], false);
35 human.filter_joints[i] = NULL_JOINT_ID;
36 }
37
38 for i in 0..BONE_COUNT {
39 if human.bones[i].joint_id.is_null() {
40 continue;
41 }
42 destroy_joint(world, human.bones[i].joint_id, false);
43 human.bones[i].joint_id = NULL_JOINT_ID;
44 }
45
46 for i in 0..BONE_COUNT {
47 if human.bones[i].body_id.is_null() {
48 continue;
49 }
50 destroy_body(world, human.bones[i].body_id);
51 human.bones[i].body_id = NULL_BODY_ID;
52 }
53
54 human.is_spawned = false;
55}
56
57pub fn human_set_velocity(human: &Human, world: &mut World, velocity: Vec3) {
59 for i in 0..BONE_COUNT {
60 let body_id = human.bones[i].body_id;
61 if body_id.is_null() {
62 continue;
63 }
64 body_set_linear_velocity(world, body_id, velocity);
65 }
66}
67
68pub fn human_apply_random_angular_impulse(human: &Human, world: &mut World, magnitude: f32) {
70 debug_assert!(human.is_spawned);
71 let range = Vec3 {
72 x: magnitude,
73 y: magnitude,
74 z: magnitude,
75 };
76 let impulse = random_vec3(neg(range), range);
77 body_apply_angular_impulse(
78 world,
79 human.bones[BoneId::Spine01 as usize].body_id,
80 impulse,
81 true,
82 );
83}
84
85pub fn human_set_joint_friction_torque(human: &mut Human, world: &mut World, torque: f32) {
87 debug_assert!(human.is_spawned);
88 human.friction_torque = torque;
89
90 for i in 1..BONE_COUNT {
91 let bone = &human.bones[i];
92 if bone.joint_type == JointType::Revolute {
93 revolute_joint_set_max_motor_torque(world, bone.joint_id, bone.joint_friction * torque);
94 } else {
95 spherical_joint_set_max_motor_torque(
96 world,
97 bone.joint_id,
98 bone.joint_friction * torque,
99 );
100 }
101 }
102}
103
104pub fn human_set_joint_spring_hertz(human: &Human, world: &mut World, hertz: f32) {
106 debug_assert!(human.is_spawned);
107 for i in 1..BONE_COUNT {
108 let bone = &human.bones[i];
109 if bone.joint_type == JointType::Revolute {
110 revolute_joint_set_spring_hertz(world, bone.joint_id, hertz);
111 } else {
112 spherical_joint_set_spring_hertz(world, bone.joint_id, hertz);
113 }
114 }
115}
116
117pub fn human_set_joint_damping_ratio(human: &Human, world: &mut World, damping_ratio: f32) {
119 debug_assert!(human.is_spawned);
120 for i in 1..BONE_COUNT {
121 let bone = &human.bones[i];
122 if bone.joint_type == JointType::Revolute {
123 revolute_joint_set_spring_damping_ratio(world, bone.joint_id, damping_ratio);
124 } else {
125 spherical_joint_set_spring_damping_ratio(world, bone.joint_id, damping_ratio);
126 }
127 }
128}
129
130pub fn human_align_spring(
132 human: &mut Human,
133 world: &mut World,
134 ground_id: BodyId,
135 hertz: f32,
136 damping_ratio: f32,
137) {
138 debug_assert!(human.is_spawned);
139
140 let bone = &mut human.bones[BoneId::Pelvis as usize];
141 debug_assert!(bone.joint_id.is_null());
142 let q = compute_quat_between_unit_vectors(VEC3_AXIS_Z, VEC3_AXIS_Y);
143 let qb = body_get_transform(world, bone.body_id).q;
144
145 let mut joint_def = default_parallel_joint_def();
146 joint_def.base.body_id_a = ground_id;
147 joint_def.base.body_id_b = bone.body_id;
148 joint_def.base.local_frame_a.q = q;
149 joint_def.base.local_frame_b.q = inv_mul_quat(qb, q);
150 joint_def.base.draw_scale = 2.0;
151 joint_def.base.collide_connected = true;
152 joint_def.hertz = hertz;
153 joint_def.damping_ratio = damping_ratio;
154
155 bone.joint_id = create_parallel_joint(world, &joint_def);
156}
157
158pub fn human_create_motor_anchors(human: &mut Human, world: &mut World) {
160 let mut anchor_def = default_body_def();
161 anchor_def.type_ = BodyType::Kinematic;
162
163 let mut motor_def = default_motor_joint_def();
164 motor_def.angular_hertz = 5.0;
165 motor_def.angular_damping_ratio = 1.0;
166 motor_def.linear_hertz = 5.0;
167 motor_def.linear_damping_ratio = 1.0;
168 motor_def.max_spring_force = f32::MAX;
169 motor_def.max_spring_torque = f32::MAX;
170
171 for i in 0..BONE_COUNT {
172 let bone = &mut human.bones[i];
173 let body_transform = body_get_transform(world, bone.body_id);
174 anchor_def.position = body_transform.p;
175 anchor_def.rotation = body_transform.q;
176 bone.anchor_id = create_body(world, &anchor_def);
177
178 motor_def.base.body_id_a = bone.anchor_id;
179 motor_def.base.body_id_b = bone.body_id;
180 bone.anchor_joint_id = create_motor_joint(world, &motor_def);
181 }
182}
183
184pub fn human_create_parallel_anchors(human: &mut Human, world: &mut World) {
186 let mut anchor_def = default_body_def();
187 anchor_def.type_ = BodyType::Kinematic;
188
189 let q_frame_world = compute_quat_between_unit_vectors(VEC3_AXIS_Z, VEC3_AXIS_Y);
190 let mut joint_def = default_parallel_joint_def();
191 joint_def.hertz = 8.0;
192 joint_def.damping_ratio = 1.0;
193 joint_def.max_torque = 800.0;
194
195 for i in 0..BONE_COUNT {
196 let bone = &mut human.bones[i];
197 let body_transform = body_get_transform(world, bone.body_id);
198 anchor_def.position = body_transform.p;
199 anchor_def.rotation = body_transform.q;
200 bone.anchor_id = create_body(world, &anchor_def);
201
202 joint_def.base.body_id_a = bone.anchor_id;
203 joint_def.base.body_id_b = bone.body_id;
204
205 let frame_quat = inv_mul_quat(body_transform.q, q_frame_world);
206 joint_def.base.local_frame_a.q = frame_quat;
207 joint_def.base.local_frame_b.q = frame_quat;
208
209 bone.anchor_joint_id = create_parallel_joint(world, &joint_def);
210 }
211}
212
213pub fn human_set_bullet(human: &Human, world: &mut World, flag: bool) {
215 for i in 0..BONE_COUNT {
216 body_set_bullet(world, human.bones[i].body_id, flag);
217 }
218}