use freecs::Entity;
use nalgebra_glm::Vec3;
use rapier3d::prelude::*;
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum JointAxisDirection {
X,
Y,
Z,
}
impl JointAxisDirection {
pub fn to_unit_vector(&self) -> UnitVector<Real> {
match self {
JointAxisDirection::X => Vector::x_axis(),
JointAxisDirection::Y => Vector::y_axis(),
JointAxisDirection::Z => Vector::z_axis(),
}
}
}
#[derive(Debug, Clone, Copy)]
pub struct JointMotor {
pub target_velocity: f32,
pub target_position: f32,
pub stiffness: f32,
pub damping: f32,
pub max_force: f32,
}
impl Default for JointMotor {
fn default() -> Self {
Self {
target_velocity: 0.0,
target_position: 0.0,
stiffness: 0.0,
damping: 0.0,
max_force: f32::MAX,
}
}
}
impl JointMotor {
pub fn velocity(target_velocity: f32, damping: f32) -> Self {
Self {
target_velocity,
target_position: 0.0,
stiffness: 0.0,
damping,
max_force: f32::MAX,
}
}
pub fn position(target_position: f32, stiffness: f32, damping: f32) -> Self {
Self {
target_velocity: 0.0,
target_position,
stiffness,
damping,
max_force: f32::MAX,
}
}
pub fn with_max_force(mut self, max_force: f32) -> Self {
self.max_force = max_force;
self
}
}
#[derive(Debug, Clone, Copy)]
pub struct JointLimits {
pub min: f32,
pub max: f32,
}
impl JointLimits {
pub fn new(min: f32, max: f32) -> Self {
Self { min, max }
}
pub fn symmetric(value: f32) -> Self {
Self {
min: -value,
max: value,
}
}
}
pub struct FixedJoint {
pub local_anchor1: Vec3,
pub local_anchor2: Vec3,
pub local_basis1: nalgebra_glm::Quat,
pub local_basis2: nalgebra_glm::Quat,
}
impl Default for FixedJoint {
fn default() -> Self {
Self {
local_anchor1: Vec3::zeros(),
local_anchor2: Vec3::zeros(),
local_basis1: nalgebra_glm::quat_identity(),
local_basis2: nalgebra_glm::quat_identity(),
}
}
}
impl FixedJoint {
pub fn new() -> Self {
Self::default()
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.local_anchor1 = anchor;
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.local_anchor2 = anchor;
self
}
pub fn with_local_basis1(mut self, basis: nalgebra_glm::Quat) -> Self {
self.local_basis1 = basis;
self
}
pub fn with_local_basis2(mut self, basis: nalgebra_glm::Quat) -> Self {
self.local_basis2 = basis;
self
}
pub fn build(&self) -> GenericJoint {
let frame1 = rapier3d::na::Isometry3::from_parts(
rapier3d::na::Translation3::new(
self.local_anchor1.x,
self.local_anchor1.y,
self.local_anchor1.z,
),
rapier3d::na::UnitQuaternion::new_normalize(rapier3d::na::Quaternion::new(
self.local_basis1.w,
self.local_basis1.i,
self.local_basis1.j,
self.local_basis1.k,
)),
);
let frame2 = rapier3d::na::Isometry3::from_parts(
rapier3d::na::Translation3::new(
self.local_anchor2.x,
self.local_anchor2.y,
self.local_anchor2.z,
),
rapier3d::na::UnitQuaternion::new_normalize(rapier3d::na::Quaternion::new(
self.local_basis2.w,
self.local_basis2.i,
self.local_basis2.j,
self.local_basis2.k,
)),
);
FixedJointBuilder::new()
.local_frame1(frame1)
.local_frame2(frame2)
.build()
.into()
}
}
pub struct SphericalJoint {
pub local_anchor1: Vec3,
pub local_anchor2: Vec3,
}
impl Default for SphericalJoint {
fn default() -> Self {
Self {
local_anchor1: Vec3::zeros(),
local_anchor2: Vec3::zeros(),
}
}
}
impl SphericalJoint {
pub fn new() -> Self {
Self::default()
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.local_anchor1 = anchor;
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.local_anchor2 = anchor;
self
}
pub fn build(&self) -> GenericJoint {
SphericalJointBuilder::new()
.local_anchor1(point![
self.local_anchor1.x,
self.local_anchor1.y,
self.local_anchor1.z
])
.local_anchor2(point![
self.local_anchor2.x,
self.local_anchor2.y,
self.local_anchor2.z
])
.build()
.into()
}
}
pub struct RevoluteJoint {
pub local_anchor1: Vec3,
pub local_anchor2: Vec3,
pub axis: JointAxisDirection,
pub limits: Option<JointLimits>,
pub motor: Option<JointMotor>,
}
impl Default for RevoluteJoint {
fn default() -> Self {
Self {
local_anchor1: Vec3::zeros(),
local_anchor2: Vec3::zeros(),
axis: JointAxisDirection::Y,
limits: None,
motor: None,
}
}
}
impl RevoluteJoint {
pub fn new(axis: JointAxisDirection) -> Self {
Self {
axis,
..Default::default()
}
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.local_anchor1 = anchor;
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.local_anchor2 = anchor;
self
}
pub fn with_limits(mut self, limits: JointLimits) -> Self {
self.limits = Some(limits);
self
}
pub fn with_motor(mut self, motor: JointMotor) -> Self {
self.motor = Some(motor);
self
}
pub fn build(&self) -> GenericJoint {
let mut builder = RevoluteJointBuilder::new(self.axis.to_unit_vector())
.local_anchor1(point![
self.local_anchor1.x,
self.local_anchor1.y,
self.local_anchor1.z
])
.local_anchor2(point![
self.local_anchor2.x,
self.local_anchor2.y,
self.local_anchor2.z
]);
if let Some(limits) = self.limits {
builder = builder.limits([limits.min, limits.max]);
}
if let Some(motor) = self.motor {
builder = builder.motor_velocity(motor.target_velocity, motor.damping);
if motor.max_force < f32::MAX {
builder = builder.motor_max_force(motor.max_force);
}
}
builder.build().into()
}
}
pub struct PrismaticJoint {
pub local_anchor1: Vec3,
pub local_anchor2: Vec3,
pub axis: JointAxisDirection,
pub limits: Option<JointLimits>,
pub motor: Option<JointMotor>,
}
impl Default for PrismaticJoint {
fn default() -> Self {
Self {
local_anchor1: Vec3::zeros(),
local_anchor2: Vec3::zeros(),
axis: JointAxisDirection::X,
limits: None,
motor: None,
}
}
}
impl PrismaticJoint {
pub fn new(axis: JointAxisDirection) -> Self {
Self {
axis,
..Default::default()
}
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.local_anchor1 = anchor;
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.local_anchor2 = anchor;
self
}
pub fn with_limits(mut self, limits: JointLimits) -> Self {
self.limits = Some(limits);
self
}
pub fn with_motor(mut self, motor: JointMotor) -> Self {
self.motor = Some(motor);
self
}
pub fn build(&self) -> GenericJoint {
let mut builder = PrismaticJointBuilder::new(self.axis.to_unit_vector())
.local_anchor1(point![
self.local_anchor1.x,
self.local_anchor1.y,
self.local_anchor1.z
])
.local_anchor2(point![
self.local_anchor2.x,
self.local_anchor2.y,
self.local_anchor2.z
]);
if let Some(limits) = self.limits {
builder = builder.limits([limits.min, limits.max]);
}
if let Some(motor) = self.motor {
builder = builder.motor_velocity(motor.target_velocity, motor.damping);
if motor.max_force < f32::MAX {
builder = builder.motor_max_force(motor.max_force);
}
}
builder.build().into()
}
}
pub struct RopeJoint {
pub local_anchor1: Vec3,
pub local_anchor2: Vec3,
pub max_distance: f32,
}
impl Default for RopeJoint {
fn default() -> Self {
Self {
local_anchor1: Vec3::zeros(),
local_anchor2: Vec3::zeros(),
max_distance: 1.0,
}
}
}
impl RopeJoint {
pub fn new(max_distance: f32) -> Self {
Self {
max_distance,
..Default::default()
}
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.local_anchor1 = anchor;
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.local_anchor2 = anchor;
self
}
pub fn build(&self) -> GenericJoint {
RopeJointBuilder::new(self.max_distance)
.local_anchor1(point![
self.local_anchor1.x,
self.local_anchor1.y,
self.local_anchor1.z
])
.local_anchor2(point![
self.local_anchor2.x,
self.local_anchor2.y,
self.local_anchor2.z
])
.build()
.into()
}
}
pub struct SpringJoint {
pub local_anchor1: Vec3,
pub local_anchor2: Vec3,
pub rest_length: f32,
pub stiffness: f32,
pub damping: f32,
}
impl Default for SpringJoint {
fn default() -> Self {
Self {
local_anchor1: Vec3::zeros(),
local_anchor2: Vec3::zeros(),
rest_length: 1.0,
stiffness: 100.0,
damping: 1.0,
}
}
}
impl SpringJoint {
pub fn new(rest_length: f32, stiffness: f32, damping: f32) -> Self {
Self {
rest_length,
stiffness,
damping,
..Default::default()
}
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.local_anchor1 = anchor;
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.local_anchor2 = anchor;
self
}
pub fn build(&self) -> GenericJoint {
SpringJointBuilder::new(self.rest_length, self.stiffness, self.damping)
.local_anchor1(point![
self.local_anchor1.x,
self.local_anchor1.y,
self.local_anchor1.z
])
.local_anchor2(point![
self.local_anchor2.x,
self.local_anchor2.y,
self.local_anchor2.z
])
.build()
.into()
}
}
pub struct GenericJointConfig {
joint: GenericJoint,
}
impl GenericJointConfig {
pub fn new() -> Self {
Self {
joint: GenericJoint::default(),
}
}
pub fn lock_axis(mut self, axis: JointAxis) -> Self {
self.joint.set_limits(axis, [0.0, 0.0]);
self
}
pub fn free_axis(mut self, axis: JointAxis) -> Self {
self.joint.set_limits(axis, [f32::MIN, f32::MAX]);
self
}
pub fn with_local_anchor1(mut self, anchor: Vec3) -> Self {
self.joint
.set_local_anchor1(point![anchor.x, anchor.y, anchor.z]);
self
}
pub fn with_local_anchor2(mut self, anchor: Vec3) -> Self {
self.joint
.set_local_anchor2(point![anchor.x, anchor.y, anchor.z]);
self
}
pub fn with_limits(mut self, axis: JointAxis, min: f32, max: f32) -> Self {
self.joint.set_limits(axis, [min, max]);
self
}
pub fn build(self) -> GenericJoint {
self.joint
}
}
impl Default for GenericJointConfig {
fn default() -> Self {
Self::new()
}
}
pub struct JointHandle(pub ImpulseJointHandle);
impl From<ImpulseJointHandle> for JointHandle {
fn from(handle: ImpulseJointHandle) -> Self {
Self(handle)
}
}
impl From<JointHandle> for ImpulseJointHandle {
fn from(handle: JointHandle) -> Self {
handle.0
}
}
use crate::ecs::world::World;
pub fn create_fixed_joint(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: FixedJoint,
) -> Option<JointHandle> {
create_joint_internal(world, parent_entity, child_entity, joint.build(), true)
}
pub fn create_spherical_joint(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: SphericalJoint,
) -> Option<JointHandle> {
create_joint_internal(world, parent_entity, child_entity, joint.build(), true)
}
pub fn create_revolute_joint(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: RevoluteJoint,
) -> Option<JointHandle> {
create_joint_internal(world, parent_entity, child_entity, joint.build(), true)
}
pub fn create_prismatic_joint(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: PrismaticJoint,
) -> Option<JointHandle> {
create_joint_internal(world, parent_entity, child_entity, joint.build(), true)
}
pub fn create_rope_joint(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: RopeJoint,
) -> Option<JointHandle> {
create_joint_internal(world, parent_entity, child_entity, joint.build(), true)
}
pub fn create_spring_joint(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: SpringJoint,
) -> Option<JointHandle> {
create_joint_internal(world, parent_entity, child_entity, joint.build(), true)
}
fn create_joint_internal(
world: &mut World,
parent_entity: Entity,
child_entity: Entity,
joint: GenericJoint,
collisions_enabled: bool,
) -> Option<JointHandle> {
let parent_handle = world
.core
.get_rigid_body(parent_entity)
.and_then(|rb| rb.handle);
let child_handle = world
.core
.get_rigid_body(child_entity)
.and_then(|rb| rb.handle);
match (parent_handle, child_handle) {
(Some(parent_h), Some(child_h)) => {
let joint_handle = world.resources.physics.impulse_joint_set.insert(
parent_h.into(),
child_h.into(),
joint,
collisions_enabled,
);
Some(JointHandle(joint_handle))
}
_ => {
world.resources.physics.pending_joints.push(
crate::ecs::physics::resources::PendingJoint {
parent_entity,
child_entity,
joint,
joint_type: crate::ecs::physics::resources::JointType::Fixed,
collisions_enabled,
spring_rest_length: None,
spring_stiffness: None,
spring_damping: None,
},
);
None
}
}
}