diff --git a/src/.elastic_cyl.rs.kak.mr22WW b/src/.elastic_cyl.rs.kak.mr22WW deleted file mode 100644 index 2005bdf53..000000000 --- a/src/.elastic_cyl.rs.kak.mr22WW +++ /dev/null @@ -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, - pub mass: Option, - - pub h: Option, - pub r: Option, - - pub pos: Option>, - pub segments: Option, - pub up: Option, - - pub damping: Option, - pub collision_group: Option, - pub top_joint: Option<(RigidBodyHandle, Vector3)>, - pub bot_joint: Option<(RigidBodyHandle, Vector3)>, -} - -impl From 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, - pub mass: Option, - pub pos: Vector3, - pub h: f32, - pub up: bool, - pub seg_h: f32, - pub segments: u64, - pub r: f32, - pub collision_group: Option, - pub top_joint: Option<(RigidBodyHandle, Vector3)>, - pub bot_joint: Option<(RigidBodyHandle, Vector3)>, -} - -#[derive(Debug, Clone)] -pub struct Handle(pub Config, VecDeque); - -impl Handle { - pub fn segs(&self) -> impl Iterator { - 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, -) -> 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 = { - 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::>() - }; - - 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)>, - top_joint: Option<(RigidBodyHandle, Vector3)>, - } - - 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 -}