1use crate::joint::{
3 DistanceJoint, Joint, JointEdge, JointSim, JointType, JointUnion, MotorJoint, ParallelJoint,
4 PrismaticJoint, RevoluteJoint, SphericalJoint, WeldJoint, WheelJoint,
5};
6use crate::math_functions::Vec2;
7use crate::recording::buffer::{RecBuffer, SnapReader};
8use crate::solver::Softness;
9
10impl RecBuffer {
11 pub fn append_vec2(&mut self, v: Vec2) {
12 self.append_f32(v.x);
13 self.append_f32(v.y);
14 }
15 pub fn append_softness(&mut self, v: Softness) {
16 self.append_f32(v.bias_rate);
17 self.append_f32(v.mass_scale);
18 self.append_f32(v.impulse_scale);
19 }
20}
21impl SnapReader<'_> {
22 pub fn vec2(&mut self) -> Vec2 {
23 Vec2 {
24 x: self.f32(),
25 y: self.f32(),
26 }
27 }
28 pub fn softness(&mut self) -> Softness {
29 Softness {
30 bias_rate: self.f32(),
31 mass_scale: self.f32(),
32 impulse_scale: self.f32(),
33 }
34 }
35}
36
37pub fn ser_joint(buf: &mut RecBuffer, j: &Joint) {
38 buf.append_u64(0); buf.append_i32(j.set_index);
40 buf.append_i32(j.color_index);
41 buf.append_i32(j.local_index);
42 for e in &j.edges {
43 buf.append_i32(e.body_id);
44 buf.append_i32(e.prev_key);
45 buf.append_i32(e.next_key);
46 }
47 buf.append_i32(j.joint_id);
48 buf.append_i32(j.island_id);
49 buf.append_i32(j.island_index);
50 buf.append_f32(j.draw_scale);
51 buf.append_i32(j.type_ as i32);
52 buf.append_u16(j.generation);
53 buf.append_bool(j.collide_connected);
54}
55
56pub fn des_joint(r: &mut SnapReader<'_>) -> Joint {
57 let _user = r.u64();
58 Joint {
59 user_data: 0,
60 set_index: r.i32(),
61 color_index: r.i32(),
62 local_index: r.i32(),
63 edges: [
64 JointEdge {
65 body_id: r.i32(),
66 prev_key: r.i32(),
67 next_key: r.i32(),
68 },
69 JointEdge {
70 body_id: r.i32(),
71 prev_key: r.i32(),
72 next_key: r.i32(),
73 },
74 ],
75 joint_id: r.i32(),
76 island_id: r.i32(),
77 island_index: r.i32(),
78 draw_scale: r.f32(),
79 type_: match r.i32() {
80 0 => JointType::Parallel,
81 1 => JointType::Distance,
82 2 => JointType::Filter,
83 3 => JointType::Motor,
84 4 => JointType::Prismatic,
85 5 => JointType::Revolute,
86 6 => JointType::Spherical,
87 7 => JointType::Weld,
88 _ => JointType::Wheel,
89 },
90 generation: r.u16(),
91 collide_connected: r.bool(),
92 }
93}
94
95pub fn ser_joint_sim(buf: &mut RecBuffer, s: &JointSim) {
96 buf.append_i32(s.joint_id);
97 buf.append_i32(s.body_id_a);
98 buf.append_i32(s.body_id_b);
99 buf.append_i32(s.type_ as i32);
100 buf.append_transform(s.local_frame_a);
101 buf.append_transform(s.local_frame_b);
102 buf.append_f32(s.inv_mass_a);
103 buf.append_f32(s.inv_mass_b);
104 buf.append_matrix3(s.inv_i_a);
105 buf.append_matrix3(s.inv_i_b);
106 buf.append_f32(s.constraint_hertz);
107 buf.append_f32(s.constraint_damping_ratio);
108 buf.append_softness(s.constraint_softness);
109 buf.append_f32(s.force_threshold);
110 buf.append_f32(s.torque_threshold);
111 buf.append_bool(s.fixed_rotation);
112 match &s.union_ {
113 JointUnion::Distance(j) => {
114 buf.append_i32(s.type_ as i32);
115 buf.append_f32(j.length);
116 buf.append_f32(j.hertz);
117 buf.append_f32(j.damping_ratio);
118 buf.append_f32(j.lower_spring_force);
119 buf.append_f32(j.upper_spring_force);
120 buf.append_f32(j.min_length);
121 buf.append_f32(j.max_length);
122 buf.append_f32(j.max_motor_force);
123 buf.append_f32(j.motor_speed);
124 buf.append_f32(j.impulse);
125 buf.append_f32(j.lower_impulse);
126 buf.append_f32(j.upper_impulse);
127 buf.append_f32(j.motor_impulse);
128 buf.append_i32(j.index_a);
129 buf.append_i32(j.index_b);
130 buf.append_vec3(j.anchor_a);
131 buf.append_vec3(j.anchor_b);
132 buf.append_vec3(j.delta_center);
133 buf.append_softness(j.distance_softness);
134 buf.append_f32(j.axial_mass);
135 buf.append_bool(j.enable_spring);
136 buf.append_bool(j.enable_limit);
137 buf.append_bool(j.enable_motor);
138 }
139 JointUnion::Motor(j) => {
140 buf.append_i32(s.type_ as i32);
141 buf.append_vec3(j.linear_velocity);
142 buf.append_vec3(j.angular_velocity);
143 buf.append_f32(j.max_velocity_force);
144 buf.append_f32(j.max_velocity_torque);
145 buf.append_f32(j.linear_hertz);
146 buf.append_f32(j.linear_damping_ratio);
147 buf.append_f32(j.max_spring_force);
148 buf.append_f32(j.angular_hertz);
149 buf.append_f32(j.angular_damping_ratio);
150 buf.append_f32(j.max_spring_torque);
151 buf.append_vec3(j.linear_velocity_impulse);
152 buf.append_vec3(j.angular_velocity_impulse);
153 buf.append_vec3(j.linear_spring_impulse);
154 buf.append_vec3(j.angular_spring_impulse);
155 buf.append_softness(j.linear_spring);
156 buf.append_softness(j.angular_spring);
157 buf.append_i32(j.index_a);
158 buf.append_i32(j.index_b);
159 buf.append_transform(j.frame_a);
160 buf.append_transform(j.frame_b);
161 buf.append_vec3(j.delta_center);
162 buf.append_matrix3(j.angular_mass);
163 }
164 JointUnion::Parallel(j) => {
165 buf.append_i32(s.type_ as i32);
166 buf.append_f32(j.hertz);
167 buf.append_f32(j.damping_ratio);
168 buf.append_f32(j.max_torque);
169 buf.append_vec2(j.perp_impulse);
170 buf.append_vec3(j.perp_axis_x);
171 buf.append_vec3(j.perp_axis_y);
172 buf.append_quat(j.quat_a);
173 buf.append_quat(j.quat_b);
174 buf.append_i32(j.index_a);
175 buf.append_i32(j.index_b);
176 buf.append_softness(j.softness);
177 }
178 JointUnion::Prismatic(j) => {
179 buf.append_i32(s.type_ as i32);
180 buf.append_vec2(j.perp_impulse);
181 buf.append_vec3(j.angular_impulse);
182 buf.append_f32(j.spring_impulse);
183 buf.append_f32(j.motor_impulse);
184 buf.append_f32(j.lower_impulse);
185 buf.append_f32(j.upper_impulse);
186 buf.append_f32(j.hertz);
187 buf.append_f32(j.damping_ratio);
188 buf.append_f32(j.max_motor_force);
189 buf.append_f32(j.motor_speed);
190 buf.append_f32(j.target_translation);
191 buf.append_f32(j.lower_translation);
192 buf.append_f32(j.upper_translation);
193 buf.append_i32(j.index_a);
194 buf.append_i32(j.index_b);
195 buf.append_transform(j.frame_a);
196 buf.append_transform(j.frame_b);
197 buf.append_vec3(j.joint_axis);
198 buf.append_vec3(j.perp_axis_y);
199 buf.append_vec3(j.perp_axis_z);
200 buf.append_vec3(j.delta_center);
201 buf.append_f32(j.delta_angle);
202 buf.append_matrix3(j.rotation_mass);
203 buf.append_softness(j.spring_softness);
204 buf.append_bool(j.enable_spring);
205 buf.append_bool(j.enable_limit);
206 buf.append_bool(j.enable_motor);
207 }
208 JointUnion::Revolute(j) => {
209 buf.append_i32(s.type_ as i32);
210 buf.append_vec3(j.linear_impulse);
211 buf.append_vec2(j.perp_impulse);
212 buf.append_f32(j.spring_impulse);
213 buf.append_f32(j.motor_impulse);
214 buf.append_f32(j.lower_impulse);
215 buf.append_f32(j.upper_impulse);
216 buf.append_f32(j.hertz);
217 buf.append_f32(j.damping_ratio);
218 buf.append_f32(j.max_motor_torque);
219 buf.append_f32(j.motor_speed);
220 buf.append_f32(j.target_angle);
221 buf.append_f32(j.lower_angle);
222 buf.append_f32(j.upper_angle);
223 buf.append_i32(j.index_a);
224 buf.append_i32(j.index_b);
225 buf.append_transform(j.frame_a);
226 buf.append_transform(j.frame_b);
227 buf.append_vec3(j.rotation_axis_z);
228 buf.append_vec3(j.perp_axis_x);
229 buf.append_vec3(j.perp_axis_y);
230 buf.append_vec3(j.delta_center);
231 buf.append_f32(j.delta_angle);
232 buf.append_f32(j.axial_mass);
233 buf.append_softness(j.spring_softness);
234 buf.append_bool(j.enable_spring);
235 buf.append_bool(j.enable_motor);
236 buf.append_bool(j.enable_limit);
237 }
238 JointUnion::Spherical(j) => {
239 buf.append_i32(s.type_ as i32);
240 buf.append_vec3(j.linear_impulse);
241 buf.append_vec3(j.spring_impulse);
242 buf.append_vec3(j.motor_impulse);
243 buf.append_f32(j.lower_twist_impulse);
244 buf.append_f32(j.upper_twist_impulse);
245 buf.append_f32(j.swing_impulse);
246 buf.append_f32(j.hertz);
247 buf.append_f32(j.damping_ratio);
248 buf.append_f32(j.max_motor_torque);
249 buf.append_vec3(j.motor_velocity);
250 buf.append_f32(j.lower_twist_angle);
251 buf.append_f32(j.upper_twist_angle);
252 buf.append_f32(j.cone_angle);
253 buf.append_quat(j.target_rotation);
254 buf.append_i32(j.index_a);
255 buf.append_i32(j.index_b);
256 buf.append_transform(j.frame_a);
257 buf.append_transform(j.frame_b);
258 buf.append_vec3(j.delta_center);
259 buf.append_vec3(j.swing_axis);
260 buf.append_vec3(j.twist_jacobian);
261 buf.append_matrix3(j.rotation_mass);
262 buf.append_f32(j.swing_mass);
263 buf.append_f32(j.twist_mass);
264 buf.append_softness(j.spring_softness);
265 buf.append_bool(j.enable_spring);
266 buf.append_bool(j.enable_motor);
267 buf.append_bool(j.enable_cone_limit);
268 buf.append_bool(j.enable_twist_limit);
269 }
270 JointUnion::Weld(j) => {
271 buf.append_i32(s.type_ as i32);
272 buf.append_f32(j.linear_hertz);
273 buf.append_f32(j.linear_damping_ratio);
274 buf.append_f32(j.angular_hertz);
275 buf.append_f32(j.angular_damping_ratio);
276 buf.append_softness(j.linear_spring);
277 buf.append_softness(j.angular_spring);
278 buf.append_vec3(j.linear_impulse);
279 buf.append_vec3(j.angular_impulse);
280 buf.append_i32(j.index_a);
281 buf.append_i32(j.index_b);
282 buf.append_transform(j.frame_a);
283 buf.append_transform(j.frame_b);
284 buf.append_vec3(j.delta_center);
285 buf.append_matrix3(j.angular_mass);
286 }
287 JointUnion::Wheel(j) => {
288 buf.append_i32(s.type_ as i32);
289 buf.append_vec2(j.linear_impulse);
290 buf.append_vec2(j.angular_impulse);
291 buf.append_f32(j.spin_impulse);
292 buf.append_f32(j.max_spin_torque);
293 buf.append_f32(j.spin_speed);
294 buf.append_f32(j.suspension_spring_impulse);
295 buf.append_f32(j.lower_suspension_impulse);
296 buf.append_f32(j.upper_suspension_impulse);
297 buf.append_f32(j.lower_suspension_limit);
298 buf.append_f32(j.upper_suspension_limit);
299 buf.append_f32(j.suspension_hertz);
300 buf.append_f32(j.suspension_damping_ratio);
301 buf.append_f32(j.steering_spring_impulse);
302 buf.append_f32(j.lower_steering_impulse);
303 buf.append_f32(j.upper_steering_impulse);
304 buf.append_f32(j.lower_steering_limit);
305 buf.append_f32(j.upper_steering_limit);
306 buf.append_f32(j.target_steering_angle);
307 buf.append_f32(j.max_steering_torque);
308 buf.append_f32(j.steering_hertz);
309 buf.append_f32(j.steering_damping_ratio);
310 buf.append_i32(j.index_a);
311 buf.append_i32(j.index_b);
312 buf.append_transform(j.frame_a);
313 buf.append_transform(j.frame_b);
314 buf.append_vec3(j.delta_center);
315 buf.append_f32(j.spin_mass);
316 buf.append_f32(j.suspension_mass);
317 buf.append_f32(j.steering_mass);
318 buf.append_softness(j.suspension_softness);
319 buf.append_softness(j.steering_softness);
320 buf.append_bool(j.enable_spin_motor);
321 buf.append_bool(j.enable_suspension_spring);
322 buf.append_bool(j.enable_suspension_limit);
323 buf.append_bool(j.enable_steering);
324 buf.append_bool(j.enable_steering_limit);
325 buf.append_bool(j.enable_steering_motor);
326 }
327 JointUnion::Filter => {
328 buf.append_i32(JointType::Filter as i32);
329 }
330 }
331}
332
333pub fn des_joint_sim(r: &mut SnapReader<'_>) -> JointSim {
334 let joint_id = r.i32();
335 let body_id_a = r.i32();
336 let body_id_b = r.i32();
337 let type_i = r.i32();
338 let type_ = match type_i {
339 0 => JointType::Parallel,
340 1 => JointType::Distance,
341 2 => JointType::Filter,
342 3 => JointType::Motor,
343 4 => JointType::Prismatic,
344 5 => JointType::Revolute,
345 6 => JointType::Spherical,
346 7 => JointType::Weld,
347 _ => JointType::Wheel,
348 };
349 let local_frame_a = r.transform();
350 let local_frame_b = r.transform();
351 let inv_mass_a = r.f32();
352 let inv_mass_b = r.f32();
353 let inv_i_a = r.matrix3();
354 let inv_i_b = r.matrix3();
355 let constraint_hertz = r.f32();
356 let constraint_damping_ratio = r.f32();
357 let constraint_softness = r.softness();
358 let force_threshold = r.f32();
359 let torque_threshold = r.f32();
360 let fixed_rotation = r.bool();
361 let union_tag = r.i32();
362 let union_ = match union_tag {
363 0 => JointUnion::Parallel(ParallelJoint {
364 hertz: r.f32(),
365 damping_ratio: r.f32(),
366 max_torque: r.f32(),
367 perp_impulse: r.vec2(),
368 perp_axis_x: r.vec3(),
369 perp_axis_y: r.vec3(),
370 quat_a: r.quat(),
371 quat_b: r.quat(),
372 index_a: r.i32(),
373 index_b: r.i32(),
374 softness: r.softness(),
375 }),
376 1 => JointUnion::Distance(DistanceJoint {
377 length: r.f32(),
378 hertz: r.f32(),
379 damping_ratio: r.f32(),
380 lower_spring_force: r.f32(),
381 upper_spring_force: r.f32(),
382 min_length: r.f32(),
383 max_length: r.f32(),
384 max_motor_force: r.f32(),
385 motor_speed: r.f32(),
386 impulse: r.f32(),
387 lower_impulse: r.f32(),
388 upper_impulse: r.f32(),
389 motor_impulse: r.f32(),
390 index_a: r.i32(),
391 index_b: r.i32(),
392 anchor_a: r.vec3(),
393 anchor_b: r.vec3(),
394 delta_center: r.vec3(),
395 distance_softness: r.softness(),
396 axial_mass: r.f32(),
397 enable_spring: r.bool(),
398 enable_limit: r.bool(),
399 enable_motor: r.bool(),
400 }),
401 2 => JointUnion::Filter,
402 3 => JointUnion::Motor(MotorJoint {
403 linear_velocity: r.vec3(),
404 angular_velocity: r.vec3(),
405 max_velocity_force: r.f32(),
406 max_velocity_torque: r.f32(),
407 linear_hertz: r.f32(),
408 linear_damping_ratio: r.f32(),
409 max_spring_force: r.f32(),
410 angular_hertz: r.f32(),
411 angular_damping_ratio: r.f32(),
412 max_spring_torque: r.f32(),
413 linear_velocity_impulse: r.vec3(),
414 angular_velocity_impulse: r.vec3(),
415 linear_spring_impulse: r.vec3(),
416 angular_spring_impulse: r.vec3(),
417 linear_spring: r.softness(),
418 angular_spring: r.softness(),
419 index_a: r.i32(),
420 index_b: r.i32(),
421 frame_a: r.transform(),
422 frame_b: r.transform(),
423 delta_center: r.vec3(),
424 angular_mass: r.matrix3(),
425 }),
426 4 => JointUnion::Prismatic(PrismaticJoint {
427 perp_impulse: r.vec2(),
428 angular_impulse: r.vec3(),
429 spring_impulse: r.f32(),
430 motor_impulse: r.f32(),
431 lower_impulse: r.f32(),
432 upper_impulse: r.f32(),
433 hertz: r.f32(),
434 damping_ratio: r.f32(),
435 max_motor_force: r.f32(),
436 motor_speed: r.f32(),
437 target_translation: r.f32(),
438 lower_translation: r.f32(),
439 upper_translation: r.f32(),
440 index_a: r.i32(),
441 index_b: r.i32(),
442 frame_a: r.transform(),
443 frame_b: r.transform(),
444 joint_axis: r.vec3(),
445 perp_axis_y: r.vec3(),
446 perp_axis_z: r.vec3(),
447 delta_center: r.vec3(),
448 delta_angle: r.f32(),
449 rotation_mass: r.matrix3(),
450 spring_softness: r.softness(),
451 enable_spring: r.bool(),
452 enable_limit: r.bool(),
453 enable_motor: r.bool(),
454 }),
455 5 => JointUnion::Revolute(RevoluteJoint {
456 linear_impulse: r.vec3(),
457 perp_impulse: r.vec2(),
458 spring_impulse: r.f32(),
459 motor_impulse: r.f32(),
460 lower_impulse: r.f32(),
461 upper_impulse: r.f32(),
462 hertz: r.f32(),
463 damping_ratio: r.f32(),
464 max_motor_torque: r.f32(),
465 motor_speed: r.f32(),
466 target_angle: r.f32(),
467 lower_angle: r.f32(),
468 upper_angle: r.f32(),
469 index_a: r.i32(),
470 index_b: r.i32(),
471 frame_a: r.transform(),
472 frame_b: r.transform(),
473 rotation_axis_z: r.vec3(),
474 perp_axis_x: r.vec3(),
475 perp_axis_y: r.vec3(),
476 delta_center: r.vec3(),
477 delta_angle: r.f32(),
478 axial_mass: r.f32(),
479 spring_softness: r.softness(),
480 enable_spring: r.bool(),
481 enable_motor: r.bool(),
482 enable_limit: r.bool(),
483 }),
484 6 => JointUnion::Spherical(SphericalJoint {
485 linear_impulse: r.vec3(),
486 spring_impulse: r.vec3(),
487 motor_impulse: r.vec3(),
488 lower_twist_impulse: r.f32(),
489 upper_twist_impulse: r.f32(),
490 swing_impulse: r.f32(),
491 hertz: r.f32(),
492 damping_ratio: r.f32(),
493 max_motor_torque: r.f32(),
494 motor_velocity: r.vec3(),
495 lower_twist_angle: r.f32(),
496 upper_twist_angle: r.f32(),
497 cone_angle: r.f32(),
498 target_rotation: r.quat(),
499 index_a: r.i32(),
500 index_b: r.i32(),
501 frame_a: r.transform(),
502 frame_b: r.transform(),
503 delta_center: r.vec3(),
504 swing_axis: r.vec3(),
505 twist_jacobian: r.vec3(),
506 rotation_mass: r.matrix3(),
507 swing_mass: r.f32(),
508 twist_mass: r.f32(),
509 spring_softness: r.softness(),
510 enable_spring: r.bool(),
511 enable_motor: r.bool(),
512 enable_cone_limit: r.bool(),
513 enable_twist_limit: r.bool(),
514 }),
515 7 => JointUnion::Weld(WeldJoint {
516 linear_hertz: r.f32(),
517 linear_damping_ratio: r.f32(),
518 angular_hertz: r.f32(),
519 angular_damping_ratio: r.f32(),
520 linear_spring: r.softness(),
521 angular_spring: r.softness(),
522 linear_impulse: r.vec3(),
523 angular_impulse: r.vec3(),
524 index_a: r.i32(),
525 index_b: r.i32(),
526 frame_a: r.transform(),
527 frame_b: r.transform(),
528 delta_center: r.vec3(),
529 angular_mass: r.matrix3(),
530 }),
531 8 => JointUnion::Wheel(WheelJoint {
532 linear_impulse: r.vec2(),
533 angular_impulse: r.vec2(),
534 spin_impulse: r.f32(),
535 max_spin_torque: r.f32(),
536 spin_speed: r.f32(),
537 suspension_spring_impulse: r.f32(),
538 lower_suspension_impulse: r.f32(),
539 upper_suspension_impulse: r.f32(),
540 lower_suspension_limit: r.f32(),
541 upper_suspension_limit: r.f32(),
542 suspension_hertz: r.f32(),
543 suspension_damping_ratio: r.f32(),
544 steering_spring_impulse: r.f32(),
545 lower_steering_impulse: r.f32(),
546 upper_steering_impulse: r.f32(),
547 lower_steering_limit: r.f32(),
548 upper_steering_limit: r.f32(),
549 target_steering_angle: r.f32(),
550 max_steering_torque: r.f32(),
551 steering_hertz: r.f32(),
552 steering_damping_ratio: r.f32(),
553 index_a: r.i32(),
554 index_b: r.i32(),
555 frame_a: r.transform(),
556 frame_b: r.transform(),
557 delta_center: r.vec3(),
558 spin_mass: r.f32(),
559 suspension_mass: r.f32(),
560 steering_mass: r.f32(),
561 suspension_softness: r.softness(),
562 steering_softness: r.softness(),
563 enable_spin_motor: r.bool(),
564 enable_suspension_spring: r.bool(),
565 enable_suspension_limit: r.bool(),
566 enable_steering: r.bool(),
567 enable_steering_limit: r.bool(),
568 enable_steering_motor: r.bool(),
569 }),
570 _ => JointUnion::Filter,
571 };
572 JointSim {
573 joint_id,
574 body_id_a,
575 body_id_b,
576 type_,
577 local_frame_a,
578 local_frame_b,
579 inv_mass_a,
580 inv_mass_b,
581 inv_i_a,
582 inv_i_b,
583 constraint_hertz,
584 constraint_damping_ratio,
585 constraint_softness,
586 force_threshold,
587 torque_threshold,
588 fixed_rotation,
589 union_,
590 }
591}