fix: kak tmpfiles
This commit is contained in:
Orion Kindel
@@ -1,298 +0,0 @@
|
||||
use nalgebra::Vector3;
|
||||
use rapier3d::prelude::*;
|
||||
use std::collections::VecDeque;
|
||||
use std::f32;
|
||||
|
||||
#[derive(Default, Debug, Clone, Copy)]
|
||||
pub struct ConfigBuilder {
|
||||
pub young: f32,
|
||||
pub poisson: f32,
|
||||
pub density: Option<f32>,
|
||||
pub mass: Option<f32>,
|
||||
|
||||
pub h: Option<f32>,
|
||||
pub r: Option<f32>,
|
||||
|
||||
pub pos: Option<Vector3<f32>>,
|
||||
pub segments: Option<u64>,
|
||||
pub up: Option<bool>,
|
||||
|
||||
pub damping: Option<f32>,
|
||||
pub collision_group: Option<InteractionGroups>,
|
||||
pub top_joint: Option<(RigidBodyHandle, Vector3<f32>)>,
|
||||
pub bot_joint: Option<(RigidBodyHandle, Vector3<f32>)>,
|
||||
}
|
||||
|
||||
impl From<ConfigBuilder> for Config {
|
||||
fn from(b: ConfigBuilder) -> Config {
|
||||
let (
|
||||
damping,
|
||||
density,
|
||||
mass,
|
||||
h,
|
||||
segments,
|
||||
pos,
|
||||
r,
|
||||
up,
|
||||
top_joint,
|
||||
bot_joint,
|
||||
collision_group,
|
||||
) = (
|
||||
b.damping.unwrap_or(0.0),
|
||||
b.density,
|
||||
b.mass,
|
||||
b.h.unwrap_or(1.0),
|
||||
b.segments.unwrap_or(10),
|
||||
b.pos.unwrap_or(Vector::zeros()),
|
||||
b.r.unwrap_or(0.5),
|
||||
b.up.unwrap_or(true),
|
||||
b.top_joint,
|
||||
b.bot_joint,
|
||||
b.collision_group,
|
||||
);
|
||||
|
||||
let seg_h = h / (segments as f32);
|
||||
let k_rot = cyl_beam_spring_rot(r, seg_h, b.young);
|
||||
|
||||
Config {
|
||||
up,
|
||||
k_rot,
|
||||
damping,
|
||||
density,
|
||||
mass,
|
||||
h,
|
||||
seg_h,
|
||||
segments,
|
||||
pos,
|
||||
r,
|
||||
top_joint,
|
||||
bot_joint,
|
||||
collision_group,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Default, Debug, Clone, Copy)]
|
||||
pub struct Config {
|
||||
pub k_rot: f32,
|
||||
pub damping: f32,
|
||||
pub density: Option<f32>,
|
||||
pub mass: Option<f32>,
|
||||
pub pos: Vector3<f32>,
|
||||
pub h: f32,
|
||||
pub up: bool,
|
||||
pub seg_h: f32,
|
||||
pub segments: u64,
|
||||
pub r: f32,
|
||||
pub collision_group: Option<InteractionGroups>,
|
||||
pub top_joint: Option<(RigidBodyHandle, Vector3<f32>)>,
|
||||
pub bot_joint: Option<(RigidBodyHandle, Vector3<f32>)>,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct Handle(pub Config, VecDeque<RigidBodyHandle>);
|
||||
|
||||
impl Handle {
|
||||
pub fn segs(&self) -> impl Iterator<Item = &RigidBodyHandle> {
|
||||
self.1.iter()
|
||||
}
|
||||
|
||||
pub fn top(&self) -> RigidBodyHandle {
|
||||
*self.1.back().unwrap()
|
||||
}
|
||||
|
||||
pub fn bot(&self) -> RigidBodyHandle {
|
||||
*self.1.front().unwrap()
|
||||
}
|
||||
}
|
||||
|
||||
pub fn spawn(
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
impulse_joints: &mut ImpulseJointSet,
|
||||
multibodies: &mut MultibodyJointSet,
|
||||
cfg: impl Into<Config>,
|
||||
) -> Handle {
|
||||
let cfg = cfg.into();
|
||||
|
||||
let start = cfg.pos;
|
||||
let end = cfg.pos - vector![0.0, cfg.h, 0.0];
|
||||
let y = vector![0.0, cfg.seg_h, 0.0];
|
||||
let y_half = y / 2.0;
|
||||
|
||||
let mut coll = ColliderBuilder::cylinder(cfg.seg_h / 2.0, cfg.r);
|
||||
if let Some(coll_group) = cfg.collision_group {
|
||||
coll = coll.collision_groups(coll_group);
|
||||
};
|
||||
|
||||
match (cfg.density, cfg.mass) {
|
||||
(Some(density), _) => coll = coll.density(density),
|
||||
(_, Some(mass)) => coll = coll.mass(mass / cfg.segments as f32),
|
||||
_ => (),
|
||||
};
|
||||
|
||||
let mut pos = if cfg.up { end + y_half } else { start - y_half };
|
||||
let pos_delta = if cfg.up { y } else { -y };
|
||||
|
||||
let jnts: Vec<GenericJoint> = {
|
||||
let b = Spheri_rot_rot_rot_rotointBuilder::new()
|
||||
.contacts_enabled(false)
|
||||
.motor_position(JointAxis::AngZ, 0.0, cfg.k_rot, cfg.damping)
|
||||
.motor_position(JointAxis::AngX, 0.0, cfg.k_rot, cfg.damping);
|
||||
|
||||
/*
|
||||
[
|
||||
0.0,
|
||||
1.0 / 2.0 * f32::consts::PI,
|
||||
f32::consts::PI,
|
||||
3.0 / 2.0 * f32::consts::PI,
|
||||
]
|
||||
.into_iter()
|
||||
.map(|t| (t.cos() * cfg.r, t.sin() * cfg.r))
|
||||
*/
|
||||
core::iter::once((0.0, 0.0))
|
||||
.map(|(x, z)| {
|
||||
if cfg.up {
|
||||
((x, y_half.y, z), (x, -y_half.y, z))
|
||||
} else {
|
||||
((x, -y_half.y, z), (x, y_half.y, z))
|
||||
}
|
||||
})
|
||||
.map(|((ax, ay, az), (bx, by, bz))| {
|
||||
b.clone()
|
||||
.local_anchor1(Point::from(vector![ax, ay, az]))
|
||||
.local_anchor2(Point::from(vector![bx, by, bz]))
|
||||
.build()
|
||||
.into()
|
||||
})
|
||||
.collect::<Vec<_>>()
|
||||
};
|
||||
|
||||
let mut ents = VecDeque::new();
|
||||
let mut prev = None;
|
||||
for n in 0..cfg.segments {
|
||||
let rb = bodies.insert(RigidBodyBuilder::dynamic().translation(pos).angular_damping(cfg.damping).linear_damping(cfg.damping) );
|
||||
colliders.insert_with_parent(coll.clone(), rb, bodies);
|
||||
if cfg.up {
|
||||
ents.push_back(rb)
|
||||
} else {
|
||||
ents.push_front(rb)
|
||||
};
|
||||
|
||||
if let Some(prev) = prev {
|
||||
jnts.iter().cloned().for_each(|jnt| {
|
||||
multibodies.insert(prev, rb, jnt.clone(), true);
|
||||
});
|
||||
}
|
||||
|
||||
struct _A {
|
||||
first: bool,
|
||||
last: bool,
|
||||
up: bool,
|
||||
bot_joint: Option<(RigidBodyHandle, Vector3<Real>)>,
|
||||
top_joint: Option<(RigidBodyHandle, Vector3<Real>)>,
|
||||
}
|
||||
|
||||
let _a = _A {
|
||||
first: n == 0,
|
||||
last: n == cfg.segments - 1,
|
||||
up: cfg.up,
|
||||
bot_joint: cfg.bot_joint,
|
||||
top_joint: cfg.top_joint,
|
||||
};
|
||||
|
||||
let jnt = |a, b| {
|
||||
GenericJoint::from(
|
||||
FixedJointBuilder::new()
|
||||
.local_anchor1(Point::from(a))
|
||||
.local_anchor2(Point::from(b))
|
||||
.contacts_enabled(false)
|
||||
.build(),
|
||||
)
|
||||
};
|
||||
|
||||
match _a {
|
||||
_A {
|
||||
first: true,
|
||||
up: true,
|
||||
bot_joint: Some((b, pos)),
|
||||
..
|
||||
} => {
|
||||
multibodies.insert(b, rb, jnt(pos, -y_half), true);
|
||||
}
|
||||
_A {
|
||||
first: true,
|
||||
up: false,
|
||||
top_joint: Some((b, pos)),
|
||||
..
|
||||
} => {
|
||||
multibodies.insert(b, rb, jnt(pos, y_half), true);
|
||||
}
|
||||
_A {
|
||||
last: true,
|
||||
up: true,
|
||||
top_joint: Some((b, pos)),
|
||||
..
|
||||
} => {
|
||||
multibodies.insert(rb, b, jnt(y_half, pos), true);
|
||||
}
|
||||
_A {
|
||||
last: true,
|
||||
up: false,
|
||||
bot_joint: Some((b, pos)),
|
||||
..
|
||||
} => {
|
||||
multibodies.insert(rb, b, jnt(-y_half, pos), true);
|
||||
}
|
||||
_ => (),
|
||||
}
|
||||
|
||||
prev = Some(rb);
|
||||
pos += pos_delta;
|
||||
}
|
||||
|
||||
Handle(cfg, ents)
|
||||
}
|
||||
|
||||
/// linear stiffness
|
||||
fn cyl_beam_spring_rot(r: f32, length: f32, young: f32) -> f32 {
|
||||
// Bending Stiffness
|
||||
// The bending stiffness (EI) of a beam (slice) is given by:
|
||||
// EI = E * (π * r^4) / 4
|
||||
// Rotational Spring Constant
|
||||
// Using the bending stiffness, the rotational spring constant (k_rot) about the axes perpendicular to the long axis can be approximated as:
|
||||
// k_rot ≈ EI / L
|
||||
// Substituting EI:
|
||||
// k_rot ≈ (E * (π * r^4) / 4) / L
|
||||
// k_rot ≈ (E * π * r^4) / (4 * L)
|
||||
let i: f32 = f32::consts::PI * r.powi(4);
|
||||
(young * i) / (4.0 * length)
|
||||
}
|
||||
|
||||
/// shear stiffness
|
||||
fn cyl_beam_spring_xz(r: f32, length: f32, young: f32, poisson: f32) -> f32 {
|
||||
// Given:
|
||||
// poisson ratio, v
|
||||
// young's modulus, E
|
||||
// shear modulus, G
|
||||
//
|
||||
// G = E / (2 * (1 + v))
|
||||
//
|
||||
// Given material of length L,
|
||||
// cross-sectional area A,
|
||||
// force F required to deform material
|
||||
// by dL orthogonal to L axis is:
|
||||
//
|
||||
// G = (F / A) / (dL / L)
|
||||
// G = (F * L) / (A * dL)
|
||||
// G = (F * L) / (A * dL)
|
||||
// F = (GA / L) * dL
|
||||
//
|
||||
// Given Hooke's Law of form F = kx, let x = dL
|
||||
// k = GA / L
|
||||
|
||||
// shear modulus
|
||||
let g: f32 = young / (2.0 * (1.0 + poisson));
|
||||
let a: f32 = f32::consts::PI * r.powi(2);
|
||||
(g * a) / length
|
||||
}
|
||||
Reference in New Issue
Block a user