1use crate::constant::{
2 CONSTRAINT_ACCUMULATOR_SLOTS, CONSTRAINT_BALL, CONSTRAINT_CONE, CONSTRAINT_DISABLE_COLLISIONS,
3 CONSTRAINT_DISTANCE, CONSTRAINT_FIXED, CONSTRAINT_GEAR, CONSTRAINT_HAS_BREAK,
4 CONSTRAINT_HAS_LIMIT, CONSTRAINT_HAS_MOTOR, CONSTRAINT_HAS_SWING, CONSTRAINT_IS_SPRING,
5 CONSTRAINT_PRISMATIC, CONSTRAINT_PULLEY, CONSTRAINT_REVOLUTE, CONSTRAINT_SIXDOF,
6 CONSTRAINT_WARM_START, set_dof_driven, set_dof_limited, set_dof_locked,
7};
8use crate::{ConstraintDescriptorRecord, ConstraintRuntimeRecord};
9use dynamis_model::{ConstraintDesc, ConstraintMotor, DofDesc};
10
11impl ConstraintDescriptorRecord {
12 pub fn build(desc: &ConstraintDesc, first_body_id: u32, second_body_id: u32) -> Self {
13 let kind = match desc.kind {
14 dynamis_model::ConstraintKind::Ball => CONSTRAINT_BALL,
15 dynamis_model::ConstraintKind::Distance => CONSTRAINT_DISTANCE,
16 dynamis_model::ConstraintKind::Revolute => CONSTRAINT_REVOLUTE,
17 dynamis_model::ConstraintKind::Prismatic => CONSTRAINT_PRISMATIC,
18 dynamis_model::ConstraintKind::Fixed => CONSTRAINT_FIXED,
19 dynamis_model::ConstraintKind::Gear => CONSTRAINT_GEAR,
20 dynamis_model::ConstraintKind::Pulley => CONSTRAINT_PULLEY,
21 dynamis_model::ConstraintKind::Cone => CONSTRAINT_CONE,
22 dynamis_model::ConstraintKind::SixDof => CONSTRAINT_SIXDOF,
23 };
24 let mut flags = 0;
25 if desc.disable_collisions {
26 flags |= CONSTRAINT_DISABLE_COLLISIONS;
27 }
28 if desc.limit.is_some() {
29 flags |= CONSTRAINT_HAS_LIMIT;
30 }
31 if desc.swing.is_some() {
32 flags |= CONSTRAINT_HAS_SWING;
33 }
34 if desc.motor.is_some() {
35 flags |= CONSTRAINT_HAS_MOTOR;
36 }
37 if desc.spring.is_some() {
38 flags |= CONSTRAINT_IS_SPRING;
39 }
40 if desc.break_threshold.is_some() {
41 flags |= CONSTRAINT_HAS_BREAK;
42 }
43 if desc.warm_start {
44 flags |= CONSTRAINT_WARM_START;
45 }
46 let dofs = desc.dofs.unwrap_or([DofDesc::free(); 6]);
47 for (index, dof) in dofs.iter().enumerate() {
48 let index = index as u32;
49 flags = set_dof_locked(flags, index, dof.locked);
50 flags = set_dof_limited(flags, index, dof.limit.is_some());
51 flags = set_dof_driven(flags, index, dof.motor.is_some());
52 }
53 let motor = |motor: &Option<ConstraintMotor>| {
54 (
55 motor.map_or(0.0, |m| m.target_velocity),
56 motor.map_or(0.0, |m| m.max_force),
57 motor.map_or(0.0, |m| m.target_position.unwrap_or(0.0)),
58 motor.map_or(0.0, |m| m.stiffness),
59 motor.map_or(0.0, |m| m.damping),
60 )
61 };
62 let (motor_speed, motor_max_force, motor_target, motor_stiffness, motor_damping) =
63 motor(&desc.motor);
64 let generic_motor = |dofs: &[DofDesc; 6]| {
65 let mut target = [0.0f32; 3];
66 let mut stiffness = [0.0f32; 3];
67 let mut damping = [0.0f32; 3];
68 let mut force = [0.0f32; 3];
69 let mut angular_target = [0.0f32; 3];
70 let mut angular_stiffness = [0.0f32; 3];
71 let mut angular_damping = [0.0f32; 3];
72 let mut angular_force = [0.0f32; 3];
73 for (index, dof) in dofs.iter().enumerate() {
74 let Some(motor) = dof.motor else { continue };
75 if index < 3 {
76 target[index] = motor.target_position.unwrap_or(motor.target_velocity);
77 stiffness[index] = motor.stiffness;
78 damping[index] = motor.damping;
79 force[index] = motor.max_force;
80 } else {
81 angular_target[index - 3] =
82 motor.target_position.unwrap_or(motor.target_velocity);
83 angular_stiffness[index - 3] = motor.stiffness;
84 angular_damping[index - 3] = motor.damping;
85 angular_force[index - 3] = motor.max_force;
86 }
87 }
88 (
89 target,
90 stiffness,
91 damping,
92 force,
93 angular_target,
94 angular_stiffness,
95 angular_damping,
96 angular_force,
97 )
98 };
99 let (
100 linear_motor_target,
101 linear_motor_stiffness,
102 linear_motor_damping,
103 linear_motor_force,
104 angular_motor_target,
105 angular_motor_stiffness,
106 angular_motor_damping,
107 angular_motor_force,
108 ) = generic_motor(&dofs);
109 let mut linear_limits: [[f32; 3]; 2] = [[0.0; 3]; 2];
110 let mut angular_limits: [[f32; 3]; 2] = [[0.0; 3]; 2];
111 for (index, dof) in dofs.iter().enumerate() {
112 let Some(limit) = dof.limit else { continue };
113 if index < 3 {
114 linear_limits[0][index] = limit.min;
115 linear_limits[1][index] = limit.max;
116 } else {
117 angular_limits[0][index - 3] = limit.min;
118 angular_limits[1][index - 3] = limit.max;
119 }
120 }
121 Self {
122 kind,
123 first_body_id,
124 second_body_id,
125 flags,
126 anchor_a: desc.anchor_a,
127 _pad1: 0.0,
128 anchor_b: desc.anchor_b,
129 _pad2: 0.0,
130 axis_a: desc.axis_a,
131 _pad3: 0.0,
132 axis_b: desc.axis_b,
133 _pad4: 0.0,
134 distance: desc.rest_length,
135 limit_min: desc.limit.map_or(0.0, |limit| limit.min),
136 limit_max: desc.limit.map_or(0.0, |limit| limit.max),
137 swing_a: desc
138 .swing
139 .map_or(std::f32::consts::PI, |swing| swing.swing_a),
140 swing_b: desc
141 .swing
142 .map_or(std::f32::consts::PI, |swing| swing.swing_b),
143 motor_speed,
144 motor_max_force,
145 spring_frequency: desc.spring.map_or(0.0, |spring| spring.frequency),
146 spring_damping_ratio: desc.spring.map_or(0.0, |spring| spring.damping_ratio),
147 break_force: desc
148 .break_threshold
149 .map_or(0.0, |threshold| threshold.force),
150 break_torque: desc
151 .break_threshold
152 .map_or(0.0, |threshold| threshold.torque),
153 gear_ratio: desc.gear_ratio,
154 pulley_fixed_a: desc.pulley_fixed_a,
155 _pad_pulley_a: 0.0,
156 pulley_fixed_b: desc.pulley_fixed_b,
157 _pad_pulley_b: 0.0,
158 motor_target,
159 motor_stiffness,
160 motor_damping,
161 cone_angle: desc.cone_angle,
162 reference: desc.reference,
163 linear_limit_min: linear_limits[0],
164 _pad_lim_min: 0.0,
165 linear_limit_max: linear_limits[1],
166 _pad_lim_max: 0.0,
167 angular_limit_min: angular_limits[0],
168 _pad_ang_min: 0.0,
169 angular_limit_max: angular_limits[1],
170 _pad_ang_max: 0.0,
171 linear_motor_target,
172 _pad_lin_target: 0.0,
173 linear_motor_stiffness,
174 _pad_lin_stiff: 0.0,
175 linear_motor_damping,
176 _pad_lin_damp: 0.0,
177 angular_motor_target,
178 _pad_ang_target: 0.0,
179 angular_motor_stiffness,
180 _pad_ang_stiff: 0.0,
181 angular_motor_damping,
182 _pad_ang_damp: 0.0,
183 linear_motor_force,
184 _pad_lin_force: 0.0,
185 angular_motor_force,
186 _pad_ang_force: 0.0,
187 }
188 }
189}
190
191impl ConstraintDescriptorRecord {
192 pub fn constraint_kind(&self) -> dynamis_model::ConstraintKind {
193 match self.kind {
194 CONSTRAINT_BALL => dynamis_model::ConstraintKind::Ball,
195 CONSTRAINT_DISTANCE => dynamis_model::ConstraintKind::Distance,
196 CONSTRAINT_REVOLUTE => dynamis_model::ConstraintKind::Revolute,
197 CONSTRAINT_PRISMATIC => dynamis_model::ConstraintKind::Prismatic,
198 CONSTRAINT_FIXED => dynamis_model::ConstraintKind::Fixed,
199 CONSTRAINT_GEAR => dynamis_model::ConstraintKind::Gear,
200 CONSTRAINT_PULLEY => dynamis_model::ConstraintKind::Pulley,
201 CONSTRAINT_CONE => dynamis_model::ConstraintKind::Cone,
202 CONSTRAINT_SIXDOF => dynamis_model::ConstraintKind::SixDof,
203 other => panic!("constraint descriptor carries an unknown kind {other}"),
204 }
205 }
206}
207
208impl ConstraintRuntimeRecord {
209 pub fn fresh(constraint_id: u32, generation: u32) -> Self {
210 Self {
211 reaction: bytemuck::Zeroable::zeroed(),
212 accumulated: [0.0; CONSTRAINT_ACCUMULATOR_SLOTS as usize],
213 broken: 0,
214 constraint_id,
215 generation,
216 _pad0: 0,
217 }
218 }
219}