diff --git a/gradle.properties b/gradle.properties index d7f3cf59..de63da27 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1,4 +1,4 @@ -version=2.0.3 +version=2.0.3-ssargfix-0.2 group=dev.ryanhcode.sable java_version=21 @@ -7,7 +7,7 @@ minecraft_version=1.21.1 mod_name=Sable mod_author=RyanHCode mod_id=sable -credits=Ocelot, Eriksonn, Cyvack, Bee, Kyan, Cake, Rhyguy1 +credits=Ocelot, Eriksonn, Cyvack, Bee, Kyan, Cake, Rhyguy1, ssarg license=PolyForm Shield License 1.0.0 description=Interactive moving block structures with physics. issues=https://github.com/ryanhcode/sable/issues diff --git a/sable_rapier/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/RapierPhysicsPipeline.java b/sable_rapier/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/RapierPhysicsPipeline.java index f741a1a1..2bb12f8e 100644 --- a/sable_rapier/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/RapierPhysicsPipeline.java +++ b/sable_rapier/src/main/java/dev/ryanhcode/sable/physics/impl/rapier/RapierPhysicsPipeline.java @@ -118,7 +118,9 @@ protected long getSceneHandle() { @Override public void init(@Nullable final Vector3dc gravity, final double universalDrag) { try { + Sable.LOGGER.info("[SableDebug] Initializing Rapier physics pipeline with gravity=({}, {}, {}), drag={}", gravity.x(), gravity.y(), gravity.z(), universalDrag); this.scene = new RapierPhysicsScene(Rapier3D.initialize(gravity.x(), gravity.y(), gravity.z(), universalDrag)); + Sable.LOGGER.info("[SableDebug] Rapier physics pipeline initialized successfully, scene handle={}", this.scene.handle()); } catch (final UnsatisfiedLinkError e) { Sable.LOGGER.error("Sable has failed to link with the natives for its Rapier pipeline. Please report with system details to " + Sable.ISSUE_TRACKER_URL, e); final CrashReport crashReport = CrashReport.forThrowable(e.getCause(), "Sable linking with Rapier natives"); @@ -145,7 +147,13 @@ public void dispose() { @Override public void prePhysicsTicks() { final double timeStep = 1.0 / 20.0; - Rapier3D.tick(this.scene.handle(), timeStep); + try { + Sable.LOGGER.debug("[SableDebug] prePhysicsTicks: calling tick(handle={}, timeStep={})", this.scene.handle(), timeStep); + Rapier3D.tick(this.scene.handle(), timeStep); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in prePhysicsTicks tick() call", e); + throw e; + } } /** @@ -156,14 +164,26 @@ public void prePhysicsTicks() { @Override public void physicsTick(final double timeStep) { this.updateContraptionPoses(); - Rapier3D.step(this.scene.handle(), timeStep); + try { + Sable.LOGGER.debug("[SableDebug] physicsTick: calling step(handle={}, timeStep={})", this.scene.handle(), timeStep); + Rapier3D.step(this.scene.handle(), timeStep); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in physicsTick step() call", e); + throw e; + } for (final PhysicsPipelineBody queuedWakeUp : this.queuedWakeUps) { if (queuedWakeUp.isRemoved()) { continue; } - Rapier3D.wakeUpObject(this.scene.handle(), queuedWakeUp.getRuntimeId()); + final int wakeId = Rapier3D.getID(queuedWakeUp); + Sable.LOGGER.debug("[SableDebug] physicsTick: waking up body id={}", wakeId); + try { + Rapier3D.wakeUpObject(this.scene.handle(), wakeId); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION waking up body id={}", wakeId, e); + } } this.queuedWakeUps.clear(); @@ -195,18 +215,25 @@ public void add(final ServerSubLevel subLevel, final Pose3dc pose) { final Quaterniondc rot = pose.orientation(); final int id = Rapier3D.getID(subLevel); - Rapier3D.createSubLevel(this.scene.handle(), id, new double[]{pos.x(), pos.y(), pos.z(), rot.x(), rot.y(), rot.z(), rot.w()}); + Sable.LOGGER.info("[SableDebug] add(SubLevel): id={}, pos=({}, {}, {}), rot=({}, {}, {}, {})", id, pos.x(), pos.y(), pos.z(), rot.x(), rot.y(), rot.z(), rot.w()); + try { + Rapier3D.createSubLevel(this.scene.handle(), id, new double[]{pos.x(), pos.y(), pos.z(), rot.x(), rot.y(), rot.z(), rot.w()}); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in add(SubLevel) createSubLevel, id={}", id, e); + throw e; + } subLevel.updateMergedMassData(1.0f); final Vector3dc centerOfMass = subLevel.getMassTracker().getCenterOfMass(); if (centerOfMass != null) { subLevel.logicalPose().rotationPoint().set(centerOfMass); - + Sable.LOGGER.info("[SableDebug] add(SubLevel): id={}, centerOfMass=({}, {}, {})", id, centerOfMass.x(), centerOfMass.y(), centerOfMass.z()); this.onStatsChanged(subLevel); } this.activeSubLevels.put(Rapier3D.getID(subLevel), subLevel); + Sable.LOGGER.info("[SableDebug] add(SubLevel): id={} added successfully, activeSubLevels.size={}", id, this.activeSubLevels.size()); } /** @@ -214,8 +241,16 @@ public void add(final ServerSubLevel subLevel, final Pose3dc pose) { */ @Override public void remove(final ServerSubLevel subLevel) { - Rapier3D.removeSubLevel(this.scene.handle(), Rapier3D.getID(subLevel)); - this.activeSubLevels.remove(Rapier3D.getID(subLevel)); + final int id = Rapier3D.getID(subLevel); + Sable.LOGGER.info("[SableDebug] remove(SubLevel): id={}", id); + try { + Rapier3D.removeSubLevel(this.scene.handle(), id); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in remove(SubLevel), id={}", id, e); + throw e; + } + this.activeSubLevels.remove(id); + Sable.LOGGER.info("[SableDebug] remove(SubLevel): id={} removed, activeSubLevels.size={}", id, this.activeSubLevels.size()); } /** @@ -301,11 +336,19 @@ public void remove(final KinematicContraption contraption) { @Override public Pose3d readPose(final ServerSubLevel subLevel, final Pose3d dest) { this.assertBodyValid(subLevel); - Rapier3D.getPose(this.scene.handle(), Rapier3D.getID(subLevel), this.poseCache); + final int id = Rapier3D.getID(subLevel); + Sable.LOGGER.debug("[SableDebug] readPose: id={}", id); + try { + Rapier3D.getPose(this.scene.handle(), id, this.poseCache); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in readPose getPose(), id={}", id, e); + throw e; + } dest.position().set(this.poseCache[0], this.poseCache[1], this.poseCache[2]); dest.orientation().set(this.poseCache[3], this.poseCache[4], this.poseCache[5], this.poseCache[6]); + Sable.LOGGER.debug("[SableDebug] readPose: id={}, pos=({}, {}, {}), rot=({}, {}, {}, {})", id, this.poseCache[0], this.poseCache[1], this.poseCache[2], this.poseCache[3], this.poseCache[4], this.poseCache[5], this.poseCache[6]); return dest; } @@ -331,6 +374,7 @@ public BoxHandle addBox(final BoxPhysicsObject box) { @Override public void handleChunkSectionAddition(final LevelChunkSection section, final int x, final int y, final int z, final boolean uploadDataIfGlobal) { this.accelerator.clearCache(); + Sable.LOGGER.debug("[SableDebug] handleChunkSectionAddition: section=({}, {}, {}), uploadDataIfGlobal={}, hasOnlyAir={}", x, y, z, uploadDataIfGlobal, section.hasOnlyAir()); // this means the x coordinate is the fastest changing, then z, then y final int[] array = new int[LevelChunkSection.SECTION_SIZE]; @@ -362,7 +406,12 @@ public void handleChunkSectionAddition(final LevelChunkSection section, final in int id = -1; if (plot != null && uploadDataIfGlobal) id = Rapier3D.getID(((ServerSubLevel) plot.getSubLevel())); - Rapier3D.addChunk(this.scene.handle(), x, y, z, array, global, id); + try { + Rapier3D.addChunk(this.scene.handle(), x, y, z, array, global, id); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in handleChunkSectionAddition addChunk({}, {}, {}), global={}, id={}", x, y, z, global, id, e); + throw e; + } } /** @@ -370,7 +419,13 @@ public void handleChunkSectionAddition(final LevelChunkSection section, final in */ @Override public void handleChunkSectionRemoval(final int x, final int y, final int z) { - Rapier3D.removeChunk(this.scene.handle(), x, y, z, !SubLevelContainer.getContainer(this.level).inBounds(x, z)); + Sable.LOGGER.debug("[SableDebug] handleChunkSectionRemoval: ({}, {}, {})", x, y, z); + try { + Rapier3D.removeChunk(this.scene.handle(), x, y, z, !SubLevelContainer.getContainer(this.level).inBounds(x, z)); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in handleChunkSectionRemoval removeChunk({}, {}, {})", x, y, z, e); + throw e; + } } /** @@ -387,6 +442,8 @@ public void handleBlockChange(final SectionPos sectionPos, final LevelChunkSecti y = (sectionPos.y() << 4) + y; z = (sectionPos.z() << 4) + z; + Sable.LOGGER.debug("[SableDebug] handleBlockChange: pos=({}, {}, {}), oldState={}, newState={}", x, y, z, oldState, newState); + final BlockPos globalBlockPos = new BlockPos(x, y, z); for (final Direction dir : Direction.values()) { @@ -395,7 +452,12 @@ public void handleBlockChange(final SectionPos sectionPos, final LevelChunkSecti final RapierVoxelColliderData colliderData = this.colliderBakery.getPhysicsDataForBlock(this.level.getBlockState(pos)); final int colliderValue = colliderData == null ? 0 : colliderData.handle() + 1; - Rapier3D.changeBlock(this.scene.handle(), pos.getX(), pos.getY(), pos.getZ(), packBlockState(state, colliderValue)); + try { + Rapier3D.changeBlock(this.scene.handle(), pos.getX(), pos.getY(), pos.getZ(), packBlockState(state, colliderValue)); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in handleBlockChange changeBlock neighbor ({}, {}, {}), colliderValue={}", pos.getX(), pos.getY(), pos.getZ(), colliderValue, e); + throw e; + } } // do it for the block without offset @@ -403,7 +465,12 @@ public void handleBlockChange(final SectionPos sectionPos, final LevelChunkSecti final RapierVoxelColliderData colliderData = this.colliderBakery.getPhysicsDataForBlock(newState); final int colliderValue = colliderData == null ? 0 : colliderData.handle() + 1; - Rapier3D.changeBlock(this.scene.handle(), x, y, z, packBlockState(state, colliderValue)); + try { + Rapier3D.changeBlock(this.scene.handle(), x, y, z, packBlockState(state, colliderValue)); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in handleBlockChange changeBlock self ({}, {}, {}), colliderValue={}", x, y, z, colliderValue, e); + throw e; + } } @Override @@ -415,11 +482,23 @@ public void onStatsChanged(@NotNull final ServerSubLevel subLevel) { final Vector3dc centerOfMass = subLevel.getMassTracker().getCenterOfMass(); if (centerOfMass != null) { - Rapier3D.setCenterOfMass(this.scene.handle(), id, centerOfMass.x(), centerOfMass.y(), centerOfMass.z()); - Rapier3D.setMassPropertiesFrom(this.scene.handle(), id, subLevel.getMassTracker()); + Sable.LOGGER.debug("[SableDebug] onStatsChanged: id={}, centerOfMass=({}, {}, {}), mass={}", id, centerOfMass.x(), centerOfMass.y(), centerOfMass.z(), subLevel.getMassTracker().getMass()); + try { + Rapier3D.setCenterOfMass(this.scene.handle(), id, centerOfMass.x(), centerOfMass.y(), centerOfMass.z()); + Rapier3D.setMassPropertiesFrom(this.scene.handle(), id, subLevel.getMassTracker()); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in onStatsChanged setCenterOfMass/setMassProperties, id={}", id, e); + throw e; + } } - Rapier3D.setLocalBounds(this.scene.handle(), id, plotBounds.minX(), plotBounds.minY(), plotBounds.minZ(), plotBounds.maxX(), plotBounds.maxY(), plotBounds.maxZ()); + Sable.LOGGER.debug("[SableDebug] onStatsChanged: id={}, bounds=({},{},{} -> {},{},{})", id, plotBounds.minX(), plotBounds.minY(), plotBounds.minZ(), plotBounds.maxX(), plotBounds.maxY(), plotBounds.maxZ()); + try { + Rapier3D.setLocalBounds(this.scene.handle(), id, plotBounds.minX(), plotBounds.minY(), plotBounds.minZ(), plotBounds.maxX(), plotBounds.maxY(), plotBounds.maxZ()); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in onStatsChanged setLocalBounds, id={}", id, e); + throw e; + } } /** @@ -433,7 +512,14 @@ public void onStatsChanged(@NotNull final ServerSubLevel subLevel) { public void teleport(final PhysicsPipelineBody body, final Vector3dc position, final Quaterniondc orientation) { this.assertBodyValid(body); - Rapier3D.teleportObject(this.scene.handle(), Rapier3D.getID(body), position.x(), position.y(), position.z(), orientation.x(), orientation.y(), orientation.z(), orientation.w()); + final int id = Rapier3D.getID(body); + Sable.LOGGER.debug("[SableDebug] teleport: id={}, pos=({}, {}, {}), rot=({}, {}, {}, {})", id, position.x(), position.y(), position.z(), orientation.x(), orientation.y(), orientation.z(), orientation.w()); + try { + Rapier3D.teleportObject(this.scene.handle(), id, position.x(), position.y(), position.z(), orientation.x(), orientation.y(), orientation.z(), orientation.w()); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in teleport, id={}", id, e); + throw e; + } if (body instanceof final ServerSubLevel subLevel) { subLevel.logicalPose().position().set(position); subLevel.logicalPose().orientation().set(orientation); @@ -451,8 +537,15 @@ public void teleport(final PhysicsPipelineBody body, final Vector3dc position, f public void applyImpulse(final PhysicsPipelineBody body, final Vector3dc position, final Vector3dc force) { this.assertBodyValid(body); + final int id = Rapier3D.getID(body); final Vector3dc centerOfMass = body.getMassTracker().getCenterOfMass(); - Rapier3D.applyForce(this.scene.handle(), Rapier3D.getID(body), position.x() - centerOfMass.x(), position.y() - centerOfMass.y(), position.z() - centerOfMass.z(), force.x(), force.y(), force.z(), true); + Sable.LOGGER.debug("[SableDebug] applyImpulse: id={}, relPos=({}, {}, {}), force=({}, {}, {})", id, position.x() - centerOfMass.x(), position.y() - centerOfMass.y(), position.z() - centerOfMass.z(), force.x(), force.y(), force.z()); + try { + Rapier3D.applyForce(this.scene.handle(), id, position.x() - centerOfMass.x(), position.y() - centerOfMass.y(), position.z() - centerOfMass.z(), force.x(), force.y(), force.z(), true); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in applyImpulse, id={}", id, e); + throw e; + } } /** @@ -464,7 +557,14 @@ public void applyImpulse(final PhysicsPipelineBody body, final Vector3dc positio @Override public void applyLinearAndAngularImpulse(final PhysicsPipelineBody body, final Vector3dc force, final Vector3dc torque, final boolean wakeUp) { this.assertBodyValid(body); - Rapier3D.applyForceAndTorque(this.scene.handle(), Rapier3D.getID(body), force.x(), force.y(), force.z(), torque.x(), torque.y(), torque.z(), wakeUp); + final int id = Rapier3D.getID(body); + Sable.LOGGER.debug("[SableDebug] applyLinearAndAngularImpulse: id={}, force=({}, {}, {}), torque=({}, {}, {}), wakeUp={}", id, force.x(), force.y(), force.z(), torque.x(), torque.y(), torque.z(), wakeUp); + try { + Rapier3D.applyForceAndTorque(this.scene.handle(), id, force.x(), force.y(), force.z(), torque.x(), torque.y(), torque.z(), wakeUp); + } catch (final Exception e) { + Sable.LOGGER.error("[SableDebug] EXCEPTION in applyLinearAndAngularImpulse, id={}", id, e); + throw e; + } } /** diff --git a/sable_rapier/src/main/rust/rapier/src/boxes.rs b/sable_rapier/src/main/rust/rapier/src/boxes.rs index faf5a3ed..3479db38 100644 --- a/sable_rapier/src/main/rust/rapier/src/boxes.rs +++ b/sable_rapier/src/main/rust/rapier/src/boxes.rs @@ -81,16 +81,16 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_rem let sim_data = &mut *sim_data; let mut sable_data = scene.sable_data.write().unwrap(); - let handle = sable_data.rigid_bodies[&(id as LevelColliderID)]; - sim_data.rigid_body_set.remove( - handle, - &mut sim_data.island_manager, - &mut sim_data.collider_set, - &mut sim_data.impulse_joint_set, - &mut sim_data.multibody_joint_set, - true, - ); - - sable_data.rigid_bodies.remove(&(id as LevelColliderID)); + if let Some(&handle) = sable_data.rigid_bodies.get(&(id as LevelColliderID)) { + sim_data.rigid_body_set.remove( + handle, + &mut sim_data.island_manager, + &mut sim_data.collider_set, + &mut sim_data.impulse_joint_set, + &mut sim_data.multibody_joint_set, + true, + ); + sable_data.rigid_bodies.remove(&(id as LevelColliderID)); + } }) } diff --git a/sable_rapier/src/main/rust/rapier/src/joints.rs b/sable_rapier/src/main/rust/rapier/src/joints.rs index 4d99b288..9b144ee3 100644 --- a/sable_rapier/src/main/rust/rapier/src/joints.rs +++ b/sable_rapier/src/main/rust/rapier/src/joints.rs @@ -1,5 +1,5 @@ use crate::config::{JOINT_SPRING_DAMPING_RATIO, JOINT_SPRING_FREQUENCY}; -use crate::scene::{LevelColliderID, PhysicsScene}; +use crate::scene::{LevelColliderID, PhysicsScene, SableSceneData}; use crate::with_handle; use jni::JNIEnv; use jni::objects::{JClass, JDoubleArray}; @@ -10,12 +10,25 @@ use rapier3d::dynamics::{ }; use rapier3d::glamx::{DVec3, Quat}; use rapier3d::math::Vec3; -use rapier3d::prelude::{FixedJointBuilder, ImpulseJointHandle}; +use rapier3d::prelude::{FixedJointBuilder, ImpulseJointHandle, RigidBodyHandle}; use std::collections::HashMap; type SableJointHandle = jlong; type RapierJointHandle = ImpulseJointHandle; +fn get_rb( + sable_data: &SableSceneData, + ground_handle: RigidBodyHandle, + id: jint, +) -> Option { + if id == -1 { + Some(ground_handle) + } else { + sable_data.rigid_bodies.get(&(id as LevelColliderID)).copied() + } +} + + struct SubLevelJoint { id_a: Option, id_b: Option, @@ -326,16 +339,14 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add let mut sable_data = scene.sable_data.write().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb_a = if id_a == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_a as LevelColliderID)] + let rb_a = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_a) { + Some(rb) => rb, + None => return -1, }; - let rb_b = if id_b == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_b as LevelColliderID)] + let rb_b = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_b) { + Some(rb) => rb, + None => return -1, }; let revolute = RevoluteJointBuilder::new( @@ -413,16 +424,14 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add let mut sable_data = scene.sable_data.write().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb_a = if id_a == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_a as LevelColliderID)] + let rb_a = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_a) { + Some(rb) => rb, + None => return -1, }; - let rb_b = if id_b == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_b as LevelColliderID)] + let rb_b = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_b) { + Some(rb) => rb, + None => return -1, }; let quat = Quat::from_xyzw( @@ -505,16 +514,14 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add let mut sable_data = scene.sable_data.write().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb_a = if id_a == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_a as LevelColliderID)] + let rb_a = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_a) { + Some(rb) => rb, + None => return -1, }; - let rb_b = if id_b == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_b as LevelColliderID)] + let rb_b = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_b) { + Some(rb) => rb, + None => return -1, }; let mut joint = GenericJointBuilder::new(JointAxesMask::empty()).softness( @@ -599,16 +606,14 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add let mut sable_data = scene.sable_data.write().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb_a = if id_a == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_a as LevelColliderID)] + let rb_a = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_a) { + Some(rb) => rb, + None => return -1, }; - let rb_b = if id_b == -1 { - scene.ground_handle.unwrap() - } else { - sable_data.rigid_bodies[&(id_b as LevelColliderID)] + let rb_b = match get_rb(&sable_data, scene.ground_handle.unwrap(), id_b) { + Some(rb) => rb, + None => return -1, }; let locked_axes = JointAxesMask::from_bits_truncate(locked_axes_mask as u8); diff --git a/sable_rapier/src/main/rust/rapier/src/lib.rs b/sable_rapier/src/main/rust/rapier/src/lib.rs index 2d224733..bca60aef 100644 --- a/sable_rapier/src/main/rust/rapier/src/lib.rs +++ b/sable_rapier/src/main/rust/rapier/src/lib.rs @@ -282,12 +282,9 @@ pub fn get_rigid_body_mut<'a>( sim: &'a mut SimulationSceneData, sable_data: &SableSceneData, id: LevelColliderID, -) -> &'a mut RigidBody { - let handle = sable_data - .rigid_bodies - .get(&id) - .expect("No rigid body for id"); - &mut sim.rigid_body_set[*handle] +) -> Option<&'a mut RigidBody> { + let handle = sable_data.rigid_bodies.get(&id)?; + sim.rigid_body_set.get_mut(*handle) } #[inline(always)] @@ -295,12 +292,9 @@ pub fn get_rigid_body<'a>( sim: &'a SimulationSceneData, sable_data: &SableSceneData, id: LevelColliderID, -) -> &'a RigidBody { - let handle = sable_data - .rigid_bodies - .get(&id) - .expect("No rigid body for id"); - &sim.rigid_body_set[*handle] +) -> Option<&'a RigidBody> { + let handle = sable_data.rigid_bodies.get(&id)?; + sim.rigid_body_set.get(*handle) } #[unsafe(no_mangle)] @@ -527,20 +521,19 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_get let sable_data = scene.sable_data.read().unwrap(); let sim_data = scene.sim_data.read().unwrap(); - let rb: &RigidBody = - &sim_data.rigid_body_set[sable_data.rigid_bodies[&(id as LevelColliderID)]]; - - let arr: [jdouble; 7] = [ - rb.translation().x as jdouble, - rb.translation().y as jdouble, - rb.translation().z as jdouble, - rb.rotation().x as jdouble, - rb.rotation().y as jdouble, - rb.rotation().z as jdouble, - rb.rotation().w as jdouble, - ]; - - env.set_double_array_region(&store, 0, &arr).unwrap(); + if let Some(rb) = get_rigid_body(&sim_data, &sable_data, id as LevelColliderID) { + let arr: [jdouble; 7] = [ + rb.translation().x as jdouble, + rb.translation().y as jdouble, + rb.translation().z as jdouble, + rb.rotation().x as jdouble, + rb.rotation().y as jdouble, + rb.rotation().z as jdouble, + rb.rotation().w as jdouble, + ]; + + let _ = env.set_double_array_region(&store, 0, &arr); + } }) } @@ -1112,12 +1105,12 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set let sable_data = scene.sable_data.read().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb = &mut sim_data.rigid_body_set[sable_data.rigid_bodies[&(id as LevelColliderID)]]; - - rb.set_additional_mass_properties( - MassProperties::with_inertia_matrix(Vec3::ZERO, mass as Real, inertia_tensor.into()), - true, - ); + if let Some(rb) = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID) { + rb.set_additional_mass_properties( + MassProperties::with_inertia_matrix(Vec3::ZERO, mass as Real, inertia_tensor.into()), + true, + ); + } }) } @@ -1142,12 +1135,12 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_tel let sable_data = scene.sable_data.read().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb = &mut sim_data.rigid_body_set[sable_data.rigid_bodies[&(id as LevelColliderID)]]; - - let mut pose = *rb.position(); - pose.translation = Vec3::new(x as Real, y as Real, z as Real); - pose.rotation = Quat::from_xyzw(i as Real, j as Real, k as Real, r as Real); - rb.set_position(pose, true); + if let Some(rb) = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID) { + let mut pose = *rb.position(); + pose.translation = Vec3::new(x as Real, y as Real, z as Real); + pose.rotation = Quat::from_xyzw(i as Real, j as Real, k as Real, r as Real); + rb.set_position(pose, true); + } }) } @@ -1164,8 +1157,9 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_wak with_handle(handle, |scene| { let sable_data = scene.sable_data.read().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb = &mut sim_data.rigid_body_set[sable_data.rigid_bodies[&(id as LevelColliderID)]]; - rb.wake_up(true); + if let Some(rb) = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID) { + rb.wake_up(true); + } }) } @@ -1188,20 +1182,20 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add with_handle(handle, |scene| { let sable_data = scene.sable_data.read().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let rb = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID); + if let Some(rb) = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID) { + if wake_up == 0 && rb.is_sleeping() { + return; + } - if wake_up == 0 && rb.is_sleeping() { - return; + rb.set_linvel( + rb.linvel() + Vec3::new(linear_x as Real, linear_y as Real, linear_z as Real), + wake_up > 0, + ); + rb.set_angvel( + rb.angvel() + Vec3::new(angular_x as Real, angular_y as Real, angular_z as Real), + wake_up > 0, + ); } - - rb.set_linvel( - rb.linvel() + Vec3::new(linear_x as Real, linear_y as Real, linear_z as Real), - wake_up > 0, - ); - rb.set_angvel( - rb.angvel() + Vec3::new(angular_x as Real, angular_y as Real, angular_z as Real), - wake_up > 0, - ); }) } @@ -1288,27 +1282,23 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_app let sable_data = scene.sable_data.read().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let body = sable_data - .rigid_bodies - .get(&(id as LevelColliderID)) - .unwrap(); - let rb = &mut sim_data.rigid_body_set[*body]; - - if wake_up == 0 && rb.is_sleeping() { - return; - } + if let Some(rb) = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID) { + if wake_up == 0 && rb.is_sleeping() { + return; + } - let force: Vec3 = rb - .rotation() - .mul_vec3(Vec3::new(fx as Real, fy as Real, fz as Real)); - let force_pos = rb - .position() - .transform_point(Vec3::new(x as Real, y as Real, z as Real)); + let force: Vec3 = rb + .rotation() + .mul_vec3(Vec3::new(fx as Real, fy as Real, fz as Real)); + let force_pos = rb + .position() + .transform_point(Vec3::new(x as Real, y as Real, z as Real)); - rb.apply_impulse(force, wake_up > 0); + rb.apply_impulse(force, wake_up > 0); - let torque_impulse = (force_pos - rb.position().translation).cross(force); - rb.apply_torque_impulse(torque_impulse, wake_up > 0); + let torque_impulse = (force_pos - rb.position().translation).cross(force); + rb.apply_torque_impulse(torque_impulse, wake_up > 0); + } }) } @@ -1333,25 +1323,21 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_app let sable_data = scene.sable_data.read().unwrap(); let mut sim_data = scene.sim_data.write().unwrap(); - let body = sable_data - .rigid_bodies - .get(&(id as LevelColliderID)) - .unwrap(); - let rb = &mut sim_data.rigid_body_set[*body]; - - if wake_up == 0 && rb.is_sleeping() { - return; - } + if let Some(rb) = get_rigid_body_mut(&mut sim_data, &sable_data, id as LevelColliderID) { + if wake_up == 0 && rb.is_sleeping() { + return; + } - let force: Vec3 = rb - .rotation() - .mul_vec3(Vec3::new(fx as Real, fy as Real, fz as Real)); - rb.apply_impulse(force, wake_up > 0); + let force: Vec3 = rb + .rotation() + .mul_vec3(Vec3::new(fx as Real, fy as Real, fz as Real)); + rb.apply_impulse(force, wake_up > 0); - let torque: Vec3 = rb - .rotation() - .mul_vec3(Vec3::new(tx as Real, ty as Real, tz as Real)); - rb.apply_torque_impulse(torque, wake_up > 0); + let torque: Vec3 = rb + .rotation() + .mul_vec3(Vec3::new(tx as Real, ty as Real, tz as Real)); + rb.apply_torque_impulse(torque, wake_up > 0); + } }) } @@ -1370,20 +1356,16 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_get let sable_data = scene.sable_data.read().unwrap(); let sim_data = scene.sim_data.read().unwrap(); - let body = sable_data - .rigid_bodies - .get(&(id as LevelColliderID)) - .unwrap(); - let rb = &sim_data.rigid_body_set[*body]; + if let Some(rb) = get_rigid_body(&sim_data, &sable_data, id as LevelColliderID) { + let vel = rb.linvel(); - let vel = rb.linvel(); - - _env.set_double_array_region( - &store, - 0, - &[vel.x as jdouble, vel.y as jdouble, vel.z as jdouble], - ) - .unwrap(); + _env.set_double_array_region( + &store, + 0, + &[vel.x as jdouble, vel.y as jdouble, vel.z as jdouble], + ) + .unwrap(); + } }) } @@ -1402,19 +1384,15 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_get let sable_data = scene.sable_data.read().unwrap(); let sim_data = scene.sim_data.read().unwrap(); - let body = sable_data - .rigid_bodies - .get(&(id as LevelColliderID)) - .unwrap(); - let rb = &sim_data.rigid_body_set[*body]; - - let vel = rb.angvel(); + if let Some(rb) = get_rigid_body(&sim_data, &sable_data, id as LevelColliderID) { + let vel = rb.angvel(); - _env.set_double_array_region( - &store, - 0, - &[vel.x as jdouble, vel.y as jdouble, vel.z as jdouble], - ) - .unwrap(); + _env.set_double_array_region( + &store, + 0, + &[vel.x as jdouble, vel.y as jdouble, vel.z as jdouble], + ) + .unwrap(); + } }) }