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