Skip to content

Commit 99d2dd6

Browse files
committed
format after change
1 parent 5554ba4 commit 99d2dd6

File tree

6 files changed

+60
-20
lines changed

6 files changed

+60
-20
lines changed

examples2d/joints2.rs

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,15 +44,21 @@ pub fn init_world(testbed: &mut Testbed) {
4444
// Vertical joint.
4545
if i > 0 {
4646
let parent_handle = *body_handles.last().unwrap();
47-
let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]).natural_frequency(5.0 * (i+1) as f32).damping_ratio(0.1 * (i+1) as f32);
47+
let joint = RevoluteJointBuilder::new()
48+
.local_anchor2(point![0.0, shift])
49+
.natural_frequency(5.0 * (i + 1) as f32)
50+
.damping_ratio(0.1 * (i + 1) as f32);
4851
impulse_joints.insert(parent_handle, child_handle, joint, true);
4952
}
5053

5154
// Horizontal joint.
5255
if k > 0 {
5356
let parent_index = body_handles.len() - numi;
5457
let parent_handle = body_handles[parent_index];
55-
let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]).natural_frequency(5.0 * (i+1) as f32).damping_ratio(0.1 * (i+1) as f32);
58+
let joint = RevoluteJointBuilder::new()
59+
.local_anchor2(point![-shift, 0.0])
60+
.natural_frequency(5.0 * (i + 1) as f32)
61+
.damping_ratio(0.1 * (i + 1) as f32);
5662
impulse_joints.insert(parent_handle, child_handle, joint, true);
5763
}
5864

examples2d/pin_slot_joint2.rs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1-
use rapier2d::prelude::*;
21
use rapier_testbed2d::Testbed;
2+
use rapier2d::prelude::*;
33

44
pub fn init_world(testbed: &mut Testbed) {
55
/*

src/dynamics/integration_parameters.rs

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -199,7 +199,6 @@ impl IntegrationParameters {
199199
pub fn contact_erp(&self) -> Real {
200200
self.dt * self.contact_erp_inv_dt()
201201
}
202-
203202

204203
/// The CFM factor to be used in the constraint resolution.
205204
///
@@ -249,7 +248,6 @@ impl IntegrationParameters {
249248
///
250249
/// This parameter is computed automatically from [`Self::joint_natural_frequency`],
251250
/// [`Self::joint_damping_ratio`] and the substep length.
252-
253251
254252
/// The joint's spring angular frequency for constraint regularization, using per-joint parameter.
255253
pub fn joint_angular_frequency_with_override(&self, natural_frequency: Real) -> Real {
@@ -278,7 +276,8 @@ impl IntegrationParameters {
278276
return 0.0;
279277
}
280278
let inv_erp_minus_one = 1.0 / joint_erp - 1.0;
281-
inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) * 4.0 * damping_ratio * damping_ratio)
279+
inv_erp_minus_one * inv_erp_minus_one
280+
/ ((1.0 + inv_erp_minus_one) * 4.0 * damping_ratio * damping_ratio)
282281
}
283282

284283
/// Amount of penetration the engine won't attempt to correct (default: `0.001` multiplied by

src/dynamics/joint/multibody_joint/unit_multibody_joint.rs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,11 +25,11 @@ pub fn unit_joint_limit_constraint(
2525
let ndofs = multibody.ndofs();
2626
let min_enabled = curr_pos < limits[0];
2727
let max_enabled = limits[1] < curr_pos;
28-
28+
2929
// Compute per-joint ERP and CFM
3030
let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio);
3131
let cfm_coeff = params.joint_cfm_coeff_with_override(natural_frequency, damping_ratio);
32-
32+
3333
let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
3434
let rhs_wo_bias = 0.0;
3535

src/dynamics/solver/joint_constraint/joint_constraint_builder.rs

Lines changed: 20 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -287,8 +287,16 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
287287
cfm_coeff: N,
288288
) -> JointConstraint<N, LANES> {
289289
let zero = N::zero();
290-
let mut constraint =
291-
self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id, erp_inv_dt, cfm_coeff);
290+
let mut constraint = self.lock_linear(
291+
params,
292+
joint_id,
293+
body1,
294+
body2,
295+
limited_axis,
296+
writeback_id,
297+
erp_inv_dt,
298+
cfm_coeff,
299+
);
292300

293301
let dist = self.lin_err.dot(&constraint.lin_jac);
294302
let min_enabled = dist.simd_le(limits[0]);
@@ -393,8 +401,16 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
393401
let inv_dt = N::splat(params.inv_dt());
394402
// Motors don't use joint erp/cfm; they use motor-specific parameters.
395403
// Pass dummy values since lock_linear will override them anyway.
396-
let mut constraint =
397-
self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id, N::zero(), N::zero());
404+
let mut constraint = self.lock_linear(
405+
params,
406+
joint_id,
407+
body1,
408+
body2,
409+
motor_axis,
410+
writeback_id,
411+
N::zero(),
412+
N::zero(),
413+
);
398414

399415
let mut rhs_wo_bias = N::zero();
400416
if motor_params.erp_inv_dt != N::zero() {

src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,10 @@ use crate::utils::{SimdDot, SimdRealCopy};
99
use crate::dynamics::solver::solver_body::SolverBodies;
1010
#[cfg(feature = "simd-is-enabled")]
1111
use crate::math::{SIMD_WIDTH, SimdReal};
12-
#[cfg(feature = "simd-is-enabled")]
13-
use na::SimdValue;
1412
#[cfg(feature = "dim2")]
1513
use crate::num::Zero;
14+
#[cfg(feature = "simd-is-enabled")]
15+
use na::SimdValue;
1616

1717
#[derive(Copy, Clone, PartialEq, Debug)]
1818
pub struct MotorParameters<N: SimdRealCopy> {
@@ -250,8 +250,16 @@ impl JointConstraint<Real, 1> {
250250
}
251251
for i in 0..DIM {
252252
if locked_axes & (1 << i) != 0 {
253-
out[len] =
254-
builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i), erp_inv_dt, cfm_coeff);
253+
out[len] = builder.lock_linear(
254+
params,
255+
[joint_id],
256+
body1,
257+
body2,
258+
i,
259+
WritebackId::Dof(i),
260+
erp_inv_dt,
261+
cfm_coeff,
262+
);
255263
len += 1;
256264
}
257265
}
@@ -369,11 +377,14 @@ impl JointConstraint<SimdReal, SIMD_WIDTH> {
369377
let ang_freq = natural_frequency * SimdReal::splat(std::f64::consts::TAU as Real);
370378
let dt = SimdReal::splat(params.dt);
371379
let erp_inv_dt = ang_freq / (dt * ang_freq + SimdReal::splat(2.0) * damping_ratio);
372-
380+
373381
let joint_erp = dt * erp_inv_dt;
374382
let inv_erp_minus_one = SimdReal::splat(1.0) / joint_erp - SimdReal::splat(1.0);
375383
let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
376-
/ ((SimdReal::splat(1.0) + inv_erp_minus_one) * SimdReal::splat(4.0) * damping_ratio * damping_ratio);
384+
/ ((SimdReal::splat(1.0) + inv_erp_minus_one)
385+
* SimdReal::splat(4.0)
386+
* damping_ratio
387+
* damping_ratio);
377388

378389
let builder = JointConstraintHelper::new(
379390
frame1,
@@ -386,8 +397,16 @@ impl JointConstraint<SimdReal, SIMD_WIDTH> {
386397
let mut len = 0;
387398
for i in 0..DIM {
388399
if locked_axes & (1 << i) != 0 {
389-
out[len] =
390-
builder.lock_linear(params, joint_id, body1, body2, i, WritebackId::Dof(i), erp_inv_dt, cfm_coeff);
400+
out[len] = builder.lock_linear(
401+
params,
402+
joint_id,
403+
body1,
404+
body2,
405+
i,
406+
WritebackId::Dof(i),
407+
erp_inv_dt,
408+
cfm_coeff,
409+
);
391410
len += 1;
392411
}
393412
}

0 commit comments

Comments
 (0)