use std::error::Error; use std::f64::consts::PI; use log::debug; use nalgebra::{point, vector, Isometry2, Unit, Vector2, Vector}; use protobuf::SpecialFields; use rapier2d_f64::prelude::{ ColliderBuilder, FixedJointBuilder, ImpulseJointHandle, MassProperties, Real, RigidBodyBuilder, RigidBodyHandle, RigidBodySet, SharedShape, PrismaticJointBuilder, }; use starkingdoms_protocol::module::ModuleType; use crate::{ entity::{get_entity_id, Entity, EntityHandler, EntityId}, manager::PhysicsData, SCALE, }; #[derive(Debug, Clone)] pub struct Module { pub handle: RigidBodyHandle, pub module_type: ModuleType, pub lifetime: f64, pub flags: u32, pub children: Vec, } impl Module { pub fn search_modules(&self, entities: &EntityHandler) -> Vec { let mut modules = vec![self.clone()]; for attachment in self.children.iter() { if let Some(Entity::Module(child_module)) = entities.entities.get(&attachment.child) { modules.append(&mut child_module.search_modules(entities)); } } modules } } #[derive(Clone)] pub struct ModuleTemplate { pub mass_properties: MassProperties, pub module_type: ModuleType, } #[derive(Debug, Clone, PartialEq, Eq)] pub struct AttachedModule { pub handle: RigidBodyHandle, pub module_type: ModuleType, pub player_id: EntityId, pub children: [Option; 4], } impl AttachedModule { pub fn attach( data: &mut PhysicsData, entities: &mut EntityHandler, parent: EntityId, player_id: EntityId, module: &Module, attachment_slot: usize, ) -> Result> { let mut entity_map = entities.entities.clone(); let parent_entity = entity_map .get_mut(&parent) .ok_or("parent id does not exist")?; let parent_handle = match parent_entity { Entity::Player(player) => { if player.children[attachment_slot].is_some() { return Err("already attached".into()); } player.handle } Entity::AttachedModule(module) => { if module.children[attachment_slot].is_some() { return Err("already attached".into()); } module.handle } _ => { panic!("unexpected parent"); } }; let parent_body = data .rigid_body_set .get(parent_handle) .ok_or("parent body does not exist")?; let parent_pos = vector![parent_body.translation().x, parent_body.translation().y]; let parent_angle = parent_body.rotation().angle(); let parent_linvel = *parent_body.linvel(); let parent_angvel = parent_body.angvel(); let (anchor, rotation) = match attachment_slot { 0 => (point![0. / SCALE, 53. / SCALE], PI), 1 => (point![-53. / SCALE, 0. / SCALE], -PI / 2.), 2 => (point![0. / SCALE, -53. / SCALE], 0.), 3 => (point![53. / SCALE, 0. / SCALE], PI / 2.), _ => (point![0. / SCALE, 53. / SCALE], 0.), }; if let Some(id) = entities.get_from_module(module) { let relative_pos = vector![ anchor.x.mul_add( (parent_body.rotation().angle()).cos(), anchor.y * -(parent_body.rotation().angle()).sin() ), anchor.x.mul_add( (parent_body.rotation().angle()).sin(), anchor.y * (parent_body.rotation().angle()).cos() ) ]; let module_pos = parent_pos + relative_pos; let module_body = data .rigid_body_set .get_mut(module.handle) .ok_or("module body does not exist")?; module_body.set_translation(module_pos, true); module_body.set_rotation(Unit::from_angle(parent_angle + rotation), true); module_body.set_linvel(parent_linvel, true); module_body.set_angvel(parent_angvel, true); let attach_joint = FixedJointBuilder::new() .local_anchor1(anchor) .local_anchor2(point![0.0, 0.0 / SCALE]) .local_frame2(Isometry2::rotation(rotation)) .build(); let attach_joint_handle = data.impulse_joint_set .insert(parent_handle, module.handle, attach_joint, true); let attached_id = get_entity_id(); match parent_entity { Entity::Player(ref mut player) => { if module.module_type == ModuleType::LandingThrusterSuspension { player.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: true, }); } else { player.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: false, }); } } Entity::AttachedModule(ref mut module) => { if module.module_type == ModuleType::LandingThrusterSuspension { module.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: true, }); } else { module.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: false, }); } } _ => { panic!("unexpected parent"); } }; let mut children: [Option; 4] = [None, None, None, None]; for child in module.children.clone() { let child_body = entities.get_module_from_id(child.child) .ok_or("suspension doesn't exist")?; let child_body_body = data.rigid_body_set .get_mut(child_body.handle) .ok_or("module body does not exist")?; if child_body.module_type == ModuleType::LandingThrusterSuspension { child_body_body.set_translation(module_pos, true); child_body_body.set_rotation(Unit::from_angle(parent_angle + rotation), true); let id = get_entity_id(); entity_map.remove(&child.child); let child = Attachment { child: id, connection: child.connection, can_detach: child.can_detach, }; children[2] = Some(child.clone()); let attached_child = AttachedModule { handle: child_body.handle, module_type: child_body.module_type, player_id, children: [None, None, None, None], }; entity_map.insert(id, Entity::AttachedModule(attached_child)); } } let attached_module = Self { handle: module.handle, module_type: module.module_type, player_id, children, }; entity_map.remove(&id); entity_map.insert(attached_id, Entity::AttachedModule(attached_module)); entities.entities = entity_map; return Ok(attached_id); } Err("entity does not exist".into()) } pub fn detach( data: &mut PhysicsData, entities: &mut EntityHandler, player_id: EntityId, module: &Self, ) -> Option { let mut entity_map = entities.entities.clone(); let new_module; if module.module_type == ModuleType::LandingThrusterSuspension { new_module = entities.get_attached_from_id(module.find_parent(module, entities)?.0 as u32)?; } else { new_module = module.clone(); } let module = new_module; // player not in parent search // also no parents included in parent search let player = entities.get_player_from_id(player_id)?; let (slot, parent_id) = player.find_parent(&module, entities)?; let parent_entity = entity_map.get_mut(&parent_id)?; match parent_entity { Entity::Player(ref mut player) => { if let Some(child) = player.children[slot as usize].clone() { data.impulse_joint_set.remove(child.connection, true); } player.children[slot as usize] = None; } Entity::AttachedModule(ref mut module) => { if let Some(child) = &module.children[slot as usize] { data.impulse_joint_set.remove(child.connection, true); } module.children[slot as usize] = None; } _ => { panic!("unexpected parent"); } }; // remove joint let tree = module.search_modules(entities); let id = get_entity_id(); if module.module_type == ModuleType::LandingThruster { let suspension = (module.children.get(2)?.clone())?; let suspension_body = entities.get_attached_from_id(suspension.child)?; let suspension_module = Module { handle: suspension_body.handle, module_type: suspension_body.module_type, lifetime: 0., flags: 0, children: Vec::new(), }; let suspension_id = get_entity_id(); let new_module = Module { handle: module.handle, module_type: module.module_type, lifetime: 0., flags: 0, children: vec![Attachment { child: suspension_id, connection: suspension.connection, can_detach: false, }], }; entity_map.remove(&entities.get_id_from_attached(&module)?); entity_map.insert(id, Entity::Module(new_module)); entity_map.remove(&suspension.child); entity_map.insert(suspension_id, Entity::Module(suspension_module)); } else { let new_module = Module { handle: module.handle, module_type: module.module_type, lifetime: 0., flags: 0, children: Vec::new(), }; entity_map.remove(&entities.get_id_from_attached(&module)?); entity_map.insert(id, Entity::Module(new_module)); } for element in tree { for child in element.clone().children.into_iter().flatten() { let child_body = entities.get_attached_from_id(child.child)?; if child_body.module_type == ModuleType::LandingThrusterSuspension { continue; } data.impulse_joint_set.remove(child.connection, true); if child_body.module_type == ModuleType::LandingThruster { let suspension = (child_body.children.get(2)?.clone())?; let suspension_body = entities.get_attached_from_id(suspension.child)?; let suspension_module = Module { handle: suspension_body.handle, module_type: suspension_body.module_type, lifetime: 0., flags: 0, children: Vec::new(), }; let attached_id = get_entity_id(); let suspension_id = get_entity_id(); let new_module = Module { handle: child_body.handle, module_type: child_body.module_type, lifetime: 0., flags: 0, children: vec![Attachment { child: suspension_id, connection: suspension.connection, can_detach: false, }], }; entity_map.remove(&entities.get_id_from_attached(&child_body)?); entity_map.insert(attached_id, Entity::Module(new_module)); entity_map.remove(&suspension.child); entity_map.insert(suspension_id, Entity::Module(suspension_module)); } else { let new_module = Module { handle: child_body.handle, module_type: child_body.module_type, lifetime: 0., flags: 0, children: Vec::new(), }; entity_map.remove(&entities.get_id_from_attached(&child_body)?); let attached_id = get_entity_id(); entity_map.insert(attached_id, Entity::Module(new_module)); } } } entities.entities = entity_map; Some(id) } pub fn attach_new( data: &mut PhysicsData, entities: &mut EntityHandler, parent: EntityId, player_id: EntityId, module: &ModuleTemplate, attachment_slot: usize, ) -> Option { let mut entity_map = entities.entities.clone(); let parent_entity = entity_map.get_mut(&parent)?; let parent_handle = match parent_entity { Entity::Player(player) => player.handle, Entity::AttachedModule(module) => module.handle, _ => { panic!("unexpected parent"); } }; let parent_body = data.rigid_body_set.get(parent_handle)?; let parent_pos = vector![parent_body.translation().x, parent_body.translation().y]; let (anchor, rotation) = if module.module_type == ModuleType::LandingThrusterSuspension { match attachment_slot { 0 => (point![0. / SCALE, 0. / SCALE], PI), 1 => (point![0. / SCALE, 0. / SCALE], -PI / 2.), 2 => (point![0. / SCALE, 0. / SCALE], 0.), 3 => (point![0. / SCALE, 0. / SCALE], PI / 2.), _ => (point![0. / SCALE, 0. / SCALE], 0.), } } else { match attachment_slot { 0 => (point![0. / SCALE, 53. / SCALE], PI), 1 => (point![-53. / SCALE, 0. / SCALE], -PI / 2.), 2 => (point![0. / SCALE, -53. / SCALE], 0.), 3 => (point![53. / SCALE, 0. / SCALE], PI / 2.), _ => (point![0. / SCALE, 53. / SCALE], 0.), } }; let relative_pos = vector![ anchor.x.mul_add( (parent_body.rotation().angle()).cos(), anchor.y * -(parent_body.rotation().angle()).sin() ), anchor.x.mul_add( (parent_body.rotation().angle()).sin(), anchor.y * (parent_body.rotation().angle()).cos() ) ]; let module_pos = parent_pos + relative_pos; // create attachment module let module_shape = if module.module_type == ModuleType::LandingThrusterSuspension { SharedShape::cuboid(25./SCALE, 1./SCALE) } else { SharedShape::cuboid(25./SCALE, 25./SCALE) }; let relative_pos = if module.module_type == ModuleType::LandingThrusterSuspension { vector![0., -24./SCALE] } else { vector![0., 0.] }; let module_collider = ColliderBuilder::new(module_shape) .mass_properties(module.mass_properties) .translation(relative_pos) .build(); let module_body = RigidBodyBuilder::dynamic() .translation(module_pos) .rotation(parent_body.rotation().angle() + rotation) .build(); let attached_handle = data.rigid_body_set.insert(module_body); data.collider_set.insert_with_parent( module_collider, attached_handle, &mut data.rigid_body_set, ); let attach_joint_handle; if module.module_type == ModuleType::LandingThrusterSuspension { let y = Vector::y_axis(); let attach_joint = PrismaticJointBuilder::new(y) .local_anchor1(anchor) .local_anchor2(point![0.0, 0.0 / SCALE]) .limits([0.0, 14.625/SCALE]) .motor_position(0., 32., 4.) .build(); attach_joint_handle = data.impulse_joint_set .insert(parent_handle, attached_handle, attach_joint, true); } else { let attach_joint = FixedJointBuilder::new() .local_anchor1(anchor) .local_anchor2(point![0.0, 0.0 / SCALE]) .local_frame2(Isometry2::rotation(rotation)) .build(); attach_joint_handle = data.impulse_joint_set .insert(parent_handle, attached_handle, attach_joint, true); } let attached_module = Self { handle: attached_handle, module_type: module.module_type, player_id, children: [None, None, None, None], }; let attached_id = get_entity_id(); match parent_entity { Entity::Player(ref mut player) => { if module.module_type == ModuleType::LandingThrusterSuspension { player.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: true, }); } else { player.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: false, }); } } Entity::AttachedModule(ref mut module) => { if module.module_type == ModuleType::LandingThrusterSuspension { module.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: true, }); } else { module.children[attachment_slot] = Some(Attachment { child: attached_id, connection: attach_joint_handle, can_detach: false, }); } } _ => { panic!("unexpected parent"); } }; entity_map.insert(attached_id, Entity::AttachedModule(attached_module)); entities.entities = entity_map; Some(attached_id) } // TODO: remove this function pub const fn to_module(&self) -> Module { Module { handle: self.handle, module_type: self.module_type, lifetime: 10., flags: 0, children: Vec::new(), } } // TODO: this one too pub fn to_module_id(&self, entities: &EntityHandler) -> Option<(EntityId, Module)> { Some(( entities.get_id_from_attached(self)?, Module { handle: self.handle, module_type: self.module_type, lifetime: 10., flags: 1, children: Vec::new(), }, )) } pub fn to_protocol( &self, entities: &EntityHandler, data: &RigidBodySet, ) -> Option { let body = data.get(self.handle)?; let children = self.children.to_vec(); let mut prot_children = Vec::new(); for i in 1..children.len() { if let Some(Some(child)) = children.get(i) { prot_children.push(starkingdoms_protocol::module::Attachment { id: child.child, slot: u32::try_from(i).ok()?, special_fields: SpecialFields::default(), }); } } Some(starkingdoms_protocol::module::AttachedModule { module_type: self.module_type.into(), rotation: body.rotation().angle(), x: body.translation().x * SCALE, y: body.translation().y * SCALE, id: entities.get_id_from_attached(self)?, children: prot_children, special_fields: SpecialFields::default(), }) } pub fn search_modules(&self, entities: &EntityHandler) -> Vec { let mut modules = vec![self.clone()]; for attachment in self.children.iter().flatten() { if let Some(Entity::AttachedModule(child_module)) = entities.entities.get(&attachment.child) { modules.append(&mut child_module.search_modules(entities)); } } modules } pub fn find_parent(&self, module: &Self, entities: &EntityHandler) -> Option<(u8, EntityId)> { for (slot, attachment) in self.children.iter().enumerate() { if let Some(attachment) = attachment { if let Entity::AttachedModule(child_module) = entities.entities.get(&attachment.child)? { if *child_module == *module { return Some(( u8::try_from(slot).ok()?, entities.get_id_from_attached(self)?, )); } let parent = child_module.find_parent(module, entities); if parent.is_some() { return parent; } } } } None } } #[derive(Debug, Clone, PartialEq, Eq)] pub struct Attachment { pub child: EntityId, pub connection: ImpulseJointHandle, pub can_detach: bool, }