diff --git a/.github/actions/build/action.yml b/.github/actions/build/action.yml index 3b706c4..5624128 100644 --- a/.github/actions/build/action.yml +++ b/.github/actions/build/action.yml @@ -18,6 +18,12 @@ inputs: sconsflags: required: true description: Extra flags + precision: + required: true + description: single or double + features: + required: true + description: simd-nightly,simd-stable,parallel,enhanced-determinism runs: using: composite @@ -42,22 +48,26 @@ runs: SCONS_CACHE: .scons-cache SCONS_CACHE_DIR: .scons-cache run: | - cd godot-cpp && scons target=${{ inputs.target }} platform=${{ inputs.platform }} arch=${{ inputs.arch }} generate_bindings=yes ${{ inputs.sconsflags }} + cd godot-cpp && \ + scons target=${{ inputs.target }} platform=${{ inputs.platform }} arch=${{ inputs.arch }} generate_bindings=yes ${{ inputs.sconsflags }} precision=${{ inputs.precision }} - name: Build Rapier shell: sh run: | - cd src/rapier2d-wrapper && cargo build ${{ inputs.rust_target }} --release + cd src/rapier2d-wrapper && \ + cargo build ${{ inputs.rust_target }} --release --features=${{ inputs.features }} - name: Build Rapier Macos Universal shell: sh # we already built for x86_64-apple-darwin for mac, now build arm64 if: ${{ inputs.platform == 'macos'}} run: | - cd src/rapier2d-wrapper && mkdir release && rustup target add aarch64-apple-darwin && cargo build --target=aarch64-apple-darwin --release && lipo -create -output target/release/librapier2d_wrapper.a target/aarch64-apple-darwin/release/librapier2d_wrapper.a target/x86_64-apple-darwin/release/librapier2d_wrapper.a + cd src/rapier2d-wrapper && mkdir release && rustup target add aarch64-apple-darwin && \ + cargo build --target=aarch64-apple-darwin --release --features=${{ inputs.features }} && \ + lipo -create -output target/release/librapier2d_wrapper.a target/aarch64-apple-darwin/release/librapier2d_wrapper.a target/x86_64-apple-darwin/release/librapier2d_wrapper.a - name: Build Physics Server Rapier shell: sh env: SCONS_CACHE: .scons-cache SCONS_CACHE_DIR: .scons-cache run: | - scons target=${{ inputs.target }} platform=${{ inputs.platform }} arch=${{ inputs.arch }} generate_bindings=no ${{ inputs.sconsflags }} + scons target=${{ inputs.target }} platform=${{ inputs.platform }} arch=${{ inputs.arch }} generate_bindings=no ${{ inputs.sconsflags }} precision=${{ inputs.precision }} diff --git a/.github/actions/upload/action.yml b/.github/actions/upload/action.yml index b7ca99c..89d4b8f 100644 --- a/.github/actions/upload/action.yml +++ b/.github/actions/upload/action.yml @@ -1,13 +1,21 @@ name: Physics Server Rapier Upload description: Upload Godot Cpp and the Physics Server 2D Extension. +inputs: + precision: + required: true + description: single or double + features: + required: true + description: simd-nightly,simd-stable,parallel,enhanced-determinism + runs: using: composite steps: - name: Upload Artifact uses: actions/upload-artifact@v3 with: - name: Godot Rapier 2D + name: Godot Rapier 2D ${{ inputs.precision }} ${{ inputs.features }} path: | ${{ github.workspace }}/bin/** retention-days: 14 diff --git a/.github/workflows/android_builds.yml b/.github/workflows/android_builds.yml index 477aaeb..a178f64 100644 --- a/.github/workflows/android_builds.yml +++ b/.github/workflows/android_builds.yml @@ -9,12 +9,14 @@ env: jobs: android: runs-on: "ubuntu-20.04" - name: Android Build ${{ matrix.target }} ${{ matrix.arch }} + name: ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} strategy: fail-fast: false matrix: target: [template_debug, template_release] arch: [x86_64] # Todo reenable arm64 + precision: [single, double] + features: [simd-stable, enhanced-determinism] steps: - uses: actions/checkout@v3 @@ -26,13 +28,18 @@ jobs: distribution: temurin java-version: 11 - - name: Build ${{ matrix.target }} ${{ matrix.arch }} + - name: Build ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/build with: sconsflags: ${{ env.SCONSFLAGS }} arch: ${{ matrix.arch }} platform: android target: ${{ matrix.target }} + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} - - name: Upload ${{ matrix.target }} ${{ matrix.arch }} + - name: Upload ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/upload + with: + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} diff --git a/.github/workflows/ios_builds.yml b/.github/workflows/ios_builds.yml index be96c8d..dfe48cf 100644 --- a/.github/workflows/ios_builds.yml +++ b/.github/workflows/ios_builds.yml @@ -9,24 +9,31 @@ env: jobs: ios: runs-on: "macos-latest" - name: iOS Build ${{ matrix.target }} ${{ matrix.arch }} + name: ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} strategy: fail-fast: false matrix: target: [template_debug, template_release] arch: [arm64] + precision: [single, double] + features: [simd-stable, enhanced-determinism] steps: - uses: actions/checkout@v3 with: submodules: true - - name: Build ${{ matrix.target }} ${{ matrix.arch }} + - name: Build ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/build with: sconsflags: ${{ env.SCONSFLAGS }} arch: ${{ matrix.arch }} platform: ios target: ${{ matrix.target }} + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} - - name: Upload ${{ matrix.target }} ${{ matrix.arch }} + - name: Upload ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/upload + with: + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} diff --git a/.github/workflows/linux_builds.yml b/.github/workflows/linux_builds.yml index 17ed149..88f129a 100644 --- a/.github/workflows/linux_builds.yml +++ b/.github/workflows/linux_builds.yml @@ -9,26 +9,33 @@ env: jobs: linux: runs-on: ubuntu-20.04 - name: Linux Build ${{ matrix.target }} ${{ matrix.arch }} + name: ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} strategy: fail-fast: false matrix: target: [template_debug, template_release] arch: [x86_64] + precision: [single, double] + features: [simd-stable, enhanced-determinism] steps: - uses: actions/checkout@v3 with: submodules: true - - name: Build ${{ matrix.target }} ${{ matrix.arch }} + - name: Build ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/build with: sconsflags: ${{ env.SCONSFLAGS }} arch: ${{ matrix.arch }} platform: linux target: ${{ matrix.target }} + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} - - name: Upload ${{ matrix.target }} ${{ matrix.arch }} + - name: Upload ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/upload + with: + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} diff --git a/.github/workflows/macos_builds.yml b/.github/workflows/macos_builds.yml index 3c5c369..317a546 100644 --- a/.github/workflows/macos_builds.yml +++ b/.github/workflows/macos_builds.yml @@ -10,19 +10,21 @@ env: jobs: macos: runs-on: "macos-latest" - name: macOS Build ${{ matrix.target }} ${{ matrix.arch }} + name: ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} strategy: fail-fast: false matrix: target: [template_debug, template_release] arch: [universal] + precision: [single, double] + features: [simd-stable, enhanced-determinism] steps: - uses: actions/checkout@v3 with: submodules: true - - name: Build ${{ matrix.target }} ${{ matrix.arch }} + - name: Build ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/build with: sconsflags: ${{ env.SCONSFLAGS }} @@ -30,8 +32,10 @@ jobs: platform: macos rust_target: --target=x86_64-apple-darwin target: ${{ matrix.target }} + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} - - name: Sign frameworks + - name: Sign frameworks ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} shell: pwsh env: APPLE_CERT_BASE64: ${{ secrets.PROD_MACOS_CERTIFICATE }} @@ -43,5 +47,8 @@ jobs: run: ./scripts/ci-sign-macos.ps1 bin/addons/godot-rapier2d/bin/libphysics_server_rapier2d.macos.${{ matrix.target }}.framework if: ${{ env.APPLE_DEV_ID }} - - name: Upload ${{ matrix.target }} ${{ matrix.arch }} + - name: Upload ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/upload + with: + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} diff --git a/.github/workflows/web_builds.yml b/.github/workflows/web_builds.yml index 08e292b..43b8103 100644 --- a/.github/workflows/web_builds.yml +++ b/.github/workflows/web_builds.yml @@ -11,12 +11,14 @@ env: jobs: web: runs-on: ubuntu-20.04 - name: Web Build ${{ matrix.target }} ${{ matrix.arch }} + name: ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} strategy: fail-fast: false matrix: target: [template_debug, template_release] arch: [wasm32] + precision: [single, double] + features: [simd-stable, enhanced-determinism] steps: - uses: actions/checkout@v3 @@ -33,14 +35,19 @@ jobs: run: | emcc -v - - name: Build ${{ matrix.target }} ${{ matrix.arch }} + - name: Build ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/build with: sconsflags: ${{ env.SCONSFLAGS }} arch: ${{ matrix.arch }} platform: javascript target: ${{ matrix.target }} + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} - - name: Upload ${{ matrix.target }} ${{ matrix.arch }} + - name: Upload ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/upload + with: + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} diff --git a/.github/workflows/windows_builds.yml b/.github/workflows/windows_builds.yml index 05857a9..048c6a9 100644 --- a/.github/workflows/windows_builds.yml +++ b/.github/workflows/windows_builds.yml @@ -9,12 +9,14 @@ env: jobs: build-windows: runs-on: "windows-latest" - name: Windows Build ${{ matrix.target }} ${{ matrix.arch }} + name: ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} strategy: fail-fast: false matrix: target: [template_debug, template_release] arch: [x86_64] # Rust doesn't have built in config for 32 bit windows build. + precision: [single, double] + features: [simd-stable, enhanced-determinism] steps: - uses: actions/checkout@v3 @@ -23,13 +25,18 @@ jobs: - name: Setup MSVC problem matcher uses: ammaraskar/msvc-problem-matcher@master - - name: Build ${{ matrix.target }} ${{ matrix.arch }} + - name: Build ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/build with: sconsflags: ${{ env.SCONSFLAGS }} arch: ${{ matrix.arch }} platform: windows target: ${{ matrix.target }} + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} - - name: Upload ${{ matrix.target }} ${{ matrix.arch }} + - name: Upload ${{ matrix.target }} ${{ matrix.arch }} ${{ matrix.precision }} ${{ matrix.features }} uses: ./.github/actions/upload + with: + precision: ${{ matrix.precision }} + features: ${{ matrix.features }} diff --git a/README.md b/README.md index cfcbca8..6bcf1ac 100644 --- a/README.md +++ b/README.md @@ -31,8 +31,20 @@ A 2d [rapier](https://github.com/dimforge/rapier) physics server for [Godot Engi - Shape scaling doesn't work (WIP) - Changing properties before they are added in world doesn't work (WIP) -# Installation +# Build Features + +## Float Precision + +This plugin is built for both single and double precision builds. + +## Rapier Features +This package supports the following features of rapier ([more on this here](https://rapier.rs/docs/user_guides/rust/getting_started)): + +- simd-stable: enables explicit SIMD optimizations using the wide crate. Has limited cross-platform support but can be used with a stable version of the Rust compiler. +- enhanced-determinism: enables cross-platform determinism (assuming the rest of your code is also deterministic) across all 32-bit and 64-bit platforms that implements the IEEE 754-2008 standard strictly. This includes most modern processors as well as WASM targets. + +# Installation - Automatic (Recommended): Download the plugin from the official [Godot Asset Store](https://godotengine.org/asset-library/asset/2267) using the `AssetLib` tab in Godot. diff --git a/SConstruct b/SConstruct index 855b3f0..36c3131 100644 --- a/SConstruct +++ b/SConstruct @@ -21,11 +21,15 @@ env.Append(LIBS=[lib]) ## Sources env.Append(CPPPATH=["src/"]) -sources = Glob("src/*.cpp") +sources = [Glob("src/*.cpp"),Glob("src/bodies/*.cpp"),Glob("src/joints/*.cpp"),Glob("src/servers/*.cpp"),Glob("src/shapes/*.cpp"),Glob("src/spaces/*.cpp")] if env["platform"] == "windows": env.Append(CPPDEFINES="WINDOWS_ENABLED") +if env["precision"] == "double": + env.Append(CPPDEFINES=["REAL_T_IS_DOUBLE"]) +else: + env.Append(CPPDEFINES=["REAL_T_IS_FLOAT"]) if env["platform"] == "macos": library = env.SharedLibrary( "bin/addons/godot-rapier2d/bin/libphysics_server_rapier2d.{}.{}.framework/libphysics_server_rapier2d.{}.{}".format( diff --git a/src/rapier_area_2d.cpp b/src/bodies/rapier_area_2d.cpp similarity index 99% rename from src/rapier_area_2d.cpp rename to src/bodies/rapier_area_2d.cpp index ac431b9..cc105c2 100644 --- a/src/rapier_area_2d.cpp +++ b/src/bodies/rapier_area_2d.cpp @@ -1,7 +1,7 @@ #include "rapier_area_2d.h" +#include "../servers/rapier_physics_server_2d.h" +#include "../spaces/rapier_space_2d.h" #include "rapier_body_2d.h" -#include "rapier_physics_server_2d.h" -#include "rapier_space_2d.h" void RapierArea2D::set_space(RapierSpace2D *p_space) { if (p_space == get_space()) { diff --git a/src/rapier_area_2d.h b/src/bodies/rapier_area_2d.h similarity index 100% rename from src/rapier_area_2d.h rename to src/bodies/rapier_area_2d.h diff --git a/src/rapier_body_2d.cpp b/src/bodies/rapier_body_2d.cpp similarity index 99% rename from src/rapier_body_2d.cpp rename to src/bodies/rapier_body_2d.cpp index 299e02a..8d26cec 100644 --- a/src/rapier_body_2d.cpp +++ b/src/bodies/rapier_body_2d.cpp @@ -1,7 +1,7 @@ #include "rapier_body_2d.h" -#include "rapier_body_direct_state_2d.h" -#include "rapier_space_2d.h" +#include "../spaces/rapier_space_2d.h" +#include "rapier_direct_body_state_2d.h" void RapierBody2D::_mass_properties_changed() { if (!get_space()) { diff --git a/src/rapier_body_2d.h b/src/bodies/rapier_body_2d.h similarity index 100% rename from src/rapier_body_2d.h rename to src/bodies/rapier_body_2d.h diff --git a/src/rapier_collision_object_2d.cpp b/src/bodies/rapier_collision_object_2d.cpp similarity index 99% rename from src/rapier_collision_object_2d.cpp rename to src/bodies/rapier_collision_object_2d.cpp index dacaf93..b0a2d94 100644 --- a/src/rapier_collision_object_2d.cpp +++ b/src/bodies/rapier_collision_object_2d.cpp @@ -1,6 +1,7 @@ #include "rapier_collision_object_2d.h" -#include "rapier_physics_server_2d.h" -#include "rapier_space_2d.h" + +#include "../servers/rapier_physics_server_2d.h" +#include "../spaces/rapier_space_2d.h" void RapierCollisionObject2D::add_shape(RapierShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) { Shape shape; diff --git a/src/rapier_collision_object_2d.h b/src/bodies/rapier_collision_object_2d.h similarity index 98% rename from src/rapier_collision_object_2d.h rename to src/bodies/rapier_collision_object_2d.h index 6262654..a544da1 100644 --- a/src/rapier_collision_object_2d.h +++ b/src/bodies/rapier_collision_object_2d.h @@ -1,10 +1,9 @@ #ifndef RAPIER_COLLISION_OBJECT_2D_H #define RAPIER_COLLISION_OBJECT_2D_H -#include "rapier_shape_2d.h" - -#include "rapier_include.h" +#include "../shapes/rapier_shape_2d.h" +#include "../rapier_include.h" #include #include #include diff --git a/src/rapier_body_direct_state_2d.cpp b/src/bodies/rapier_direct_body_state_2d.cpp similarity index 97% rename from src/rapier_body_direct_state_2d.cpp rename to src/bodies/rapier_direct_body_state_2d.cpp index fd74d1c..023aa40 100644 --- a/src/rapier_body_direct_state_2d.cpp +++ b/src/bodies/rapier_direct_body_state_2d.cpp @@ -1,8 +1,8 @@ -#include "rapier_body_direct_state_2d.h" +#include "rapier_direct_body_state_2d.h" +#include "../servers/rapier_physics_server_2d.h" +#include "../spaces/rapier_direct_space_state_2d.h" #include "rapier_body_2d.h" -#include "rapier_physics_server_2d.h" -#include "rapier_space_2d.h" Vector2 RapierDirectBodyState2D::_get_total_gravity() const { ERR_FAIL_COND_V(body->get_space(), Vector2()); diff --git a/src/rapier_body_direct_state_2d.h b/src/bodies/rapier_direct_body_state_2d.h similarity index 96% rename from src/rapier_body_direct_state_2d.h rename to src/bodies/rapier_direct_body_state_2d.h index b77152c..8b1197b 100644 --- a/src/rapier_body_direct_state_2d.h +++ b/src/bodies/rapier_direct_body_state_2d.h @@ -1,5 +1,5 @@ -#ifndef RAPIER_BODY_DIRECT_STATE_2D_H -#define RAPIER_BODY_DIRECT_STATE_2D_H +#ifndef RAPIER_DIRECT_BODY_STATE_2D_H +#define RAPIER_DIRECT_BODY_STATE_2D_H #include #include @@ -78,4 +78,4 @@ class RapierDirectBodyState2D : public PhysicsDirectBodyState2DExtension { virtual double _get_step() const override; }; -#endif // RAPIER_BODY_DIRECT_STATE_2D_H +#endif // RAPIER_DIRECT_BODY_STATE_2D_H diff --git a/src/joints/rapier_damped_spring_joint_2d.cpp b/src/joints/rapier_damped_spring_joint_2d.cpp new file mode 100644 index 0000000..df9c239 --- /dev/null +++ b/src/joints/rapier_damped_spring_joint_2d.cpp @@ -0,0 +1,46 @@ +#include "rapier_damped_spring_joint_2d.h" + +void RapierDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + rest_length = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + damping = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + stiffness = p_value; + } break; + } +} + +real_t RapierDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + return rest_length; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + return damping; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + return stiffness; + } break; + } + + ERR_FAIL_V(0); +} + +RapierDampedSpringJoint2D::RapierDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RapierBody2D *p_body_a, RapierBody2D *p_body_b) : + RapierJoint2D(p_body_a, p_body_b) { + Vector2 anchor_A = A->get_inv_transform().xform(p_anchor_a); + Vector2 anchor_B = B->get_inv_transform().xform(p_anchor_b); + + rest_length = p_anchor_a.distance_to(p_anchor_b); + + // TODO: create rapier joint when available + // See https://github.com/dimforge/rapier/issues/241 + ERR_FAIL_MSG("Spring joints not supported for now"); + + //A->add_constraint(this, 0); + //B->add_constraint(this, 1); +} diff --git a/src/joints/rapier_damped_spring_joint_2d.h b/src/joints/rapier_damped_spring_joint_2d.h new file mode 100644 index 0000000..14c14c1 --- /dev/null +++ b/src/joints/rapier_damped_spring_joint_2d.h @@ -0,0 +1,20 @@ +#ifndef RAPIER_DAMPED_SPRING_JOINT_2D_H +#define RAPIER_DAMPED_SPRING_JOINT_2D_H + +#include "rapier_joint_2d.h" + +class RapierDampedSpringJoint2D : public RapierJoint2D { + real_t rest_length = 0.0; + real_t damping = 1.5; + real_t stiffness = 20.0; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } + + void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; + + RapierDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RapierBody2D *p_body_a, RapierBody2D *p_body_b); +}; + +#endif // RAPIER_DAMPED_SPRING_JOINT_2D_H diff --git a/src/joints/rapier_groove_joint_2d.cpp b/src/joints/rapier_groove_joint_2d.cpp new file mode 100644 index 0000000..a8520b7 --- /dev/null +++ b/src/joints/rapier_groove_joint_2d.cpp @@ -0,0 +1,30 @@ +#include "rapier_groove_joint_2d.h" +#include "../spaces/rapier_space_2d.h" + +RapierGrooveJoint2D::RapierGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RapierBody2D *p_body_a, RapierBody2D *p_body_b) : + RapierJoint2D(p_body_a, p_body_b) { + Vector2 point_A_1 = A->get_inv_transform().xform(p_a_groove1); + Vector2 point_A_2 = A->get_inv_transform().xform(p_a_groove2); + + Vector2 anchor_B = B->get_inv_transform().xform(p_b_anchor); + + Vector2 axis = (point_A_2 - point_A_1).normalized(); + real_t length = (point_A_2 - point_A_1).length(); + + rapier2d::Vector rapier_anchor_A = { point_A_1.x, point_A_1.y }; + rapier2d::Vector rapier_anchor_B = { anchor_B.x, anchor_B.y }; + + rapier2d::Vector rapier_axis = { axis.x, axis.y }; + rapier2d::Vector rapier_limits = { 0.0, length }; + + ERR_FAIL_COND(!p_body_a->get_space()); + ERR_FAIL_COND(p_body_a->get_space() != p_body_b->get_space()); + space_handle = p_body_a->get_space()->get_handle(); + ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); + + handle = rapier2d::joint_create_prismatic(space_handle, p_body_a->get_body_handle(), p_body_b->get_body_handle(), &rapier_axis, &rapier_anchor_A, &rapier_anchor_B, &rapier_limits); + ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); + + //A->add_constraint(this, 0); + //B->add_constraint(this, 1); +} diff --git a/src/joints/rapier_groove_joint_2d.h b/src/joints/rapier_groove_joint_2d.h new file mode 100644 index 0000000..c541291 --- /dev/null +++ b/src/joints/rapier_groove_joint_2d.h @@ -0,0 +1,13 @@ +#ifndef RAPIER_GROOVE_JOINT_2D_H +#define RAPIER_GROOVE_JOINT_2D_H + +#include "rapier_joint_2d.h" + +class RapierGrooveJoint2D : public RapierJoint2D { +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } + + RapierGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RapierBody2D *p_body_a, RapierBody2D *p_body_b); +}; + +#endif // RAPIER_GROOVE_JOINT_2D_H diff --git a/src/joints/rapier_joint_2d.cpp b/src/joints/rapier_joint_2d.cpp new file mode 100644 index 0000000..696a7da --- /dev/null +++ b/src/joints/rapier_joint_2d.cpp @@ -0,0 +1,22 @@ +#include "rapier_joint_2d.h" +#include "../spaces/rapier_space_2d.h" + +RapierJoint2D::RapierJoint2D(RapierBody2D *p_body_a, RapierBody2D *p_body_b) { + A = p_body_a; + B = p_body_b; +} + +void RapierJoint2D::copy_settings_from(RapierJoint2D *p_joint) { + set_rid(p_joint->get_rid()); + set_max_force(p_joint->get_max_force()); + set_bias(p_joint->get_bias()); + set_max_bias(p_joint->get_max_bias()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); +} + +RapierJoint2D::~RapierJoint2D() { + if (rapier2d::is_handle_valid(handle)) { + ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); + rapier2d::joint_destroy(space_handle, handle); + } +}; diff --git a/src/joints/rapier_joint_2d.h b/src/joints/rapier_joint_2d.h new file mode 100644 index 0000000..d3b62c2 --- /dev/null +++ b/src/joints/rapier_joint_2d.h @@ -0,0 +1,48 @@ +#ifndef RAPIER_JOINT_2D_H +#define RAPIER_JOINT_2D_H + +#include "../bodies/rapier_body_2d.h" + +using namespace godot; + +class RapierJoint2D { + real_t bias = 0; + real_t max_bias = 3.40282e+38; + real_t max_force = 3.40282e+38; + + bool disabled_collisions_between_bodies = true; + RID rid; + +protected: + RapierBody2D *A; + RapierBody2D *B; + + rapier2d::Handle space_handle = rapier2d::invalid_handle(); + rapier2d::Handle handle = rapier2d::invalid_handle(); + +public: + _FORCE_INLINE_ void set_rid(const RID &p_rid) { rid = p_rid; } + _FORCE_INLINE_ RID get_rid() const { return rid; } + + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } + + _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } + _FORCE_INLINE_ real_t get_max_force() const { return max_force; } + + _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } + _FORCE_INLINE_ real_t get_bias() const { return bias; } + + _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } + _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } + + void copy_settings_from(RapierJoint2D *p_joint); + + virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } + + RapierJoint2D(RapierBody2D *p_body_a, RapierBody2D *p_body_b = nullptr); + RapierJoint2D() {} + virtual ~RapierJoint2D(); +}; + +#endif // RAPIER_JOINT_2D_H diff --git a/src/joints/rapier_pin_joint_2d.cpp b/src/joints/rapier_pin_joint_2d.cpp new file mode 100644 index 0000000..e5eaca6 --- /dev/null +++ b/src/joints/rapier_pin_joint_2d.cpp @@ -0,0 +1,84 @@ +#include "rapier_pin_joint_2d.h" +#include "../spaces/rapier_space_2d.h" + +void RapierPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: { + angular_limit_upper = p_value; + } break; + case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: { + angular_limit_lower = p_value; + } break; + case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: { + motor_target_velocity = p_value; + } break; + case PhysicsServer2D::PIN_JOINT_SOFTNESS: { + WARN_PRINT_ONCE("PIN_JOINT_SOFTNESS is unused"); + return; + } + } + ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); + ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); + rapier2d::joint_change_revolute_params(space_handle, handle, angular_limit_lower, angular_limit_upper, angular_limit_enabled, motor_target_velocity, motor_enabled); +} + +real_t RapierPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const { + switch (p_param) { + case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: { + return angular_limit_upper; + } + case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: { + return angular_limit_lower; + } + case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: { + return motor_target_velocity; + } + case PhysicsServer2D::PIN_JOINT_SOFTNESS: { + WARN_PRINT_ONCE("PIN_JOINT_SOFTNESS is unused"); + return 0.0; + } + } + ERR_FAIL_V(0); +} + +void RapierPinJoint2D::set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled) { + switch (p_flag) { + case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: { + angular_limit_enabled = p_enabled; + } break; + case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: { + motor_enabled = p_enabled; + } break; + } + ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); + ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); + rapier2d::joint_change_revolute_params(space_handle, handle, angular_limit_lower, angular_limit_upper, angular_limit_enabled, motor_target_velocity, motor_enabled); +} + +bool RapierPinJoint2D::get_flag(PhysicsServer2D::PinJointFlag p_flag) const { + switch (p_flag) { + case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: { + return angular_limit_enabled; + } + case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: { + return motor_enabled; + } + } + ERR_FAIL_V(0); +} + +RapierPinJoint2D::RapierPinJoint2D(const Vector2 &p_pos, RapierBody2D *p_body_a, RapierBody2D *p_body_b) : + RapierJoint2D(p_body_a, p_body_b) { + Vector2 anchor_A = p_body_a->get_inv_transform().xform(p_pos); + Vector2 anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; + + rapier2d::Vector rapier_anchor_A = { anchor_A.x, anchor_A.y }; + rapier2d::Vector rapier_anchor_B = { anchor_B.x, anchor_B.y }; + + ERR_FAIL_COND(!p_body_a->get_space()); + ERR_FAIL_COND(p_body_a->get_space() != p_body_b->get_space()); + space_handle = p_body_a->get_space()->get_handle(); + ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); + handle = rapier2d::joint_create_revolute(space_handle, p_body_a->get_body_handle(), p_body_b->get_body_handle(), &rapier_anchor_A, &rapier_anchor_B, angular_limit_lower, angular_limit_upper, angular_limit_enabled, motor_target_velocity, motor_enabled); + ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); +} diff --git a/src/joints/rapier_pin_joint_2d.h b/src/joints/rapier_pin_joint_2d.h new file mode 100644 index 0000000..eceafa6 --- /dev/null +++ b/src/joints/rapier_pin_joint_2d.h @@ -0,0 +1,27 @@ +#ifndef RAPIER_PIN_JOINT_2D_H +#define RAPIER_PIN_JOINT_2D_H + +#include "rapier_joint_2d.h" + +using namespace godot; + +class RapierPinJoint2D : public RapierJoint2D { + real_t angular_limit_lower = 0.0; + real_t angular_limit_upper = 0.0; + real_t motor_target_velocity = 0.0; + bool motor_enabled = false; + bool angular_limit_enabled = false; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } + + void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::PinJointParam p_param) const; + + void set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled); + bool get_flag(PhysicsServer2D::PinJointFlag p_flag) const; + + RapierPinJoint2D(const Vector2 &p_pos, RapierBody2D *p_body_a, RapierBody2D *p_body_b = nullptr); +}; + +#endif // RAPIER_PIN_JOINT_2D_H diff --git a/src/rapier2d-wrapper/Cargo.toml b/src/rapier2d-wrapper/Cargo.toml index 4986de5..b070032 100644 --- a/src/rapier2d-wrapper/Cargo.toml +++ b/src/rapier2d-wrapper/Cargo.toml @@ -9,12 +9,20 @@ edition = "2021" name = "rapier2d_wrapper" crate-type = ["staticlib"] # Creates static lib +[features] +f64 = [] +enhanced-determinism = ["rapier2d/enhanced-determinism", "rapier2d-f64/enhanced-determinism"] +simd-stable = ["rapier2d/simd-stable", "rapier2d-f64/simd-stable"] +simd-nightly = ["rapier2d/simd-nightly", "rapier2d-f64/simd-nightly"] +parallel = ["rapier2d/parallel", "rapier2d-f64/parallel"] + [dependencies] # Revert this when changes to parry enter mainstream # rapier2d = { features = [ "simd-stable"] } # This changes the parry contacts to work even in some weird cases where in main branch it would return None -rapier2d = {git = "https://github.com/appsinacup/rapier", branch = "possible-fix-for-contacts", features = ["simd-stable"]} +rapier2d = {git = "https://github.com/appsinacup/rapier", branch = "possible-fix-for-contacts"} +rapier2d-f64 = {git = "https://github.com/appsinacup/rapier", branch = "possible-fix-for-contacts"} lazy_static = "1.4.0" [profile.release] diff --git a/src/rapier2d-wrapper/cbindgen.toml b/src/rapier2d-wrapper/cbindgen.toml index 9976714..654c121 100644 --- a/src/rapier2d-wrapper/cbindgen.toml +++ b/src/rapier2d-wrapper/cbindgen.toml @@ -14,10 +14,10 @@ language = "C++" # header = "/* Text to put at the beginning of the generated file. Probably a license. */" # trailer = "/* Text to put at the end of the generated file */" -# include_guard = "my_bindings_h" -pragma_once = true +include_guard = "RAPIER_WRAPPER_H" +#pragma_once = true # autogen_warning = "/* Warning, this file is autogenerated by cbindgen. Don't modify this manually. */" -include_version = false +include_version = true namespace = "rapier2d" namespaces = [] using_namespaces = [] @@ -53,8 +53,10 @@ usize_is_size_t = true [defines] # "target_os = freebsd" = "DEFINE_FREEBSD" # "feature = serde" = "DEFINE_SERDE" - - +"feature = f32" = "REAL_T_IS_FLOAT" +"feature = f64" = "REAL_T_IS_DOUBLE" +# Not needed +"feature = spin_no_std" = "DEFINE_SPIN_NO_STD" [export] include = [] @@ -137,7 +139,7 @@ bitflags = false ############## Options for How Your Rust library Should Be Parsed ############## [parse] -parse_deps = false +parse_deps = true # include = [] exclude = [] clean = false @@ -149,4 +151,4 @@ extra_bindings = [] crates = [] all_features = false default_features = true -features = [] \ No newline at end of file +features = [] diff --git a/src/rapier2d-wrapper/includes/rapier2d_wrapper.h b/src/rapier2d-wrapper/includes/rapier2d_wrapper.h index 28f924c..d832b6c 100644 --- a/src/rapier2d-wrapper/includes/rapier2d_wrapper.h +++ b/src/rapier2d-wrapper/includes/rapier2d_wrapper.h @@ -1,15 +1,51 @@ -#pragma once +#ifndef RAPIER_WRAPPER_H +#define RAPIER_WRAPPER_H + +/* Generated with cbindgen:0.26.0 */ namespace rapier2d { +#if !defined(DEFINE_SPIN_NO_STD) +template +struct Lazy; +#endif + +#if defined(DEFINE_SPIN_NO_STD) +template +struct Lazy; +#endif + +/// A feature id where the feature type is packed into the same value as the feature index. +struct PackedFeatureId; + struct Handle { uint32_t id; uint32_t generation; }; +#if defined(REAL_T_IS_DOUBLE) +/// The scalar type used throughout this crate. +using Real = double; +#endif + +#if defined(REAL_T_IS_FLOAT) +/// The scalar type used throughout this crate. +using Real = float; +#endif + +#if defined(REAL_T_IS_DOUBLE) +/// The scalar type used throughout this crate. +using Real = double; +#endif + +#if defined(REAL_T_IS_FLOAT) +/// The scalar type used throughout this crate. +using Real = float; +#endif + struct Vector { - float x; - float y; + Real x; + Real y; }; struct UserData { @@ -18,67 +54,23 @@ struct UserData { }; struct Material { - float friction; - float restitution; + Real friction; + Real restitution; }; -struct SimulationSettings { - /// The timestep length (default: `1.0 / 60.0`) - float dt; - /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`) - /// - /// When CCD with multiple substeps is enabled, the timestep is subdivided - /// into smaller pieces. This timestep subdivision won't generate timestep - /// lengths smaller than `min_ccd_dt`. - /// - /// Setting this to a large value will reduce the opportunity to performing - /// CCD substepping, resulting in potentially more time dropped by the - /// motion-clamping mechanism. Setting this to an very small value may lead - /// to numerical instabilities. - float min_ccd_dt; - /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) - /// will be compensated for during the velocity solve. - /// (default `0.8`). - float erp; - /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. - /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations - /// before stabilization). - /// (default `0.25`). - float damping_ratio; - /// 0-1: multiplier for how much of the joint violation - /// will be compensated for during the velocity solve. - /// (default `1.0`). - float joint_erp; - /// The fraction of critical damping applied to the joint for constraints regularization. - /// (default `0.25`). - float joint_damping_ratio; - /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). - float allowed_linear_error; - /// Maximum amount of penetration the solver will attempt to resolve in one timestep. - float max_penetration_correction; - /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). - float prediction_distance; - /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). - size_t max_velocity_iterations; - /// Maximum number of iterations performed to solve friction constraints (default: `8`). - size_t max_velocity_friction_iterations; - /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). - size_t max_stabilization_iterations; - /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, - /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). - bool interleave_restitution_and_friction_resolution; - /// Minimum number of dynamic bodies in each active island (default: `128`). - size_t min_island_size; - /// Maximum number of substeps performed by the solver (default: `1`). - size_t max_ccd_substeps; - Vector gravity; +struct QueryExcludedInfo { + uint32_t query_collision_layer_mask; + uint64_t query_canvas_instance_id; + Handle *query_exclude; + uint32_t query_exclude_size; + int64_t query_exclude_body; }; struct WorldSettings { - float sleep_linear_threshold; - float sleep_angular_threshold; - float sleep_time_until_sleep; - float solver_prediction_distance; + Real sleep_linear_threshold; + Real sleep_angular_threshold; + Real sleep_time_until_sleep; + Real solver_prediction_distance; }; struct PointHitInfo { @@ -88,7 +80,8 @@ struct PointHitInfo { using QueryHandleExcludedCallback = bool (*)(Handle world_handle, Handle collider_handle, - const UserData *user_data); + const UserData *user_data, + const QueryExcludedInfo *handle_excluded_info); struct RayHitInfo { Vector position; @@ -99,7 +92,7 @@ struct RayHitInfo { struct ShapeCastResult { bool collided; - float toi; + Real toi; Vector witness1; Vector witness2; Vector normal1; @@ -111,13 +104,13 @@ struct ShapeCastResult { struct ShapeInfo { Handle handle; Vector position; - float rotation; + Real rotation; }; struct ContactResult { bool collided; bool within_margin; - float distance; + Real distance; Vector point1; Vector point2; Vector normal1; @@ -166,12 +159,66 @@ struct ContactPointInfo { Vector velocity_pos_1; Vector velocity_pos_2; Vector normal; - float distance; - float impulse; - float tangent_impulse; + Real distance; + Real impulse; + Real tangent_impulse; }; -using ContactPointCallback = bool (*)(Handle world_handle, const ContactPointInfo *contact_info); +using ContactPointCallback = bool (*)(Handle world_handle, + const ContactPointInfo *contact_info, + const ContactForceEventInfo *event_info); + +struct SimulationSettings { + /// The timestep length (default: `1.0 / 60.0`) + Real dt; + /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`) + /// + /// When CCD with multiple substeps is enabled, the timestep is subdivided + /// into smaller pieces. This timestep subdivision won't generate timestep + /// lengths smaller than `min_ccd_dt`. + /// + /// Setting this to a large value will reduce the opportunity to performing + /// CCD substepping, resulting in potentially more time dropped by the + /// motion-clamping mechanism. Setting this to an very small value may lead + /// to numerical instabilities. + Real min_ccd_dt; + /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) + /// will be compensated for during the velocity solve. + /// (default `0.8`). + Real erp; + /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. + /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations + /// before stabilization). + /// (default `0.25`). + Real damping_ratio; + /// 0-1: multiplier for how much of the joint violation + /// will be compensated for during the velocity solve. + /// (default `1.0`). + Real joint_erp; + /// The fraction of critical damping applied to the joint for constraints regularization. + /// (default `0.25`). + Real joint_damping_ratio; + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). + Real allowed_linear_error; + /// Maximum amount of penetration the solver will attempt to resolve in one timestep. + Real max_penetration_correction; + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). + Real prediction_distance; + /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). + size_t max_velocity_iterations; + /// Maximum number of iterations performed to solve friction constraints (default: `8`). + size_t max_velocity_friction_iterations; + /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). + size_t max_stabilization_iterations; + /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, + /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). + bool interleave_restitution_and_friction_resolution; + /// Minimum number of dynamic bodies in each active island (default: `128`). + size_t min_island_size; + /// Maximum number of substeps performed by the solver (default: `1`). + size_t max_ccd_substeps; + Vector gravity; +}; extern "C" { @@ -184,7 +231,7 @@ void body_add_force_at_point(Handle world_handle, const Vector *force, const Vector *point); -void body_add_torque(Handle world_handle, Handle body_handle, float torque); +void body_add_torque(Handle world_handle, Handle body_handle, Real torque); void body_apply_impulse(Handle world_handle, Handle body_handle, const Vector *impulse); @@ -193,29 +240,29 @@ void body_apply_impulse_at_point(Handle world_handle, const Vector *impulse, const Vector *point); -void body_apply_torque_impulse(Handle world_handle, Handle body_handle, float torque_impulse); +void body_apply_torque_impulse(Handle world_handle, Handle body_handle, Real torque_impulse); Handle body_create_dynamic(Handle world_handle, const Vector *pos, - float rot, + Real rot, const UserData *user_data); Handle body_create_fixed(Handle world_handle, const Vector *pos, - float rot, + Real rot, const UserData *user_data); void body_destroy(Handle world_handle, Handle body_handle); void body_force_sleep(Handle world_handle, Handle body_handle); -float body_get_angle(Handle world_handle, Handle body_handle); +Real body_get_angle(Handle world_handle, Handle body_handle); -float body_get_angular_velocity(Handle world_handle, Handle body_handle); +Real body_get_angular_velocity(Handle world_handle, Handle body_handle); Vector body_get_constant_force(Handle world_handle, Handle body_handle); -float body_get_constant_torque(Handle world_handle, Handle body_handle); +Real body_get_constant_torque(Handle world_handle, Handle body_handle); Vector body_get_linear_velocity(Handle world_handle, Handle body_handle); @@ -227,9 +274,9 @@ void body_reset_forces(Handle world_handle, Handle body_handle); void body_reset_torques(Handle world_handle, Handle body_handle); -void body_set_angular_damping(Handle world_handle, Handle body_handle, float angular_damping); +void body_set_angular_damping(Handle world_handle, Handle body_handle, Real angular_damping); -void body_set_angular_velocity(Handle world_handle, Handle body_handle, float vel); +void body_set_angular_velocity(Handle world_handle, Handle body_handle, Real vel); void body_set_can_sleep(Handle world_handle, Handle body_handle, bool can_sleep); @@ -237,17 +284,17 @@ void body_set_ccd_enabled(Handle world_handle, Handle body_handle, bool enable); void body_set_gravity_scale(Handle world_handle, Handle body_handle, - float gravity_scale, + Real gravity_scale, bool wake_up); -void body_set_linear_damping(Handle world_handle, Handle body_handle, float linear_damping); +void body_set_linear_damping(Handle world_handle, Handle body_handle, Real linear_damping); void body_set_linear_velocity(Handle world_handle, Handle body_handle, const Vector *vel); void body_set_mass_properties(Handle world_handle, Handle body_handle, - float mass, - float inertia, + Real mass, + Real inertia, const Vector *local_com, bool wake_up, bool force_update); @@ -255,7 +302,7 @@ void body_set_mass_properties(Handle world_handle, void body_set_transform(Handle world_handle, Handle body_handle, const Vector *pos, - float rot, + Real rot, bool wake_up); void body_update_material(Handle world_handle, Handle body_handle, const Material *mat); @@ -275,7 +322,7 @@ Handle collider_create_solid(Handle world_handle, void collider_destroy(Handle world_handle, Handle handle); -float collider_get_angle(Handle world_handle, Handle handle); +Real collider_get_angle(Handle world_handle, Handle handle); Vector collider_get_position(Handle world_handle, Handle handle); @@ -283,11 +330,11 @@ void collider_set_collision_events_enabled(Handle world_handle, Handle handle, b void collider_set_contact_force_events_enabled(Handle world_handle, Handle handle, bool enable); -void collider_set_transform(Handle world_handle, Handle handle, const Vector *pos, float rot); +void collider_set_transform(Handle world_handle, Handle handle, const Vector *pos, Real rot); Material default_material(); -SimulationSettings default_simulation_settings(); +QueryExcludedInfo default_query_excluded_info(); WorldSettings default_world_settings(); @@ -298,7 +345,8 @@ size_t intersect_aabb(Handle world_handle, bool collide_with_area, PointHitInfo *hit_info_array, size_t hit_info_length, - QueryHandleExcludedCallback handle_excluded_callback); + QueryHandleExcludedCallback handle_excluded_callback, + const QueryExcludedInfo *handle_excluded_info); size_t intersect_point(Handle world_handle, const Vector *position, @@ -306,27 +354,30 @@ size_t intersect_point(Handle world_handle, bool collide_with_area, PointHitInfo *hit_info_array, size_t hit_info_length, - QueryHandleExcludedCallback handle_excluded_callback); + QueryHandleExcludedCallback handle_excluded_callback, + const QueryExcludedInfo *handle_excluded_info); bool intersect_ray(Handle world_handle, const Vector *from, const Vector *dir, - float length, + Real length, bool collide_with_body, bool collide_with_area, bool hit_from_inside, RayHitInfo *hit_info, - QueryHandleExcludedCallback handle_excluded_callback); + QueryHandleExcludedCallback handle_excluded_callback, + const QueryExcludedInfo *handle_excluded_info); size_t intersect_shape(Handle world_handle, const Vector *position, - float rotation, + Real rotation, Handle shape_handle, bool collide_with_body, bool collide_with_area, PointHitInfo *hit_info_array, size_t hit_info_length, - QueryHandleExcludedCallback handle_excluded_callback); + QueryHandleExcludedCallback handle_excluded_callback, + const QueryExcludedInfo *handle_excluded_info); Handle invalid_handle(); @@ -338,10 +389,10 @@ bool is_user_data_valid(UserData user_data); void joint_change_revolute_params(Handle world_handle, Handle joint_handle, - float angular_limit_lower, - float angular_limit_upper, + Real angular_limit_lower, + Real angular_limit_upper, bool angular_limit_enabled, - float motor_target_velocity, + Real motor_target_velocity, bool motor_enabled); Handle joint_create_prismatic(Handle world_handle, @@ -357,10 +408,10 @@ Handle joint_create_revolute(Handle world_handle, Handle body_handle_2, const Vector *anchor_1, const Vector *anchor_2, - float angular_limit_lower, - float angular_limit_upper, + Real angular_limit_lower, + Real angular_limit_upper, bool angular_limit_enabled, - float motor_target_velocity, + Real motor_target_velocity, bool motor_enabled); void joint_destroy(Handle world_handle, Handle joint_handle); @@ -368,17 +419,18 @@ void joint_destroy(Handle world_handle, Handle joint_handle); ShapeCastResult shape_casting(Handle world_handle, const Vector *motion, const Vector *position, - float rotation, + Real rotation, Handle shape_handle, bool collide_with_body, bool collide_with_area, - QueryHandleExcludedCallback handle_excluded_callback); + QueryHandleExcludedCallback handle_excluded_callback, + const QueryExcludedInfo *handle_excluded_info); Handle shape_create_box(const Vector *size); -Handle shape_create_capsule(float half_height, float radius); +Handle shape_create_capsule(Real half_height, Real radius); -Handle shape_create_circle(float radius); +Handle shape_create_circle(Real radius); Handle shape_create_compound(const ShapeInfo *shapes, size_t shape_count); @@ -393,11 +445,11 @@ void shape_destroy(Handle shape_handle); ContactResult shapes_contact(Handle world_handle, Handle shape_handle1, const Vector *position1, - float rotation1, + Real rotation1, Handle shape_handle2, const Vector *position2, - float rotation2, - float margin); + Real rotation2, + Real margin); Handle world_create(const WorldSettings *settings); @@ -423,3 +475,5 @@ void world_step(Handle world_handle, const SimulationSettings *settings); } // extern "C" } // namespace rapier2d + +#endif // RAPIER_WRAPPER_H diff --git a/src/rapier2d-wrapper/src/body.rs b/src/rapier2d-wrapper/src/body.rs new file mode 100644 index 0000000..429e4b2 --- /dev/null +++ b/src/rapier2d-wrapper/src/body.rs @@ -0,0 +1,349 @@ +use rapier2d::prelude::*; +use crate::handle::*; +use crate::vector::Vector; +use crate::user_data::UserData; +use crate::physics_world::*; +use crate::collider::*; + +fn set_rigid_body_properties_internal(rigid_body : &mut RigidBody, pos : &Vector, rot : Real, wake_up : bool) { + if rigid_body.is_dynamic() { + rigid_body.set_rotation(Rotation::new(rot), wake_up); + rigid_body.set_translation(vector![pos.x, pos.y], wake_up); + } else { + rigid_body.set_next_kinematic_position(Isometry::new(vector![pos.x, pos.y], rot)); + } +} + +#[no_mangle] +pub extern "C" fn body_create_fixed(world_handle : Handle, pos : &Vector, rot : Real, user_data : &UserData) -> Handle { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let mut rigid_body = RigidBodyBuilder::kinematic_position_based().build(); + set_rigid_body_properties_internal(&mut rigid_body, pos, rot, true); + rigid_body.user_data = user_data.get_data(); + let body_handle = physics_world.rigid_body_set.insert(rigid_body); + return rigid_body_handle_to_handle(body_handle); +} + +#[no_mangle] +pub extern "C" fn body_create_dynamic(world_handle : Handle, pos : &Vector, rot : Real, user_data : &UserData) -> Handle { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let mut rigid_body = RigidBodyBuilder::dynamic().can_sleep(true).sleeping(true).build(); + let activation = rigid_body.activation_mut(); + activation.time_until_sleep = physics_world.sleep_time_until_sleep; + activation.linear_threshold = physics_world.sleep_linear_threshold; + activation.angular_threshold = physics_world.sleep_angular_threshold; + set_rigid_body_properties_internal(&mut rigid_body, pos, rot, false); + rigid_body.user_data = user_data.get_data(); + return physics_world.insert_rigid_body(rigid_body); +} + +#[no_mangle] +pub extern "C" fn body_destroy(world_handle : Handle, body_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.remove_rigid_body(body_handle); +} + +#[no_mangle] +pub extern "C" fn body_get_position(world_handle : Handle, body_handle : Handle) -> Vector { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get(rigid_body_handle); + assert!(body.is_some()); + let body_vector = body.unwrap().translation(); + return Vector { x : body_vector.x, y : body_vector.y }; +} + +#[no_mangle] +pub extern "C" fn body_get_angle(world_handle : Handle, body_handle : Handle) -> Real { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get(rigid_body_handle); + assert!(body.is_some()); + return body.unwrap().rotation().angle(); +} + +#[no_mangle] +pub extern "C" fn body_set_transform(world_handle : Handle, body_handle : Handle, pos : &Vector, rot : Real, wake_up : bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + let body = body.unwrap(); + set_rigid_body_properties_internal(body, pos, rot, wake_up); +} + +#[no_mangle] +pub extern "C" fn body_get_linear_velocity(world_handle : Handle, body_handle : Handle) -> Vector { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get(rigid_body_handle); + assert!(body.is_some()); + let body_vel = body.unwrap().linvel(); + return Vector { x : body_vel.x, y : body_vel.y }; +} + +#[no_mangle] +pub extern "C" fn body_set_linear_velocity(world_handle : Handle, body_handle : Handle, vel : &Vector) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().set_linvel(vector![vel.x, vel.y], true); +} + +#[no_mangle] +pub extern "C" fn body_update_material(world_handle : Handle, body_handle : Handle, mat : &Material) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + for collider in body.unwrap().colliders() { + if let Some(col) = physics_world.collider_set.get_mut(*collider) { + col.set_friction(mat.friction); + col.set_restitution(mat.restitution) + } + } +} + +#[no_mangle] +pub extern "C" fn body_get_angular_velocity(world_handle : Handle, body_handle : Handle) -> Real { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get(rigid_body_handle); + assert!(body.is_some()); + return body.unwrap().angvel(); +} + +#[no_mangle] +pub extern "C" fn body_set_angular_velocity(world_handle : Handle, body_handle : Handle, vel : Real) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().set_angvel(vel, true); +} + +#[no_mangle] +pub extern "C" fn body_set_linear_damping(world_handle : Handle, body_handle : Handle, linear_damping : Real) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().set_linear_damping(linear_damping); +} + +#[no_mangle] +pub extern "C" fn body_set_angular_damping(world_handle : Handle, body_handle : Handle, angular_damping : Real) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().set_angular_damping(angular_damping); +} + +#[no_mangle] +pub extern "C" fn body_set_gravity_scale(world_handle : Handle, body_handle : Handle, gravity_scale : Real, wake_up : bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().set_gravity_scale(gravity_scale, wake_up); +} + +#[no_mangle] +pub extern "C" fn body_set_can_sleep(world_handle : Handle, body_handle : Handle, can_sleep: bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + let body = body.unwrap(); + + if can_sleep && body.activation().angular_threshold == -1.0 { + let activation = body.activation_mut(); + activation.angular_threshold = physics_world.sleep_angular_threshold; + activation.linear_threshold = physics_world.sleep_linear_threshold; + } else if !can_sleep && body.activation().angular_threshold != -1.0 { + let activation = body.activation_mut(); + activation.angular_threshold = -1.0; + activation.linear_threshold = -1.0; + } + + // TODO: Check if is requiered + if !can_sleep && body.is_sleeping() { + body.wake_up(true); + } +} + +#[no_mangle] +pub extern "C" fn body_is_ccd_enabled(world_handle : Handle, body_handle : Handle) -> bool { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().is_ccd_enabled() +} + +#[no_mangle] +pub extern "C" fn body_set_ccd_enabled(world_handle : Handle, body_handle : Handle, enable: bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().enable_ccd(enable); +} + +#[no_mangle] +pub extern "C" fn body_set_mass_properties(world_handle : Handle, body_handle : Handle, mass : Real, inertia : Real, local_com : &Vector, wake_up : bool, force_update : bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + let body_ref = body.unwrap(); + body_ref.set_additional_mass_properties(MassProperties::new(point![local_com.x, local_com.y], mass, inertia), wake_up); + if force_update { + body_ref.recompute_mass_properties_from_colliders(&physics_world.collider_set); + } +} + +#[no_mangle] +pub extern "C" fn body_add_force(world_handle : Handle, body_handle : Handle, force : &Vector) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().add_force(vector!(force.x, force.y), true); +} + +#[no_mangle] +pub extern "C" fn body_add_force_at_point(world_handle : Handle, body_handle : Handle, force : &Vector, point : &Vector) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().add_force_at_point(vector!(force.x, force.y),point![point.x, point.y] , true); +} + +#[no_mangle] +pub extern "C" fn body_add_torque(world_handle : Handle, body_handle : Handle, torque: Real) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().add_torque(torque, true); +} + +#[no_mangle] +pub extern "C" fn body_apply_impulse(world_handle : Handle, body_handle : Handle, impulse : &Vector) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().apply_impulse(vector!(impulse.x, impulse.y), true); +} + +#[no_mangle] +pub extern "C" fn body_apply_impulse_at_point(world_handle : Handle, body_handle : Handle, impulse : &Vector, point : &Vector) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().apply_impulse_at_point(vector!(impulse.x, impulse.y),point![point.x, point.y] , true); +} + +#[no_mangle] +pub extern "C" fn body_get_constant_force(world_handle : Handle, body_handle : Handle) -> Vector { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + let constant_force = body.unwrap().user_force(); + return Vector { x : constant_force.x, y : constant_force.y }; +} + +#[no_mangle] +pub extern "C" fn body_get_constant_torque(world_handle : Handle, body_handle : Handle) -> Real { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().user_torque() +} + +#[no_mangle] +pub extern "C" fn body_apply_torque_impulse(world_handle : Handle, body_handle : Handle, torque_impulse : Real) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().apply_torque_impulse(torque_impulse, true); +} + +#[no_mangle] +pub extern "C" fn body_reset_torques(world_handle : Handle, body_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().reset_torques(false); +} + +#[no_mangle] +pub extern "C" fn body_reset_forces(world_handle : Handle, body_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().reset_forces(false); +} + +#[no_mangle] +pub extern "C" fn body_wake_up(world_handle : Handle, body_handle : Handle, strong : bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + // TODO: check if this is OK, at the startup call to RapierPhysicsServer2D::free where body is not some + assert!(body.is_some()); + let body = body.unwrap(); + if body.is_sleeping() { + body.wake_up(strong); + } +} + +#[no_mangle] +pub extern "C" fn body_force_sleep(world_handle : Handle, body_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); + assert!(body.is_some()); + body.unwrap().sleep(); +} diff --git a/src/rapier2d-wrapper/src/collider.rs b/src/rapier2d-wrapper/src/collider.rs new file mode 100644 index 0000000..87d4921 --- /dev/null +++ b/src/rapier2d-wrapper/src/collider.rs @@ -0,0 +1,123 @@ +use rapier2d::prelude::*; +use crate::handle::*; +use crate::user_data::*; +use crate::vector::Vector; +use crate::physics_world::*; + +#[repr(C)] +pub struct Material { + pub friction : Real, + pub restitution : Real, +} + +#[no_mangle] +pub extern "C" fn default_material() -> Material { + Material { + friction : 1.0, + restitution : 0.0, + } +} + +#[no_mangle] +pub extern "C" fn collider_create_solid(world_handle : Handle, shape_handle : Handle, mat : &Material, body_handle : Handle, user_data : &UserData) -> Handle { + let mut physics_engine = SINGLETON.lock().unwrap(); + let shape = physics_engine.get_shape(shape_handle); + let mut collider = ColliderBuilder::new(shape.clone()).build(); + collider.set_friction(mat.friction); + collider.set_restitution(mat.restitution); + collider.set_friction_combine_rule(CoefficientCombineRule::Multiply); + collider.set_restitution_combine_rule(CoefficientCombineRule::Max); + collider.set_density(0.0); + collider.user_data = user_data.get_data(); + collider.set_active_hooks(ActiveHooks::FILTER_CONTACT_PAIRS | ActiveHooks::FILTER_INTERSECTION_PAIR | ActiveHooks::MODIFY_SOLVER_CONTACTS); + let physics_world = physics_engine.get_world(world_handle); + return physics_world.insert_collider(collider, body_handle); +} + +#[no_mangle] +pub extern "C" fn collider_create_sensor(world_handle : Handle, shape_handle : Handle, body_handle : Handle, user_data : &UserData) -> Handle { + let mut physics_engine = SINGLETON.lock().unwrap(); + let shape = physics_engine.get_shape(shape_handle); + let mut collider = ColliderBuilder::new(shape.clone()).build(); + collider.set_sensor(true); + collider.set_active_events(ActiveEvents::COLLISION_EVENTS); + let mut collision_types = collider.active_collision_types(); + collision_types |= ActiveCollisionTypes::KINEMATIC_KINEMATIC; + collider.set_active_collision_types(collision_types); + collider.user_data = user_data.get_data(); + collider.set_active_hooks(ActiveHooks::FILTER_INTERSECTION_PAIR); + let physics_world = physics_engine.get_world(world_handle); + return physics_world.insert_collider(collider, body_handle); +} + +#[no_mangle] +pub extern "C" fn collider_destroy(world_handle : Handle, handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + return physics_world.remove_collider(handle); +} + +#[no_mangle] +pub extern "C" fn collider_get_position(world_handle : Handle, handle : Handle) -> Vector { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let collider_handle = handle_to_collider_handle(handle); + let collider = physics_world.collider_set.get(collider_handle); + assert!(collider.is_some()); + let collider_vector = collider.unwrap().translation(); + return Vector { x : collider_vector.x, y : collider_vector.y }; +} + +#[no_mangle] +pub extern "C" fn collider_get_angle(world_handle : Handle, handle : Handle) -> Real { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let collider_handle = handle_to_collider_handle(handle); + let collider = physics_world.collider_set.get(collider_handle); + assert!(collider.is_some()); + return collider.unwrap().rotation().angle(); +} + +#[no_mangle] +pub extern "C" fn collider_set_transform(world_handle : Handle, handle : Handle, pos : &Vector, rot : Real) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let collider_handle = handle_to_collider_handle(handle); + let collider = physics_world.collider_set.get_mut(collider_handle); + assert!(collider.is_some()); + collider.unwrap().set_position_wrt_parent(Isometry::new(vector![pos.x, pos.y], rot)); +} + +#[no_mangle] +pub extern "C" fn collider_set_collision_events_enabled(world_handle : Handle, handle : Handle, enable : bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let collider_handle = handle_to_collider_handle(handle); + let collider = physics_world.collider_set.get_mut(collider_handle); + assert!(collider.is_some()); + let collider_access = collider.unwrap(); + let mut active_events = collider_access.active_events(); + if enable { + active_events |= ActiveEvents::COLLISION_EVENTS; + } else { + active_events &= !ActiveEvents::COLLISION_EVENTS; + } + collider_access.set_active_events(active_events); +} + +#[no_mangle] +pub extern "C" fn collider_set_contact_force_events_enabled(world_handle : Handle, handle : Handle, enable : bool) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + let collider_handle = handle_to_collider_handle(handle); + let collider = physics_world.collider_set.get_mut(collider_handle); + assert!(collider.is_some()); + let collider_access = collider.unwrap(); + let mut active_events = collider_access.active_events(); + if enable { + active_events |= ActiveEvents::CONTACT_FORCE_EVENTS; + } else { + active_events &= !ActiveEvents::CONTACT_FORCE_EVENTS; + } + collider_access.set_active_events(active_events); +} diff --git a/src/rapier2d-wrapper/src/handle.rs b/src/rapier2d-wrapper/src/handle.rs new file mode 100644 index 0000000..809a2c3 --- /dev/null +++ b/src/rapier2d-wrapper/src/handle.rs @@ -0,0 +1,107 @@ +use rapier2d::data::Index; +use rapier2d::prelude::*; + +#[repr(C)] +#[derive(Copy, Clone, Eq, Hash, PartialEq)] +pub struct Handle { + id : u32, + generation : u32, +} + +impl Default for Handle { + fn default() -> Handle { + Handle { + id : u32::MAX, + generation : u32::MAX, + } + } +} + +impl Handle { + pub fn is_valid(&self) -> bool { + return (self.id != u32::MAX) && (self.generation != u32::MAX); + } +} + + +pub fn world_handle_to_handle(world_handle : Index) -> Handle { + let raw_parts = world_handle.into_raw_parts(); + return Handle { + id : raw_parts.0, + generation : raw_parts.1, + } +} + + +pub fn handle_to_world_handle(handle : Handle) -> Index { + return Index::from_raw_parts(handle.id, handle.generation); +} + +pub fn shape_handle_to_handle(shape_handle : Index) -> Handle { + let raw_parts = shape_handle.into_raw_parts(); + return Handle { + id : raw_parts.0, + generation : raw_parts.1, + } +} + + +pub fn handle_to_shape_handle(handle : Handle) -> Index { + return Index::from_raw_parts(handle.id, handle.generation); +} + +pub fn collider_handle_to_handle(collider_handle : ColliderHandle) -> Handle { + let raw_parts = collider_handle.into_raw_parts(); + return Handle { + id : raw_parts.0, + generation : raw_parts.1, + } +} + +pub fn handle_to_collider_handle(handle : Handle) -> ColliderHandle { + return ColliderHandle::from_raw_parts(handle.id, handle.generation); +} + +pub fn rigid_body_handle_to_handle(rigid_body_handle : RigidBodyHandle) -> Handle { + let raw_parts = rigid_body_handle.into_raw_parts(); + return Handle { + id : raw_parts.0, + generation : raw_parts.1, + } +} + +pub fn handle_to_rigid_body_handle(handle : Handle) -> RigidBodyHandle { + return RigidBodyHandle::from_raw_parts(handle.id, handle.generation); +} + +pub fn joint_handle_to_handle(joint_handle : ImpulseJointHandle) -> Handle { + let raw_parts = joint_handle.into_raw_parts(); + return Handle { + id : raw_parts.0, + generation : raw_parts.1, + } +} + +pub fn handle_to_joint_handle(handle : Handle) -> ImpulseJointHandle { + return ImpulseJointHandle::from_raw_parts(handle.id, handle.generation); +} + + + +#[no_mangle] +pub extern "C" fn invalid_handle() -> Handle { + Handle { + id : u32::MAX, + generation : u32::MAX, + } +} + +#[no_mangle] +pub extern "C" fn is_handle_valid(handle : Handle) -> bool { + return handle.is_valid(); +} + +#[no_mangle] +pub extern "C" fn are_handles_equal(handle1 : Handle, handle2 : Handle) -> bool { + return (handle1.id == handle2.id) && (handle1.generation == handle2.generation); +} diff --git a/src/rapier2d-wrapper/src/joint.rs b/src/rapier2d-wrapper/src/joint.rs new file mode 100644 index 0000000..1571435 --- /dev/null +++ b/src/rapier2d-wrapper/src/joint.rs @@ -0,0 +1,63 @@ +use rapier2d::prelude::*; +use crate::handle::*; +use crate::physics_world::*; +use crate::vector::Vector; + +#[no_mangle] +pub extern "C" fn joint_create_revolute(world_handle : Handle, body_handle_1 : Handle, body_handle_2 : Handle, anchor_1 : &Vector, anchor_2 : &Vector, angular_limit_lower: Real, angular_limit_upper: Real, angular_limit_enabled: bool, motor_target_velocity: Real, motor_enabled: bool) -> Handle { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + + let mut joint = RevoluteJointBuilder::new() + .local_anchor1(point!(anchor_1.x, anchor_1.y)) + .local_anchor2(point!(anchor_2.x, anchor_2.y)) + .motor_model(MotorModel::AccelerationBased); + if angular_limit_enabled { + joint = joint.limits([angular_limit_lower, angular_limit_upper]); + } + if motor_enabled { + joint = joint.motor_velocity(motor_target_velocity, 0.0); + } + + return physics_world.insert_joint(body_handle_1, body_handle_2, joint); +} + + +#[no_mangle] +pub extern "C" fn joint_change_revolute_params(world_handle : Handle, joint_handle: Handle , angular_limit_lower: Real, angular_limit_upper: Real, angular_limit_enabled: bool, motor_target_velocity: Real, motor_enabled: bool){ + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + + let joint = physics_world.impulse_joint_set.get_mut(handle_to_joint_handle(joint_handle)); + assert!(joint.is_some()); + let joint = joint.unwrap().data.as_revolute_mut(); + assert!(joint.is_some()); + let mut joint = joint.unwrap(); + if motor_enabled { + joint = joint.set_motor_velocity(motor_target_velocity, 0.0); + } + if angular_limit_enabled { + joint.set_limits([angular_limit_lower, angular_limit_upper]); + } +} + +#[no_mangle] +pub extern "C" fn joint_create_prismatic(world_handle : Handle, body_handle_1 : Handle, body_handle_2 : Handle, axis : &Vector, anchor_1 : &Vector, anchor_2 : &Vector, limits : &Vector) -> Handle { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + + let joint = PrismaticJointBuilder::new(UnitVector::new_normalize(vector![axis.x, axis.y])) + .local_anchor1(point!(anchor_1.x, anchor_1.y)) + .local_anchor2(point!(anchor_2.x, anchor_2.y)) + .limits([limits.x, limits.y]); + + return physics_world.insert_joint(body_handle_1, body_handle_2, joint); +} + +#[no_mangle] +pub extern "C" fn joint_destroy(world_handle : Handle, joint_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + + return physics_world.remove_joint(joint_handle); +} diff --git a/src/rapier2d-wrapper/src/lib.rs b/src/rapier2d-wrapper/src/lib.rs index ea878ca..bcbeab0 100644 --- a/src/rapier2d-wrapper/src/lib.rs +++ b/src/rapier2d-wrapper/src/lib.rs @@ -1,1890 +1,13 @@ -use rapier2d::crossbeam; -use rapier2d::data::Arena; -use rapier2d::data::Index; -use rapier2d::parry; -use rapier2d::prelude::*; - -// Handle - -#[repr(C)] -#[derive(Copy, Clone, Eq, Hash, PartialEq)] -pub struct Handle { - id : u32, - generation : u32, -} - -impl Default for Handle { - fn default() -> Handle { - Handle { - id : u32::MAX, - generation : u32::MAX, - } - } -} - -impl Handle { - pub fn is_valid(&self) -> bool { - return (self.id != u32::MAX) && (self.generation != u32::MAX); - } -} - -#[no_mangle] -pub extern "C" fn invalid_handle() -> Handle { - Handle { - id : u32::MAX, - generation : u32::MAX, - } -} - -#[no_mangle] -pub extern "C" fn is_handle_valid(handle : Handle) -> bool { - return handle.is_valid(); -} - -#[no_mangle] -pub extern "C" fn are_handles_equal(handle1 : Handle, handle2 : Handle) -> bool { - return (handle1.id == handle2.id) && (handle1.generation == handle2.generation); -} - -// User data - -#[repr(C)] -#[derive(Copy, Clone, Eq, Hash, PartialEq)] -pub struct UserData { - part1 : u64, - part2 : u64, -} - -impl UserData { - fn new(data : u128) -> UserData { - let data2 : u128 = data >> 64; - let data1 : u128 = data - (data2 << 64); - UserData { - part1 : data1.try_into().unwrap(), - part2 : data2.try_into().unwrap(), - } - } - - pub fn is_valid(&self) -> bool { - return (self.part1 != u64::MAX) && (self.part2 != u64::MAX); - } - - pub fn get_data(&self) -> u128 { - let data1 : u128 = self.part1.into(); - let data2 : u128 = self.part2.into(); - return data1 + (data2 << 64); - } -} - -#[no_mangle] -pub extern "C" fn invalid_user_data() -> UserData { - UserData { - part1 : u64::MAX, - part2 : u64::MAX, - } -} - -#[no_mangle] -pub extern "C" fn is_user_data_valid(user_data : UserData) -> bool { - return user_data.is_valid(); -} - -// Active body info - -#[repr(C)] -pub struct ActiveBodyInfo { - body_handle: Handle, - body_user_data: UserData, -} - -impl ActiveBodyInfo { - fn new() -> ActiveBodyInfo { - ActiveBodyInfo { - body_handle: invalid_handle(), - body_user_data: invalid_user_data(), - } - } -} - -// Collision filter info - -#[repr(C)] -pub struct CollisionFilterInfo { - user_data1: UserData, - user_data2: UserData, -} - -impl CollisionFilterInfo { - fn new() -> CollisionFilterInfo { - CollisionFilterInfo { - user_data1: invalid_user_data(), - user_data2: invalid_user_data(), - } - } -} - -// Events info - -#[repr(C)] -pub struct CollisionEventInfo { - collider1: Handle, - collider2: Handle, - user_data1: UserData, - user_data2: UserData, - is_sensor: bool, - is_started: bool, - is_removed: bool, -} - -impl CollisionEventInfo { - fn new() -> CollisionEventInfo { - CollisionEventInfo { - collider1: invalid_handle(), - collider2: invalid_handle(), - user_data1: invalid_user_data(), - user_data2: invalid_user_data(), - is_sensor: false, - is_started: false, - is_removed: false, - } - } -} - -#[repr(C)] -pub struct ContactForceEventInfo { - collider1: Handle, - collider2: Handle, - user_data1: UserData, - user_data2: UserData, -} - -impl ContactForceEventInfo { - fn new() -> ContactForceEventInfo { - ContactForceEventInfo { - collider1: invalid_handle(), - collider2: invalid_handle(), - user_data1: invalid_user_data(), - user_data2: invalid_user_data(), - } - } -} - -// Contact point info - -#[repr(C)] -pub struct ContactPointInfo { - local_pos_1: Vector, - local_pos_2: Vector, - velocity_pos_1: Vector, - velocity_pos_2: Vector, - normal: Vector, - distance: f32, - impulse: f32, - tangent_impulse: f32, -} - -impl ContactPointInfo { - fn new() -> ContactPointInfo { - ContactPointInfo { - local_pos_1: Vector { x : 0.0, y : 0.0 }, - local_pos_2: Vector { x : 0.0, y : 0.0 }, - velocity_pos_1: Vector { x : 0.0, y : 0.0 }, - velocity_pos_2: Vector { x : 0.0, y : 0.0 }, - normal: Vector { x : 0.0, y : 0.0 }, - distance: 0.0, - impulse: 0.0, - tangent_impulse: 0.0, - } - } -} - -// Query results - -#[repr(C)] -pub struct RayHitInfo { - position: Vector, - normal: Vector, - collider: Handle, - user_data: UserData, -} - -#[repr(C)] -pub struct PointHitInfo { - collider: Handle, - user_data: UserData, -} - -#[repr(C)] -pub struct ShapeCastResult { - collided: bool, - toi : f32, - witness1 : Vector, - witness2 : Vector, - normal1 : Vector, - normal2 : Vector, - collider: Handle, - user_data: UserData -} - -impl ShapeCastResult { - fn new() -> ShapeCastResult { - ShapeCastResult { - collided : false, - toi : 1.0, - collider : invalid_handle(), - witness1 : Vector{ x: 0.0, y: 0.0 }, - witness2 : Vector{ x: 0.0, y: 0.0 }, - normal1 : Vector{ x: 0.0, y: 0.0 }, - normal2 : Vector{ x: 0.0, y: 0.0 }, - user_data: UserData { part1: 0, part2: 0 } - } - } -} - -#[repr(C)] -pub struct ContactResult { - collided: bool, - within_margin: bool, - distance: f32, - point1 : Vector, - point2 : Vector, - normal1 : Vector, - normal2 : Vector -} - -impl ContactResult { - fn new() -> ContactResult { - ContactResult { - collided : false, - within_margin: false, - distance : 0.0, - point1 : Vector{ x: 0.0, y: 0.0 }, - point2 : Vector{ x: 0.0, y: 0.0 }, - normal1 : Vector{ x: 0.0, y: 0.0 }, - normal2 : Vector{ x: 0.0, y: 0.0 } - } - } -} - -// Shapes - -#[repr(C)] -pub struct ShapeInfo { - handle: Handle, - position : Vector, - rotation : f32, -} - -// Simulation Settings - -#[repr(C)] -pub struct SimulationSettings { - /// The timestep length (default: `1.0 / 60.0`) - pub dt: f32, - /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`) - /// - /// When CCD with multiple substeps is enabled, the timestep is subdivided - /// into smaller pieces. This timestep subdivision won't generate timestep - /// lengths smaller than `min_ccd_dt`. - /// - /// Setting this to a large value will reduce the opportunity to performing - /// CCD substepping, resulting in potentially more time dropped by the - /// motion-clamping mechanism. Setting this to an very small value may lead - /// to numerical instabilities. - pub min_ccd_dt: f32, - - /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) - /// will be compensated for during the velocity solve. - /// (default `0.8`). - pub erp: f32, - /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. - /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations - /// before stabilization). - /// (default `0.25`). - pub damping_ratio: f32, - - /// 0-1: multiplier for how much of the joint violation - /// will be compensated for during the velocity solve. - /// (default `1.0`). - pub joint_erp: f32, - - /// The fraction of critical damping applied to the joint for constraints regularization. - /// (default `0.25`). - pub joint_damping_ratio: f32, - - /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). - pub allowed_linear_error: f32, - /// Maximum amount of penetration the solver will attempt to resolve in one timestep. - pub max_penetration_correction: f32, - /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). - pub prediction_distance: f32, - /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). - pub max_velocity_iterations: usize, - /// Maximum number of iterations performed to solve friction constraints (default: `8`). - pub max_velocity_friction_iterations: usize, - /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). - pub max_stabilization_iterations: usize, - /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, - /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). - pub interleave_restitution_and_friction_resolution: bool, - /// Minimum number of dynamic bodies in each active island (default: `128`). - pub min_island_size: usize, - /// Maximum number of substeps performed by the solver (default: `1`). - pub max_ccd_substeps: usize, - - gravity : Vector, -} - -#[no_mangle] -pub extern "C" fn default_simulation_settings() -> SimulationSettings { - SimulationSettings { - dt: 1.0 / 60.0, - min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.8, - damping_ratio: 0.25, - joint_erp: 1.0, - joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, - max_penetration_correction: Real::MAX, - prediction_distance: 0.002, - max_velocity_iterations: 4, - max_velocity_friction_iterations: 8, - max_stabilization_iterations: 1, - interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. - // TODO: what is the optimal value for min_island_size? - // It should not be too big so that we don't end up with - // huge islands that don't fit in cache. - // However we don't want it to be too small and end up with - // tons of islands, reducing SIMD parallelism opportunities. - min_island_size: 128, - max_ccd_substeps: 1, - gravity : Vector { x : 0.0, y : -9.81 }, - } -} - -// World Settings - -#[repr(C)] -pub struct WorldSettings { - sleep_linear_threshold: f32, - sleep_angular_threshold: f32, - sleep_time_until_sleep: f32, - solver_prediction_distance : f32, -} - -#[no_mangle] -pub extern "C" fn default_world_settings() -> WorldSettings { - WorldSettings { - sleep_linear_threshold : 0.1, - sleep_angular_threshold : 0.1, - sleep_time_until_sleep : 1.0, - solver_prediction_distance : 0.002, - } -} - -// Material - -#[repr(C)] -pub struct Material { - friction : f32, - restitution : f32, -} - -#[no_mangle] -pub extern "C" fn default_material() -> Material { - Material { - friction : 1.0, - restitution : 0.0, - } -} - -#[repr(C)] -pub struct Vector { - x : f32, - y : f32, -} - -// Misc Utilities - -fn point_array_to_vec(data : &Vector, data_count : usize) -> Vec::> { - let mut vec = Vec::>::with_capacity(data_count); - unsafe { - let data_raw = std::slice::from_raw_parts(data, data_count); - for point in data_raw { - vec.push(Point:: { coords : vector![point.x, point.y] }); - } - } - return vec; -} - -fn world_handle_to_handle(world_handle : Index) -> Handle { - let raw_parts = world_handle.into_raw_parts(); - return Handle { - id : raw_parts.0, - generation : raw_parts.1, - } -} - - -fn handle_to_world_handle(handle : Handle) -> Index { - return Index::from_raw_parts(handle.id, handle.generation); -} - -fn shape_handle_to_handle(shape_handle : Index) -> Handle { - let raw_parts = shape_handle.into_raw_parts(); - return Handle { - id : raw_parts.0, - generation : raw_parts.1, - } -} - - -fn handle_to_shape_handle(handle : Handle) -> Index { - return Index::from_raw_parts(handle.id, handle.generation); -} - -fn collider_handle_to_handle(collider_handle : ColliderHandle) -> Handle { - let raw_parts = collider_handle.into_raw_parts(); - return Handle { - id : raw_parts.0, - generation : raw_parts.1, - } -} - -fn handle_to_collider_handle(handle : Handle) -> ColliderHandle { - return ColliderHandle::from_raw_parts(handle.id, handle.generation); -} - -fn rigid_body_handle_to_handle(rigid_body_handle : RigidBodyHandle) -> Handle { - let raw_parts = rigid_body_handle.into_raw_parts(); - return Handle { - id : raw_parts.0, - generation : raw_parts.1, - } -} - -fn handle_to_rigid_body_handle(handle : Handle) -> RigidBodyHandle { - return RigidBodyHandle::from_raw_parts(handle.id, handle.generation); -} - -fn joint_handle_to_handle(joint_handle : ImpulseJointHandle) -> Handle { - let raw_parts = joint_handle.into_raw_parts(); - return Handle { - id : raw_parts.0, - generation : raw_parts.1, - } -} - -fn handle_to_joint_handle(handle : Handle) -> ImpulseJointHandle { - return ImpulseJointHandle::from_raw_parts(handle.id, handle.generation); -} - -// Physics Engine - -struct PhysicsEngine { - physics_worlds : Arena, - shapes : Arena, -} - -impl PhysicsEngine { - fn new() -> PhysicsEngine { - PhysicsEngine { - physics_worlds: Arena::new(), - shapes: Arena::new(), - } - } - - fn insert_world(&mut self, world : PhysicsWorld) -> Handle { - let world_handle = self.physics_worlds.insert(world); - return world_handle_to_handle(world_handle); - } - - fn remove_world(&mut self, handle : Handle) { - let world_handle = handle_to_world_handle(handle); - self.physics_worlds.remove(world_handle); - } - - fn get_world(&mut self, handle : Handle) -> &mut PhysicsWorld { - let world_handle = handle_to_world_handle(handle); - let world = self.physics_worlds.get_mut(world_handle); - assert!(world.is_some()); - return world.unwrap(); - } - - fn insert_shape(&mut self, shape : SharedShape) -> Handle { - let shape_handle = self.shapes.insert(shape); - return shape_handle_to_handle(shape_handle); - } - - fn remove_shape(&mut self, handle : Handle) { - let shape_handle = handle_to_shape_handle(handle); - self.shapes.remove(shape_handle); - } - - fn get_shape(&mut self, handle : Handle) -> &SharedShape { - let shape_handle = handle_to_shape_handle(handle); - let shape = self.shapes.get(shape_handle); - assert!(shape.is_some()); - return shape.unwrap(); - } -} - -#[macro_use] -extern crate lazy_static; -use std::sync::Mutex; - -lazy_static! { - static ref SINGLETON: Mutex = Mutex::new(PhysicsEngine::new()); -} - -// Physics World - -type ActiveBodyCallback = Option; -type CollisionFilterCallback = Option bool>; - -type CollisionEventCallback = Option; - -type ContactForceEventCallback = Option bool>; -type ContactPointCallback = Option bool>; - -struct PhysicsHooksCollisionFilter<'a> { - world_handle : Handle, - collision_filter_body_callback : &'a CollisionFilterCallback, - collision_filter_sensor_callback : &'a CollisionFilterCallback, -} - -impl<'a> PhysicsHooks for PhysicsHooksCollisionFilter<'a> { - fn filter_contact_pair(&self, context: &PairFilterContext) -> Option { - if self.collision_filter_body_callback.is_some() { - let callback = self.collision_filter_body_callback.unwrap(); - - let user_data1 = context.colliders[context.collider1].user_data; - let user_data2 = context.colliders[context.collider2].user_data; - - let mut filter_info = CollisionFilterInfo::new(); - filter_info.user_data1 = UserData::new(user_data1); - filter_info.user_data2 = UserData::new(user_data2); - - // Handle contact filtering for rigid bodies - if !callback(self.world_handle, &filter_info) { - return None; - } - } - - return Some(SolverFlags::COMPUTE_IMPULSES); - } - - fn filter_intersection_pair(&self, context: &PairFilterContext) -> bool { - if self.collision_filter_sensor_callback.is_some() { - let callback = self.collision_filter_sensor_callback.unwrap(); - - let user_data1 = context.colliders[context.collider1].user_data; - let user_data2 = context.colliders[context.collider2].user_data; - - let mut filter_info = CollisionFilterInfo::new(); - filter_info.user_data1 = UserData::new(user_data1); - filter_info.user_data2 = UserData::new(user_data2); - - // Handle intersection filtering for sensors - return callback(self.world_handle, &filter_info); - } - - return true; - } - - fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { - // TODO implement conveyer belt - // for this we need to store the static object constant speed somewhere - /* - if context.rigid_body1.is_none() || context.rigid_body2.is_none() { - return; - } - let rigid_body1 = context.bodies.get(context.rigid_body1.unwrap()); - let rigid_body2 = context.bodies.get(context.rigid_body1.unwrap()); - if rigid_body1.is_none() || rigid_body2.is_none() { - return; - } - let mut rigid_body1 = rigid_body1.unwrap(); - let mut rigid_body2 = rigid_body2.unwrap(); - - if rigid_body2.is_fixed() && !rigid_body1.is_fixed() { - (rigid_body2, rigid_body1) = (rigid_body1, rigid_body2); - } - */ - // static and non static - //if rigid_body1.is_fixed() && !rigid_body2.is_fixed() { - for solver_contact in &mut *context.solver_contacts { - //solver_contact.tangent_velocity.x = 100.0; - } - //} - } -} - -struct PhysicsWorld { - query_pipeline: QueryPipeline, - physics_pipeline : PhysicsPipeline, - island_manager : IslandManager, - broad_phase : BroadPhase, - narrow_phase : NarrowPhase, - impulse_joint_set : ImpulseJointSet, - multibody_joint_set : MultibodyJointSet, - ccd_solver : CCDSolver, - - sleep_linear_threshold: f32, - sleep_angular_threshold: f32, - sleep_time_until_sleep: f32, - solver_prediction_distance : f32, - - active_body_callback : ActiveBodyCallback, - collision_filter_body_callback : CollisionFilterCallback, - collision_filter_sensor_callback : CollisionFilterCallback, - - collision_event_callback : CollisionEventCallback, - contact_force_event_callback : ContactForceEventCallback, - - contact_point_callback : ContactPointCallback, - - collider_set : ColliderSet, - rigid_body_set : RigidBodySet, - - handle : Handle, -} - -impl PhysicsWorld { - fn new(settings : &WorldSettings) -> PhysicsWorld { - PhysicsWorld { - query_pipeline : QueryPipeline::new(), - physics_pipeline : PhysicsPipeline::new(), - island_manager : IslandManager::new(), - broad_phase : BroadPhase::new(), - narrow_phase : NarrowPhase::new(), - impulse_joint_set : ImpulseJointSet::new(), - multibody_joint_set : MultibodyJointSet::new(), - ccd_solver : CCDSolver::new(), - - sleep_linear_threshold : settings.sleep_linear_threshold, - sleep_angular_threshold : settings.sleep_angular_threshold, - sleep_time_until_sleep : settings.sleep_time_until_sleep, - solver_prediction_distance :settings.solver_prediction_distance, - - active_body_callback : None, - collision_filter_body_callback : None, - collision_filter_sensor_callback : None, - - collision_event_callback : None, - contact_force_event_callback : None, - - contact_point_callback : None, - - rigid_body_set : RigidBodySet::new(), - collider_set : ColliderSet::new(), - - handle : invalid_handle(), - } - } - - fn step(&mut self, settings : &SimulationSettings) { - let mut integration_parameters = IntegrationParameters::default(); - - integration_parameters.dt = settings.dt; - integration_parameters.min_ccd_dt = settings.min_ccd_dt; - integration_parameters.erp = settings.erp; - integration_parameters.damping_ratio = settings.damping_ratio; - integration_parameters.joint_erp = settings.joint_erp; - integration_parameters.joint_damping_ratio = settings.joint_damping_ratio; - integration_parameters.allowed_linear_error = settings.allowed_linear_error; - integration_parameters.max_penetration_correction = settings.max_penetration_correction; - integration_parameters.prediction_distance = settings.prediction_distance; - integration_parameters.max_velocity_iterations = settings.max_velocity_iterations; - integration_parameters.max_velocity_friction_iterations = settings.max_velocity_friction_iterations; - integration_parameters.max_stabilization_iterations = settings.max_stabilization_iterations; - integration_parameters.interleave_restitution_and_friction_resolution = settings.interleave_restitution_and_friction_resolution; - integration_parameters.min_island_size = settings.min_island_size; - integration_parameters.max_ccd_substeps = settings.max_ccd_substeps; - - let gravity = vector![settings.gravity.x, settings.gravity.y]; - - let physics_hooks = PhysicsHooksCollisionFilter { - world_handle : self.handle, - collision_filter_body_callback : &self.collision_filter_body_callback, - collision_filter_sensor_callback : &self.collision_filter_sensor_callback, - }; - - // Initialize the event collector. - let (collision_send, collision_recv) = crossbeam::channel::unbounded(); - let (contact_force_send, contact_force_recv) = crossbeam::channel::unbounded(); - let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); - - self.physics_pipeline.step( - &gravity, - &integration_parameters, - &mut self.island_manager, - &mut self.broad_phase, - &mut self.narrow_phase, - &mut self.rigid_body_set, - &mut self.collider_set, - &mut self.impulse_joint_set, - &mut self.multibody_joint_set, - &mut self.ccd_solver, - Some(&mut self.query_pipeline), - &physics_hooks, - &event_handler, - ); - - if self.active_body_callback.is_some() { - let callback = self.active_body_callback.unwrap(); - for handle in self.island_manager.active_dynamic_bodies() { - // Send the active body event. - let mut active_body_info = ActiveBodyInfo::new(); - active_body_info.body_handle = rigid_body_handle_to_handle(*handle); - active_body_info.body_user_data = self.get_rigid_body_user_data(*handle); - - callback(self.handle, &active_body_info); - } - } - - if self.collision_event_callback.is_some() { - let callback = self.collision_event_callback.unwrap(); - while let Ok(collision_event) = collision_recv.try_recv() { - let handle1 = collision_event.collider1(); - let handle2 = collision_event.collider2(); - - // Handle the collision event. - let mut event_info = CollisionEventInfo::new(); - event_info.is_sensor = collision_event.sensor(); - event_info.is_removed = collision_event.removed(); - event_info.is_started = collision_event.started(); - event_info.collider1 = collider_handle_to_handle(handle1); - event_info.collider2 = collider_handle_to_handle(handle2); - event_info.user_data1 = self.get_collider_user_data(handle1); - event_info.user_data2 = self.get_collider_user_data(handle2); - - callback(self.handle, &event_info); - } - } - - if self.contact_force_event_callback.is_some() { - let callback = self.contact_force_event_callback.unwrap(); - while let Ok(contact_force_event) = contact_force_recv.try_recv() { - let collider1 = self.collider_set.get(contact_force_event.collider1).unwrap(); - let collider2 = self.collider_set.get(contact_force_event.collider2).unwrap(); - - // Handle the contact force event. - let mut event_info = ContactForceEventInfo::new(); - event_info.collider1 = collider_handle_to_handle(contact_force_event.collider1); - event_info.collider2 = collider_handle_to_handle(contact_force_event.collider2); - event_info.user_data1 = UserData::new(collider1.user_data); - event_info.user_data2 = UserData::new(collider2.user_data); - - let mut send_contact_points = callback(self.handle, &event_info); - - if send_contact_points && self.contact_point_callback.is_some() { - let contact_callback = self.contact_point_callback.unwrap(); - - let body1: &RigidBody = self.get_collider_rigid_body(collider1).unwrap(); - let body2: &RigidBody = self.get_collider_rigid_body(collider2).unwrap(); - - // Find the contact pair, if it exists, between two colliders - if let Some(contact_pair) = self.narrow_phase.contact_pair(contact_force_event.collider1, contact_force_event.collider2) { - let mut contact_info = ContactPointInfo::new(); - - let mut swap = false; - if contact_force_event.collider1 != contact_pair.collider1 { - assert!(contact_force_event.collider1 == contact_pair.collider2); - assert!(contact_force_event.collider2 == contact_pair.collider1); - swap = true; - } else { - assert!(contact_force_event.collider2 == contact_pair.collider2); - } - - // We may also read the contact manifolds to access the contact geometry. - for manifold in &contact_pair.manifolds { - let manifold_normal = manifold.data.normal; - contact_info.normal = Vector { x : manifold_normal.x, y : manifold_normal.y }; - - // Read the geometric contacts. - for contact_point in &manifold.points { - let collider_pos_1 = collider1.position() * contact_point.local_p1; - let collider_pos_2 = collider2.position() * contact_point.local_p2; - let point_velocity_1 = body1.velocity_at_point(&collider_pos_1); - let point_velocity_2 = body2.velocity_at_point(&collider_pos_2); - - if swap { - contact_info.local_pos_1 = Vector { x : collider_pos_2.x, y : collider_pos_2.y }; - contact_info.local_pos_2 = Vector { x : collider_pos_1.x, y : collider_pos_1.y }; - contact_info.velocity_pos_1 = Vector { x : point_velocity_2.x, y : point_velocity_2.y }; - contact_info.velocity_pos_2 = Vector { x : point_velocity_1.x, y : point_velocity_1.y }; - } else { - contact_info.local_pos_1 = Vector { x : collider_pos_1.x, y : collider_pos_1.y }; - contact_info.local_pos_2 = Vector { x : collider_pos_2.x, y : collider_pos_2.y }; - contact_info.velocity_pos_1 = Vector { x : point_velocity_1.x, y : point_velocity_1.y }; - contact_info.velocity_pos_2 = Vector { x : point_velocity_2.x, y : point_velocity_2.y }; - } - contact_info.distance = contact_point.dist; - contact_info.impulse = contact_point.data.impulse; - contact_info.tangent_impulse = contact_point.data.tangent_impulse; - - send_contact_points = contact_callback(self.handle, &contact_info); - if !send_contact_points { - break; - } - } - - if !send_contact_points { - break; - } - } - } - } - } - } - } - - fn insert_collider(&mut self, collider : Collider, body_handle : Handle) -> Handle { - if body_handle.is_valid() - { - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let collider_handle = self.collider_set.insert_with_parent(collider, rigid_body_handle, &mut self.rigid_body_set); - return collider_handle_to_handle(collider_handle); - } - else - { - let collider_handle = self.collider_set.insert(collider); - return collider_handle_to_handle(collider_handle); - } - } - - fn remove_collider(&mut self, handle : Handle) { - let collider_handle = handle_to_collider_handle(handle); - self.collider_set.remove(collider_handle - , &mut self.island_manager - , &mut self.rigid_body_set - , false - ); - } - - fn get_collider_user_data(&self, collider_handle : ColliderHandle) -> UserData { - let collider = self.collider_set.get(collider_handle); - if !collider.is_some() { - return invalid_user_data(); - } else{ - return UserData::new(collider.unwrap().user_data); - } - } - - fn get_collider_rigid_body(&self, collider : &Collider) -> Option<&RigidBody> { - let parent = collider.parent(); - if parent.is_some() { - return self.rigid_body_set.get(parent.unwrap()); - } else { - return None; - } - } - - fn insert_rigid_body(&mut self, rigid_body : RigidBody) -> Handle { - let body_handle = self.rigid_body_set.insert(rigid_body); - return rigid_body_handle_to_handle(body_handle); - } - - fn remove_rigid_body(&mut self, body_handle : Handle) { - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - self.rigid_body_set.remove(rigid_body_handle - , &mut self.island_manager - , &mut self.collider_set - , &mut self.impulse_joint_set - , &mut self.multibody_joint_set - , true - ); - } - - fn get_rigid_body_user_data(&self, rigid_body_handle : RigidBodyHandle) -> UserData { - let rigid_body = self.rigid_body_set.get(rigid_body_handle); - if !rigid_body.is_some() { - return invalid_user_data(); - } else{ - return UserData::new(rigid_body.unwrap().user_data); - } - } - - fn insert_joint(&mut self, body_handle_1 : Handle, body_handle_2 : Handle, joint : impl Into) -> Handle { - let rigid_body_1_handle = handle_to_rigid_body_handle(body_handle_1); - let rigid_body_2_handle = handle_to_rigid_body_handle(body_handle_2); - - let joint_handle = self.impulse_joint_set.insert(rigid_body_1_handle, rigid_body_2_handle, joint, true); - return joint_handle_to_handle(joint_handle); - } - - fn remove_joint(&mut self, handle : Handle) { - let joint_handle = handle_to_joint_handle(handle); - self.impulse_joint_set.remove(joint_handle, true); - } -} - -// World interface - -#[no_mangle] -pub extern "C" fn world_create(settings: &WorldSettings) -> Handle { - let physics_world = PhysicsWorld::new(settings); - let mut physics_engine = SINGLETON.lock().unwrap(); - let world_handle = physics_engine.insert_world(physics_world); - let physics_world = physics_engine.get_world(world_handle); - physics_world.handle = world_handle; - return world_handle; -} - -#[no_mangle] -pub extern "C" fn world_destroy(world_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.handle = invalid_handle(); - physics_engine.remove_world(world_handle); -} - -#[no_mangle] -pub extern "C" fn world_step(world_handle : Handle, settings : &SimulationSettings) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.step(settings); -} - -#[no_mangle] -pub extern "C" fn world_set_active_body_callback(world_handle : Handle, callback : ActiveBodyCallback) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.active_body_callback = callback; -} - -#[no_mangle] -pub extern "C" fn world_set_body_collision_filter_callback(world_handle : Handle, callback : CollisionFilterCallback) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.collision_filter_body_callback = callback; -} - -#[no_mangle] -pub extern "C" fn world_set_sensor_collision_filter_callback(world_handle : Handle, callback : CollisionFilterCallback) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.collision_filter_sensor_callback = callback; -} - -#[no_mangle] -pub extern "C" fn world_set_collision_event_callback(world_handle : Handle, callback : CollisionEventCallback) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.collision_event_callback = callback; -} - -#[no_mangle] -pub extern "C" fn world_set_contact_force_event_callback(world_handle : Handle, callback : ContactForceEventCallback) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.contact_force_event_callback = callback; -} - -#[no_mangle] -pub extern "C" fn world_set_contact_point_callback(world_handle : Handle, callback : ContactPointCallback) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.contact_point_callback = callback; -} - -// Shape interface - -#[no_mangle] -pub extern "C" fn shape_create_box(size : &Vector) -> Handle { - let shape = SharedShape::cuboid(0.5 * size.x, 0.5 * size.y); - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_create_halfspace(normal : &Vector) -> Handle { - let shape = SharedShape::halfspace(UnitVector::new_normalize(vector![normal.x, normal.y])); - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_create_circle(radius : f32) -> Handle { - let shape = SharedShape::ball(radius); - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_create_capsule(half_height : f32, radius : f32) -> Handle { - let top_circle = SharedShape::ball(radius); - let top_circle_position = Isometry::new(vector![0.0, -half_height], 0.0); - let bottom_circle = SharedShape::ball(radius); - let bottom_circle_position = Isometry::new(vector![0.0, half_height], 0.0); - let square = SharedShape::cuboid(0.5 * radius, 0.5 * (half_height - radius)); - let square_pos = Isometry::new(vector![0.0, 0.0], 0.0); - let mut shapes_vec = Vec::<(Isometry, SharedShape)>::new(); - shapes_vec.push((top_circle_position, top_circle)); - shapes_vec.push((bottom_circle_position, bottom_circle)); - shapes_vec.push((square_pos, square)); - let shape = SharedShape::compound(shapes_vec); - // For now create the shape using circles and squares as the default capsule is buggy - // in case of distance checking(returns invalid distances when close to the ends) - // overall results in a 1.33x decrease in performance - // TODO only do this in case of static objects? - //let shape = SharedShape::capsule_y(half_height, radius); - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_create_convex_polyline(points : &Vector, point_count : usize) -> Handle { - let points_vec = point_array_to_vec(points, point_count); - let shape_data = SharedShape::convex_polyline(points_vec); - if shape_data.is_none() { - return Handle::default(); - } - let shape = shape_data.unwrap(); - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_create_convave_polyline(points : &Vector, point_count : usize) -> Handle { - let points_vec = point_array_to_vec(points, point_count); - let shape = SharedShape::polyline(points_vec, None); - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_create_compound(shapes : &ShapeInfo, shape_count : usize) -> Handle { - let mut shapes_vec = Vec::<(Isometry, SharedShape)>::with_capacity(shape_count); - let mut physics_engine = SINGLETON.lock().unwrap(); - unsafe { - let data_raw = std::slice::from_raw_parts(shapes, shape_count); - for shape_info in data_raw { - let shape = physics_engine.get_shape(shape_info.handle); - let pos = vector![shape_info.position.x, shape_info.position.y]; - shapes_vec.push((Isometry::new(pos, shape_info.rotation), shape.clone())); - } - } - - let shape = SharedShape::compound(shapes_vec); - return physics_engine.insert_shape(shape); -} - -#[no_mangle] -pub extern "C" fn shape_destroy(shape_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - return physics_engine.remove_shape(shape_handle); -} - -// Collider interface - -#[no_mangle] -pub extern "C" fn collider_create_solid(world_handle : Handle, shape_handle : Handle, mat : &Material, body_handle : Handle, user_data : &UserData) -> Handle { - let mut physics_engine = SINGLETON.lock().unwrap(); - let shape = physics_engine.get_shape(shape_handle); - let mut collider = ColliderBuilder::new(shape.clone()).build(); - collider.set_friction(mat.friction); - collider.set_restitution(mat.restitution); - collider.set_friction_combine_rule(CoefficientCombineRule::Multiply); - collider.set_restitution_combine_rule(CoefficientCombineRule::Max); - collider.set_density(0.0); - collider.user_data = user_data.get_data(); - collider.set_active_hooks(ActiveHooks::FILTER_CONTACT_PAIRS | ActiveHooks::FILTER_INTERSECTION_PAIR | ActiveHooks::MODIFY_SOLVER_CONTACTS); - let physics_world = physics_engine.get_world(world_handle); - return physics_world.insert_collider(collider, body_handle); -} - -#[no_mangle] -pub extern "C" fn collider_create_sensor(world_handle : Handle, shape_handle : Handle, body_handle : Handle, user_data : &UserData) -> Handle { - let mut physics_engine = SINGLETON.lock().unwrap(); - let shape = physics_engine.get_shape(shape_handle); - let mut collider = ColliderBuilder::new(shape.clone()).build(); - collider.set_sensor(true); - collider.set_active_events(ActiveEvents::COLLISION_EVENTS); - let mut collision_types = collider.active_collision_types(); - collision_types |= ActiveCollisionTypes::FIXED_FIXED; - collider.set_active_collision_types(collision_types); - collider.user_data = user_data.get_data(); - collider.set_active_hooks(ActiveHooks::FILTER_INTERSECTION_PAIR); - let physics_world = physics_engine.get_world(world_handle); - return physics_world.insert_collider(collider, body_handle); -} - -#[no_mangle] -pub extern "C" fn collider_destroy(world_handle : Handle, handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - return physics_world.remove_collider(handle); -} - -#[no_mangle] -pub extern "C" fn collider_get_position(world_handle : Handle, handle : Handle) -> Vector { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let collider_handle = handle_to_collider_handle(handle); - let collider = physics_world.collider_set.get(collider_handle); - assert!(collider.is_some()); - let collider_vector = collider.unwrap().translation(); - return Vector { x : collider_vector.x, y : collider_vector.y }; -} - -#[no_mangle] -pub extern "C" fn collider_get_angle(world_handle : Handle, handle : Handle) -> f32 { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let collider_handle = handle_to_collider_handle(handle); - let collider = physics_world.collider_set.get(collider_handle); - assert!(collider.is_some()); - return collider.unwrap().rotation().angle(); -} - -#[no_mangle] -pub extern "C" fn collider_set_transform(world_handle : Handle, handle : Handle, pos : &Vector, rot : f32) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let collider_handle = handle_to_collider_handle(handle); - let collider = physics_world.collider_set.get_mut(collider_handle); - assert!(collider.is_some()); - collider.unwrap().set_position_wrt_parent(Isometry::new(vector![pos.x, pos.y], rot)); -} - -#[no_mangle] -pub extern "C" fn collider_set_collision_events_enabled(world_handle : Handle, handle : Handle, enable : bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let collider_handle = handle_to_collider_handle(handle); - let collider = physics_world.collider_set.get_mut(collider_handle); - assert!(collider.is_some()); - let collider_access = collider.unwrap(); - let mut active_events = collider_access.active_events(); - if enable { - active_events |= ActiveEvents::COLLISION_EVENTS; - } else { - active_events &= !ActiveEvents::COLLISION_EVENTS; - } - collider_access.set_active_events(active_events); -} - -#[no_mangle] -pub extern "C" fn collider_set_contact_force_events_enabled(world_handle : Handle, handle : Handle, enable : bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let collider_handle = handle_to_collider_handle(handle); - let collider = physics_world.collider_set.get_mut(collider_handle); - assert!(collider.is_some()); - let collider_access = collider.unwrap(); - let mut active_events = collider_access.active_events(); - if enable { - active_events |= ActiveEvents::CONTACT_FORCE_EVENTS; - } else { - active_events &= !ActiveEvents::CONTACT_FORCE_EVENTS; - } - collider_access.set_active_events(active_events); -} - -// Rigid body interface - -fn set_rigid_body_properties_internal(rigid_body : &mut RigidBody, pos : &Vector, rot : f32, wake_up : bool) { - if rigid_body.is_dynamic() { - rigid_body.set_rotation(Rotation::new(rot), wake_up); - rigid_body.set_translation(vector![pos.x, pos.y], wake_up); - } else { - rigid_body.set_next_kinematic_position(Isometry::new(vector![pos.x, pos.y], rot)); - } -} - -#[no_mangle] -pub extern "C" fn body_create_fixed(world_handle : Handle, pos : &Vector, rot : f32, user_data : &UserData) -> Handle { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let mut rigid_body = RigidBodyBuilder::kinematic_position_based().build(); - set_rigid_body_properties_internal(&mut rigid_body, pos, rot, true); - rigid_body.user_data = user_data.get_data(); - let body_handle = physics_world.rigid_body_set.insert(rigid_body); - return rigid_body_handle_to_handle(body_handle); -} - -#[no_mangle] -pub extern "C" fn body_create_dynamic(world_handle : Handle, pos : &Vector, rot : f32, user_data : &UserData) -> Handle { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let mut rigid_body = RigidBodyBuilder::dynamic().can_sleep(true).sleeping(true).build(); - let activation = rigid_body.activation_mut(); - activation.time_until_sleep = physics_world.sleep_time_until_sleep; - activation.linear_threshold = physics_world.sleep_linear_threshold; - activation.angular_threshold = physics_world.sleep_angular_threshold; - set_rigid_body_properties_internal(&mut rigid_body, pos, rot, false); - rigid_body.user_data = user_data.get_data(); - return physics_world.insert_rigid_body(rigid_body); -} - -#[no_mangle] -pub extern "C" fn body_destroy(world_handle : Handle, body_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - physics_world.remove_rigid_body(body_handle); -} - -#[no_mangle] -pub extern "C" fn body_get_position(world_handle : Handle, body_handle : Handle) -> Vector { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get(rigid_body_handle); - assert!(body.is_some()); - let body_vector = body.unwrap().translation(); - return Vector { x : body_vector.x, y : body_vector.y }; -} - -#[no_mangle] -pub extern "C" fn body_get_angle(world_handle : Handle, body_handle : Handle) -> f32 { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get(rigid_body_handle); - assert!(body.is_some()); - return body.unwrap().rotation().angle(); -} - -#[no_mangle] -pub extern "C" fn body_set_transform(world_handle : Handle, body_handle : Handle, pos : &Vector, rot : f32, wake_up : bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - let body = body.unwrap(); - set_rigid_body_properties_internal(body, pos, rot, wake_up); -} - -#[no_mangle] -pub extern "C" fn body_get_linear_velocity(world_handle : Handle, body_handle : Handle) -> Vector { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get(rigid_body_handle); - assert!(body.is_some()); - let body_vel = body.unwrap().linvel(); - return Vector { x : body_vel.x, y : body_vel.y }; -} - -#[no_mangle] -pub extern "C" fn body_set_linear_velocity(world_handle : Handle, body_handle : Handle, vel : &Vector) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().set_linvel(vector![vel.x, vel.y], true); -} - -#[no_mangle] -pub extern "C" fn body_update_material(world_handle : Handle, body_handle : Handle, mat : &Material) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - for collider in body.unwrap().colliders() { - if let Some(col) = physics_world.collider_set.get_mut(*collider) { - col.set_friction(mat.friction); - col.set_restitution(mat.restitution) - } - } -} - -#[no_mangle] -pub extern "C" fn body_get_angular_velocity(world_handle : Handle, body_handle : Handle) -> f32 { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get(rigid_body_handle); - assert!(body.is_some()); - return body.unwrap().angvel(); -} - -#[no_mangle] -pub extern "C" fn body_set_angular_velocity(world_handle : Handle, body_handle : Handle, vel : f32) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().set_angvel(vel, true); -} - -#[no_mangle] -pub extern "C" fn body_set_linear_damping(world_handle : Handle, body_handle : Handle, linear_damping : f32) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().set_linear_damping(linear_damping); -} - -#[no_mangle] -pub extern "C" fn body_set_angular_damping(world_handle : Handle, body_handle : Handle, angular_damping : f32) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().set_angular_damping(angular_damping); -} - -#[no_mangle] -pub extern "C" fn body_set_gravity_scale(world_handle : Handle, body_handle : Handle, gravity_scale : f32, wake_up : bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().set_gravity_scale(gravity_scale, wake_up); -} - -#[no_mangle] -pub extern "C" fn body_set_can_sleep(world_handle : Handle, body_handle : Handle, can_sleep: bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - let body = body.unwrap(); - - if can_sleep && body.activation().angular_threshold == -1.0 { - let activation = body.activation_mut(); - activation.angular_threshold = physics_world.sleep_angular_threshold; - activation.linear_threshold = physics_world.sleep_linear_threshold; - } else if !can_sleep && body.activation().angular_threshold != -1.0 { - let activation = body.activation_mut(); - activation.angular_threshold = -1.0; - activation.linear_threshold = -1.0; - } - - // TODO: Check if is requiered - if !can_sleep && body.is_sleeping() { - body.wake_up(true); - } -} - -#[no_mangle] -pub extern "C" fn body_is_ccd_enabled(world_handle : Handle, body_handle : Handle) -> bool { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().is_ccd_enabled() -} - -#[no_mangle] -pub extern "C" fn body_set_ccd_enabled(world_handle : Handle, body_handle : Handle, enable: bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().enable_ccd(enable); -} - -#[no_mangle] -pub extern "C" fn body_set_mass_properties(world_handle : Handle, body_handle : Handle, mass : f32, inertia : f32, local_com : &Vector, wake_up : bool, force_update : bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - let body_ref = body.unwrap(); - body_ref.set_additional_mass_properties(MassProperties::new(point![local_com.x, local_com.y], mass, inertia), wake_up); - if force_update { - body_ref.recompute_mass_properties_from_colliders(&physics_world.collider_set); - } -} - -#[no_mangle] -pub extern "C" fn body_add_force(world_handle : Handle, body_handle : Handle, force : &Vector) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().add_force(vector!(force.x, force.y), true); -} - -#[no_mangle] -pub extern "C" fn body_add_force_at_point(world_handle : Handle, body_handle : Handle, force : &Vector, point : &Vector) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().add_force_at_point(vector!(force.x, force.y),point![point.x, point.y] , true); -} - -#[no_mangle] -pub extern "C" fn body_add_torque(world_handle : Handle, body_handle : Handle, torque: f32) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().add_torque(torque, true); -} - -#[no_mangle] -pub extern "C" fn body_apply_impulse(world_handle : Handle, body_handle : Handle, impulse : &Vector) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().apply_impulse(vector!(impulse.x, impulse.y), true); -} - -#[no_mangle] -pub extern "C" fn body_apply_impulse_at_point(world_handle : Handle, body_handle : Handle, impulse : &Vector, point : &Vector) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().apply_impulse_at_point(vector!(impulse.x, impulse.y),point![point.x, point.y] , true); -} - -#[no_mangle] -pub extern "C" fn body_get_constant_force(world_handle : Handle, body_handle : Handle) -> Vector { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - let constant_force = body.unwrap().user_force(); - return Vector { x : constant_force.x, y : constant_force.y }; -} - -#[no_mangle] -pub extern "C" fn body_get_constant_torque(world_handle : Handle, body_handle : Handle) -> f32 { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().user_torque() -} - -#[no_mangle] -pub extern "C" fn body_apply_torque_impulse(world_handle : Handle, body_handle : Handle, torque_impulse : f32) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().apply_torque_impulse(torque_impulse, true); -} - -#[no_mangle] -pub extern "C" fn body_reset_torques(world_handle : Handle, body_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().reset_torques(false); -} - -#[no_mangle] -pub extern "C" fn body_reset_forces(world_handle : Handle, body_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().reset_forces(false); -} - -#[no_mangle] -pub extern "C" fn body_wake_up(world_handle : Handle, body_handle : Handle, strong : bool) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - // TODO: check if this is OK, at the startup call to RapierPhysicsServer2D::free where body is not some - assert!(body.is_some()); - let body = body.unwrap(); - if body.is_sleeping() { - body.wake_up(strong); - } -} - -#[no_mangle] -pub extern "C" fn body_force_sleep(world_handle : Handle, body_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - let rigid_body_handle = handle_to_rigid_body_handle(body_handle); - let body = physics_world.rigid_body_set.get_mut(rigid_body_handle); - assert!(body.is_some()); - body.unwrap().sleep(); -} - -// Joint interface - -#[no_mangle] -pub extern "C" fn joint_create_revolute(world_handle : Handle, body_handle_1 : Handle, body_handle_2 : Handle, anchor_1 : &Vector, anchor_2 : &Vector, angular_limit_lower: f32, angular_limit_upper: f32, angular_limit_enabled: bool, motor_target_velocity: f32, motor_enabled: bool) -> Handle { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - - let mut joint = RevoluteJointBuilder::new() - .local_anchor1(point!(anchor_1.x, anchor_1.y)) - .local_anchor2(point!(anchor_2.x, anchor_2.y)) - .motor_model(MotorModel::AccelerationBased); - if angular_limit_enabled { - joint = joint.limits([angular_limit_lower, angular_limit_upper]); - } - if motor_enabled { - joint = joint.motor_velocity(motor_target_velocity, 0.0); - } - - return physics_world.insert_joint(body_handle_1, body_handle_2, joint); -} - - -#[no_mangle] -pub extern "C" fn joint_change_revolute_params(world_handle : Handle, joint_handle: Handle , angular_limit_lower: f32, angular_limit_upper: f32, angular_limit_enabled: bool, motor_target_velocity: f32, motor_enabled: bool){ - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - - let joint = physics_world.impulse_joint_set.get_mut(handle_to_joint_handle(joint_handle)); - assert!(joint.is_some()); - let joint = joint.unwrap().data.as_revolute_mut(); - assert!(joint.is_some()); - let mut joint = joint.unwrap(); - if motor_enabled { - joint = joint.set_motor_velocity(motor_target_velocity, 0.0); - } - if angular_limit_enabled { - joint.set_limits([angular_limit_lower, angular_limit_upper]); - } -} - -#[no_mangle] -pub extern "C" fn joint_create_prismatic(world_handle : Handle, body_handle_1 : Handle, body_handle_2 : Handle, axis : &Vector, anchor_1 : &Vector, anchor_2 : &Vector, limits : &Vector) -> Handle { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - - let joint = PrismaticJointBuilder::new(UnitVector::new_normalize(vector![axis.x, axis.y])) - .local_anchor1(point!(anchor_1.x, anchor_1.y)) - .local_anchor2(point!(anchor_2.x, anchor_2.y)) - .limits([limits.x, limits.y]); - - return physics_world.insert_joint(body_handle_1, body_handle_2, joint); -} - -#[no_mangle] -pub extern "C" fn joint_destroy(world_handle : Handle, joint_handle : Handle) { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - - return physics_world.remove_joint(joint_handle); -} - -// Scene queries - -// PhysicsDirectSpaceState2DExtension -type QueryHandleExcludedCallback = Option bool>; - -#[no_mangle] -pub extern "C" fn intersect_ray(world_handle : Handle, from : &Vector, dir : &Vector, length: f32, collide_with_body: bool, collide_with_area: bool, hit_from_inside: bool, hit_info : &mut RayHitInfo, handle_excluded_callback: QueryHandleExcludedCallback) -> bool { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - - let ray = Ray::new(point![from.x, from.y], vector![dir.x, dir.y]); - let solid = true; - let mut filter = QueryFilter::new(); - - if !collide_with_body { - filter = filter.exclude_solids(); - } - if !collide_with_area { - filter = filter.exclude_sensors(); - } - - let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { - if handle_excluded_callback.is_some() { - return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle)); - } - return true; - }; - - filter.predicate = Some(&predicate); - - let mut result = false; - physics_world.query_pipeline.intersections_with_ray(&physics_world.rigid_body_set, &physics_world.collider_set, &ray, length, solid, filter, - |handle, intersection| { - // Callback called on each collider hit by the ray. - - if hit_from_inside || intersection.toi != 0.0 { - result = true; - - let hit_point = ray.point_at(intersection.toi); - let hit_normal = intersection.normal; - hit_info.position = Vector { - x: hit_point.x, - y: hit_point.y, - }; - hit_info.normal = Vector { - x: hit_normal.x, - y: hit_normal.y, - }; - hit_info.collider = collider_handle_to_handle(handle); - hit_info.user_data = physics_world.get_collider_user_data(handle); - return false; // We found a collision hit. - } - true // Continue to search. - }, - ); - - return result; -} - -#[no_mangle] -pub extern "C" fn intersect_point(world_handle : Handle, position : &Vector, collide_with_body: bool, collide_with_area: bool, hit_info_array : *mut PointHitInfo, hit_info_length : usize, handle_excluded_callback: QueryHandleExcludedCallback) -> usize { - let mut physics_engine = SINGLETON.lock().unwrap(); - let physics_world = physics_engine.get_world(world_handle); - - let point = Point::new(position.x, position.y); - let mut filter = QueryFilter::new(); - - if !collide_with_body { - filter = filter.exclude_solids(); - } - if !collide_with_area { - filter = filter.exclude_sensors(); - } - - let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { - if handle_excluded_callback.is_some() { - return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle)); - } - return true; - }; - - filter.predicate = Some(&predicate); - - assert!(hit_info_length > 0); - let hit_info_slice_opt; - unsafe { - hit_info_slice_opt = Some(std::slice::from_raw_parts_mut(hit_info_array, hit_info_length)); - } - assert!(hit_info_slice_opt.is_some()); - let hit_info_slice = hit_info_slice_opt.unwrap(); - - let mut cpt_hit = 0; - physics_world.query_pipeline.intersections_with_point( &physics_world.rigid_body_set, &physics_world.collider_set, &point, filter, - |handle| { - // Callback called on each collider hit by the ray. - hit_info_slice[cpt_hit].collider = collider_handle_to_handle(handle); - hit_info_slice[cpt_hit].user_data = physics_world.get_collider_user_data(handle); - cpt_hit += 1; - let keep_searching = cpt_hit < hit_info_length; - keep_searching // Continue to search collisions if we still have space for results. - }); - - return cpt_hit; -} - -#[no_mangle] -pub extern "C" fn shape_casting(world_handle : Handle, motion : &Vector, position: &Vector, rotation: f32, shape_handle : Handle, collide_with_body: bool, collide_with_area: bool, handle_excluded_callback: QueryHandleExcludedCallback) -> ShapeCastResult { - let mut physics_engine = SINGLETON.lock().unwrap(); - - let shared_shape = physics_engine.get_shape(shape_handle).clone(); - - let physics_world = physics_engine.get_world(world_handle); - - let shape_vel = vector![motion.x, motion.y]; - let shape_transform = Isometry::new(vector![position.x, position.y], rotation); - - let mut filter = QueryFilter::new(); - - if !collide_with_body { - filter = filter.exclude_solids(); - } - if !collide_with_area { - filter = filter.exclude_sensors(); - } - - let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { - if handle_excluded_callback.is_some() { - return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle)); - } - return true; - }; - - filter.predicate = Some(&predicate); - - let mut result = ShapeCastResult::new(); - - if let Some((collider_handle, hit)) = physics_world.query_pipeline.cast_shape( - &physics_world.rigid_body_set, &physics_world.collider_set, &shape_transform, &shape_vel, shared_shape.as_ref(), 1.0, false, filter - ) { - result.collided = true; - result.toi = hit.toi; - result.witness1 = Vector{ x: hit.witness1.x, y: hit.witness1.y }; - result.witness2 = Vector{ x: hit.witness2.x, y: hit.witness2.y }; - result.normal1 = Vector{ x: hit.normal1.x, y: hit.normal1.y }; - result.normal2 = Vector{ x: hit.normal2.x, y: hit.normal2.y }; - result.collider = collider_handle_to_handle(collider_handle); - result.user_data = physics_world.get_collider_user_data(collider_handle); - return result; - } - return result; -} - -#[no_mangle] -pub extern "C" fn intersect_shape(world_handle : Handle, position: &Vector, rotation: f32, shape_handle : Handle, collide_with_body: bool, collide_with_area: bool, hit_info_array : *mut PointHitInfo, hit_info_length : usize, handle_excluded_callback: QueryHandleExcludedCallback) -> usize { - let mut physics_engine = SINGLETON.lock().unwrap(); - - let shared_shape = physics_engine.get_shape(shape_handle).clone(); - - let physics_world = physics_engine.get_world(world_handle); - - let shape_transform = Isometry::new(vector![position.x, position.y], rotation); - - let mut filter = QueryFilter::new(); - - if !collide_with_body { - filter = filter.exclude_solids(); - } - if !collide_with_area { - filter = filter.exclude_sensors(); - } - - let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { - if handle_excluded_callback.is_some() { - return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle)); - } - return true; - }; - - filter.predicate = Some(&predicate); - - assert!(hit_info_length > 0); - let hit_info_slice_opt; - unsafe { - hit_info_slice_opt = Some(std::slice::from_raw_parts_mut(hit_info_array, hit_info_length)); - } - assert!(hit_info_slice_opt.is_some()); - let hit_info_slice = hit_info_slice_opt.unwrap(); - - let mut cpt_hit = 0; - physics_world.query_pipeline.intersections_with_shape ( - &physics_world.rigid_body_set, &physics_world.collider_set, &shape_transform, shared_shape.as_ref(), filter, - |handle| { - // Callback called on each collider hit by the ray. - hit_info_slice[cpt_hit].collider = collider_handle_to_handle(handle); - hit_info_slice[cpt_hit].user_data = physics_world.get_collider_user_data(handle); - cpt_hit += 1; - let keep_searching = cpt_hit < hit_info_length; - keep_searching // Continue to search collisions if we still have space for results. - } - ); - - return cpt_hit; -} - -#[no_mangle] -pub extern "C" fn intersect_aabb(world_handle : Handle, aabb_min : &Vector, aabb_max : &Vector, collide_with_body: bool, collide_with_area: bool, hit_info_array : *mut PointHitInfo, hit_info_length : usize, handle_excluded_callback: QueryHandleExcludedCallback) -> usize { - let mut physics_engine = SINGLETON.lock().unwrap(); - - let physics_world = physics_engine.get_world(world_handle); - - // let aabb_transform = Isometry::new(vector![position.x, position.y], rotation); - let aabb_min_point = Point::new(aabb_min.x, aabb_min.y); - let aabb_max_point = Point::new(aabb_max.x, aabb_max.y); - - // let transformed_aabb_min = aabb_transform * aabb_min_point; - // let transformed_aabb_max = aabb_transform * aabb_max_point; - - let aabb = Aabb { - mins: aabb_min_point, - maxs: aabb_max_point, - }; - - assert!(hit_info_length > 0); - let hit_info_slice_opt; - unsafe { - hit_info_slice_opt = Some(std::slice::from_raw_parts_mut(hit_info_array, hit_info_length)); - } - assert!(hit_info_slice_opt.is_some()); - let hit_info_slice = hit_info_slice_opt.unwrap(); - - let mut cpt_hit = 0; - physics_world.query_pipeline.colliders_with_aabb_intersecting_aabb(&aabb, - |handle| { - - let mut valid_hit = false; - if let Some(collider) = physics_world.collider_set.get(*handle) { - - // type filder - if collider.is_sensor() && collide_with_area { - valid_hit = true; - } else if !collider.is_sensor() && collide_with_body { - valid_hit = true; - } - - if valid_hit && handle_excluded_callback.is_some(){ - valid_hit = !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(*handle), &physics_world.get_collider_user_data(*handle)); - } - } - - if !valid_hit { - return true; // continue - } - - // Callback called on each collider hit by the ray. - hit_info_slice[cpt_hit].collider = collider_handle_to_handle(*handle); - hit_info_slice[cpt_hit].user_data = physics_world.get_collider_user_data(*handle); - cpt_hit += 1; - let keep_searching = cpt_hit < hit_info_length; - keep_searching // Continue to search collisions if we still have space for results. - }); - - return cpt_hit; -} - -#[no_mangle] -pub extern "C" fn shapes_contact(world_handle : Handle, shape_handle1 : Handle, position1: &Vector, rotation1: f32, shape_handle2 : Handle, position2: &Vector, rotation2: f32, margin: f32) -> ContactResult { - let mut physics_engine = SINGLETON.lock().unwrap(); - - let physics_world = physics_engine.get_world(world_handle); - - let prediction = f32::max(physics_world.solver_prediction_distance, margin); - - let shared_shape1 = physics_engine.get_shape(shape_handle1).clone(); - let shared_shape2 = physics_engine.get_shape(shape_handle2).clone(); - - let shape_transform1 = Isometry::new(vector![position1.x, position1.y], rotation1); - let shape_transform2 = Isometry::new(vector![position2.x, position2.y], rotation2); - - let mut result = ContactResult::new(); - if let Ok(Some(contact)) = parry::query::contact( - &shape_transform1, shared_shape1.as_ref(), &shape_transform2, shared_shape2.as_ref(), prediction - ) { - // the distance is negative if there is intersection - // and positive if the objects are separated by distance less than margin - result.distance = contact.dist; - if contact.dist <= 0.0 { - result.within_margin = false; - } else { - result.within_margin = true; - } - result.collided = true; - result.point1 = Vector{ x: contact.point1.x + prediction * contact.normal1.x, y: contact.point1.y + prediction * contact.normal1.y }; - result.point2 = Vector{ x: contact.point2.x, y: contact.point2.y }; - result.normal1 = Vector{ x: contact.normal1.x, y: contact.normal1.y }; - result.normal2 = Vector{ x: contact.normal2.x, y: contact.normal2.y }; - return result; - } - return result; -} +#[cfg(feature = "f64")] +extern crate rapier2d_f64 as rapier2d; +mod body; +mod collider; +mod handle; +mod joint; +mod physics_hooks; +mod physics_world; +mod query; +mod settings; +mod shape; +mod user_data; +mod vector; diff --git a/src/rapier2d-wrapper/src/physics_hooks.rs b/src/rapier2d-wrapper/src/physics_hooks.rs new file mode 100644 index 0000000..5cea146 --- /dev/null +++ b/src/rapier2d-wrapper/src/physics_hooks.rs @@ -0,0 +1,93 @@ +use rapier2d::prelude::*; +use crate::handle::*; +use crate::user_data::*; + +pub type CollisionFilterCallback = Option bool>; + +#[repr(C)] +pub struct CollisionFilterInfo { + pub user_data1: UserData, + pub user_data2: UserData, +} + +impl CollisionFilterInfo { + pub fn new() -> CollisionFilterInfo { + CollisionFilterInfo { + user_data1: invalid_user_data(), + user_data2: invalid_user_data(), + } + } +} + +pub struct PhysicsHooksCollisionFilter<'a> { + pub world_handle : Handle, + pub collision_filter_body_callback : &'a CollisionFilterCallback, + pub collision_filter_sensor_callback : &'a CollisionFilterCallback, +} + +impl<'a> PhysicsHooks for PhysicsHooksCollisionFilter<'a> { + fn filter_contact_pair(&self, context: &PairFilterContext) -> Option { + if self.collision_filter_body_callback.is_some() { + let callback = self.collision_filter_body_callback.unwrap(); + + let user_data1 = context.colliders[context.collider1].user_data; + let user_data2 = context.colliders[context.collider2].user_data; + + let mut filter_info = CollisionFilterInfo::new(); + filter_info.user_data1 = UserData::new(user_data1); + filter_info.user_data2 = UserData::new(user_data2); + + // Handle contact filtering for rigid bodies + if !callback(self.world_handle, &filter_info) { + return None; + } + } + + return Some(SolverFlags::COMPUTE_IMPULSES); + } + + fn filter_intersection_pair(&self, context: &PairFilterContext) -> bool { + if self.collision_filter_sensor_callback.is_some() { + let callback = self.collision_filter_sensor_callback.unwrap(); + + let user_data1 = context.colliders[context.collider1].user_data; + let user_data2 = context.colliders[context.collider2].user_data; + + let mut filter_info = CollisionFilterInfo::new(); + filter_info.user_data1 = UserData::new(user_data1); + filter_info.user_data2 = UserData::new(user_data2); + + // Handle intersection filtering for sensors + return callback(self.world_handle, &filter_info); + } + + return true; + } + + fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) { + // TODO implement conveyer belt + // for this we need to store the static object constant speed somewhere + /* + if context.rigid_body1.is_none() || context.rigid_body2.is_none() { + return; + } + let rigid_body1 = context.bodies.get(context.rigid_body1.unwrap()); + let rigid_body2 = context.bodies.get(context.rigid_body1.unwrap()); + if rigid_body1.is_none() || rigid_body2.is_none() { + return; + } + let mut rigid_body1 = rigid_body1.unwrap(); + let mut rigid_body2 = rigid_body2.unwrap(); + + if rigid_body2.is_fixed() && !rigid_body1.is_fixed() { + (rigid_body2, rigid_body1) = (rigid_body1, rigid_body2); + } + */ + // static and non static + //if rigid_body1.is_fixed() && !rigid_body2.is_fixed() { + //for solver_contact in &mut *context.solver_contacts { + //solver_contact.tangent_velocity.x = 100.0; + //} + //} + } +} diff --git a/src/rapier2d-wrapper/src/physics_world.rs b/src/rapier2d-wrapper/src/physics_world.rs new file mode 100644 index 0000000..7da51af --- /dev/null +++ b/src/rapier2d-wrapper/src/physics_world.rs @@ -0,0 +1,523 @@ +use rapier2d::crossbeam; +use rapier2d::data::Arena; +use rapier2d::prelude::*; +use std::sync::Mutex; +use lazy_static::lazy_static; +use crate::handle::*; +use crate::user_data::*; +use crate::settings::*; +use crate::vector::Vector; +use crate::physics_hooks::*; + +#[repr(C)] +pub struct ActiveBodyInfo { + body_handle: Handle, + body_user_data: UserData, +} + +impl ActiveBodyInfo { + fn new() -> ActiveBodyInfo { + ActiveBodyInfo { + body_handle: invalid_handle(), + body_user_data: invalid_user_data(), + } + } +} + + +#[repr(C)] +pub struct ContactPointInfo { + local_pos_1: Vector, + local_pos_2: Vector, + velocity_pos_1: Vector, + velocity_pos_2: Vector, + normal: Vector, + distance: Real, + impulse: Real, + tangent_impulse: Real, +} + +impl ContactPointInfo { + fn new() -> ContactPointInfo { + ContactPointInfo { + local_pos_1: Vector { x : 0.0, y : 0.0 }, + local_pos_2: Vector { x : 0.0, y : 0.0 }, + velocity_pos_1: Vector { x : 0.0, y : 0.0 }, + velocity_pos_2: Vector { x : 0.0, y : 0.0 }, + normal: Vector { x : 0.0, y : 0.0 }, + distance: 0.0, + impulse: 0.0, + tangent_impulse: 0.0, + } + } +} + +type ActiveBodyCallback = Option; + +type CollisionEventCallback = Option; + +type ContactForceEventCallback = Option bool>; +type ContactPointCallback = Option bool>; + +#[repr(C)] +pub struct CollisionEventInfo { + collider1: Handle, + collider2: Handle, + user_data1: UserData, + user_data2: UserData, + is_sensor: bool, + is_started: bool, + is_removed: bool, +} + +impl CollisionEventInfo { + fn new() -> CollisionEventInfo { + CollisionEventInfo { + collider1: invalid_handle(), + collider2: invalid_handle(), + user_data1: invalid_user_data(), + user_data2: invalid_user_data(), + is_sensor: false, + is_started: false, + is_removed: false, + } + } +} + +#[repr(C)] +pub struct ContactForceEventInfo { + collider1: Handle, + collider2: Handle, + user_data1: UserData, + user_data2: UserData, +} + +impl ContactForceEventInfo { + fn new() -> ContactForceEventInfo { + ContactForceEventInfo { + collider1: invalid_handle(), + collider2: invalid_handle(), + user_data1: invalid_user_data(), + user_data2: invalid_user_data(), + } + } +} + +pub struct PhysicsWorld { + pub query_pipeline: QueryPipeline, + pub physics_pipeline : PhysicsPipeline, + pub island_manager : IslandManager, + pub broad_phase : BroadPhase, + pub narrow_phase : NarrowPhase, + pub impulse_joint_set : ImpulseJointSet, + pub multibody_joint_set : MultibodyJointSet, + pub ccd_solver : CCDSolver, + + pub sleep_linear_threshold: Real, + pub sleep_angular_threshold: Real, + pub sleep_time_until_sleep: Real, + pub solver_prediction_distance : Real, + + pub active_body_callback : ActiveBodyCallback, + pub collision_filter_body_callback : CollisionFilterCallback, + pub collision_filter_sensor_callback : CollisionFilterCallback, + + pub collision_event_callback : CollisionEventCallback, + pub contact_force_event_callback : ContactForceEventCallback, + + pub contact_point_callback : ContactPointCallback, + + pub collider_set : ColliderSet, + pub rigid_body_set : RigidBodySet, + + pub handle : Handle, +} + +impl PhysicsWorld { + pub fn new(settings : &WorldSettings) -> PhysicsWorld { + PhysicsWorld { + query_pipeline : QueryPipeline::new(), + physics_pipeline : PhysicsPipeline::new(), + island_manager : IslandManager::new(), + broad_phase : BroadPhase::new(), + narrow_phase : NarrowPhase::new(), + impulse_joint_set : ImpulseJointSet::new(), + multibody_joint_set : MultibodyJointSet::new(), + ccd_solver : CCDSolver::new(), + + sleep_linear_threshold : settings.sleep_linear_threshold, + sleep_angular_threshold : settings.sleep_angular_threshold, + sleep_time_until_sleep : settings.sleep_time_until_sleep, + solver_prediction_distance :settings.solver_prediction_distance, + + active_body_callback : None, + collision_filter_body_callback : None, + collision_filter_sensor_callback : None, + + collision_event_callback : None, + contact_force_event_callback : None, + + contact_point_callback : None, + + rigid_body_set : RigidBodySet::new(), + collider_set : ColliderSet::new(), + + handle : invalid_handle(), + } + } + + pub fn step(&mut self, settings : &SimulationSettings) { + let mut integration_parameters = IntegrationParameters::default(); + + integration_parameters.dt = settings.dt; + integration_parameters.min_ccd_dt = settings.min_ccd_dt; + integration_parameters.erp = settings.erp; + integration_parameters.damping_ratio = settings.damping_ratio; + integration_parameters.joint_erp = settings.joint_erp; + integration_parameters.joint_damping_ratio = settings.joint_damping_ratio; + integration_parameters.allowed_linear_error = settings.allowed_linear_error; + integration_parameters.max_penetration_correction = settings.max_penetration_correction; + integration_parameters.prediction_distance = settings.prediction_distance; + integration_parameters.max_velocity_iterations = settings.max_velocity_iterations; + integration_parameters.max_velocity_friction_iterations = settings.max_velocity_friction_iterations; + integration_parameters.max_stabilization_iterations = settings.max_stabilization_iterations; + integration_parameters.interleave_restitution_and_friction_resolution = settings.interleave_restitution_and_friction_resolution; + integration_parameters.min_island_size = settings.min_island_size; + integration_parameters.max_ccd_substeps = settings.max_ccd_substeps; + + let gravity = vector![settings.gravity.x, settings.gravity.y]; + + let physics_hooks = PhysicsHooksCollisionFilter { + world_handle : self.handle, + collision_filter_body_callback : &self.collision_filter_body_callback, + collision_filter_sensor_callback : &self.collision_filter_sensor_callback, + }; + + // Initialize the event collector. + let (collision_send, collision_recv) = crossbeam::channel::unbounded(); + let (contact_force_send, contact_force_recv) = crossbeam::channel::unbounded(); + let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + + self.physics_pipeline.step( + &gravity, + &integration_parameters, + &mut self.island_manager, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut self.rigid_body_set, + &mut self.collider_set, + &mut self.impulse_joint_set, + &mut self.multibody_joint_set, + &mut self.ccd_solver, + Some(&mut self.query_pipeline), + &physics_hooks, + &event_handler, + ); + + if self.active_body_callback.is_some() { + let callback = self.active_body_callback.unwrap(); + for handle in self.island_manager.active_dynamic_bodies() { + // Send the active body event. + let mut active_body_info = ActiveBodyInfo::new(); + active_body_info.body_handle = rigid_body_handle_to_handle(*handle); + active_body_info.body_user_data = self.get_rigid_body_user_data(*handle); + + callback(self.handle, &active_body_info); + } + } + + if self.collision_event_callback.is_some() { + let callback = self.collision_event_callback.unwrap(); + while let Ok(collision_event) = collision_recv.try_recv() { + let handle1 = collision_event.collider1(); + let handle2 = collision_event.collider2(); + + // Handle the collision event. + let mut event_info = CollisionEventInfo::new(); + event_info.is_sensor = collision_event.sensor(); + event_info.is_removed = collision_event.removed(); + event_info.is_started = collision_event.started(); + event_info.collider1 = collider_handle_to_handle(handle1); + event_info.collider2 = collider_handle_to_handle(handle2); + event_info.user_data1 = self.get_collider_user_data(handle1); + event_info.user_data2 = self.get_collider_user_data(handle2); + + callback(self.handle, &event_info); + } + } + + if self.contact_force_event_callback.is_some() { + let callback = self.contact_force_event_callback.unwrap(); + while let Ok(contact_force_event) = contact_force_recv.try_recv() { + let collider1 = self.collider_set.get(contact_force_event.collider1).unwrap(); + let collider2 = self.collider_set.get(contact_force_event.collider2).unwrap(); + + // Handle the contact force event. + let mut event_info = ContactForceEventInfo::new(); + event_info.collider1 = collider_handle_to_handle(contact_force_event.collider1); + event_info.collider2 = collider_handle_to_handle(contact_force_event.collider2); + event_info.user_data1 = UserData::new(collider1.user_data); + event_info.user_data2 = UserData::new(collider2.user_data); + + let mut send_contact_points = callback(self.handle, &event_info); + + if send_contact_points && self.contact_point_callback.is_some() { + let contact_callback = self.contact_point_callback.unwrap(); + + let body1: &RigidBody = self.get_collider_rigid_body(collider1).unwrap(); + let body2: &RigidBody = self.get_collider_rigid_body(collider2).unwrap(); + + // Find the contact pair, if it exists, between two colliders + if let Some(contact_pair) = self.narrow_phase.contact_pair(contact_force_event.collider1, contact_force_event.collider2) { + let mut contact_info = ContactPointInfo::new(); + + let mut swap = false; + if contact_force_event.collider1 != contact_pair.collider1 { + assert!(contact_force_event.collider1 == contact_pair.collider2); + assert!(contact_force_event.collider2 == contact_pair.collider1); + swap = true; + } else { + assert!(contact_force_event.collider2 == contact_pair.collider2); + } + + // We may also read the contact manifolds to access the contact geometry. + for manifold in &contact_pair.manifolds { + let manifold_normal = manifold.data.normal; + contact_info.normal = Vector { x : manifold_normal.x, y : manifold_normal.y }; + + // Read the geometric contacts. + for contact_point in &manifold.points { + let collider_pos_1 = collider1.position() * contact_point.local_p1; + let collider_pos_2 = collider2.position() * contact_point.local_p2; + let point_velocity_1 = body1.velocity_at_point(&collider_pos_1); + let point_velocity_2 = body2.velocity_at_point(&collider_pos_2); + + if swap { + contact_info.local_pos_1 = Vector { x : collider_pos_2.x, y : collider_pos_2.y }; + contact_info.local_pos_2 = Vector { x : collider_pos_1.x, y : collider_pos_1.y }; + contact_info.velocity_pos_1 = Vector { x : point_velocity_2.x, y : point_velocity_2.y }; + contact_info.velocity_pos_2 = Vector { x : point_velocity_1.x, y : point_velocity_1.y }; + } else { + contact_info.local_pos_1 = Vector { x : collider_pos_1.x, y : collider_pos_1.y }; + contact_info.local_pos_2 = Vector { x : collider_pos_2.x, y : collider_pos_2.y }; + contact_info.velocity_pos_1 = Vector { x : point_velocity_1.x, y : point_velocity_1.y }; + contact_info.velocity_pos_2 = Vector { x : point_velocity_2.x, y : point_velocity_2.y }; + } + contact_info.distance = contact_point.dist; + contact_info.impulse = contact_point.data.impulse; + contact_info.tangent_impulse = contact_point.data.tangent_impulse; + + send_contact_points = contact_callback(self.handle, &contact_info, &event_info); + if !send_contact_points { + break; + } + } + + if !send_contact_points { + break; + } + } + } + } + } + } + } + + pub fn insert_collider(&mut self, collider : Collider, body_handle : Handle) -> Handle { + if body_handle.is_valid() + { + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + let collider_handle = self.collider_set.insert_with_parent(collider, rigid_body_handle, &mut self.rigid_body_set); + return collider_handle_to_handle(collider_handle); + } + else + { + let collider_handle = self.collider_set.insert(collider); + return collider_handle_to_handle(collider_handle); + } + } + + pub fn remove_collider(&mut self, handle : Handle) { + let collider_handle = handle_to_collider_handle(handle); + self.collider_set.remove(collider_handle + , &mut self.island_manager + , &mut self.rigid_body_set + , false + ); + } + + pub fn get_collider_user_data(&self, collider_handle : ColliderHandle) -> UserData { + let collider = self.collider_set.get(collider_handle); + if !collider.is_some() { + return invalid_user_data(); + } else{ + return UserData::new(collider.unwrap().user_data); + } + } + + pub fn get_collider_rigid_body(&self, collider : &Collider) -> Option<&RigidBody> { + let parent = collider.parent(); + if parent.is_some() { + return self.rigid_body_set.get(parent.unwrap()); + } else { + return None; + } + } + + pub fn insert_rigid_body(&mut self, rigid_body : RigidBody) -> Handle { + let body_handle = self.rigid_body_set.insert(rigid_body); + return rigid_body_handle_to_handle(body_handle); + } + + pub fn remove_rigid_body(&mut self, body_handle : Handle) { + let rigid_body_handle = handle_to_rigid_body_handle(body_handle); + self.rigid_body_set.remove(rigid_body_handle + , &mut self.island_manager + , &mut self.collider_set + , &mut self.impulse_joint_set + , &mut self.multibody_joint_set + , true + ); + } + + pub fn get_rigid_body_user_data(&self, rigid_body_handle : RigidBodyHandle) -> UserData { + let rigid_body = self.rigid_body_set.get(rigid_body_handle); + if !rigid_body.is_some() { + return invalid_user_data(); + } else{ + return UserData::new(rigid_body.unwrap().user_data); + } + } + + pub fn insert_joint(&mut self, body_handle_1 : Handle, body_handle_2 : Handle, joint : impl Into) -> Handle { + let rigid_body_1_handle = handle_to_rigid_body_handle(body_handle_1); + let rigid_body_2_handle = handle_to_rigid_body_handle(body_handle_2); + + let joint_handle = self.impulse_joint_set.insert(rigid_body_1_handle, rigid_body_2_handle, joint, true); + return joint_handle_to_handle(joint_handle); + } + + pub fn remove_joint(&mut self, handle : Handle) { + let joint_handle = handle_to_joint_handle(handle); + self.impulse_joint_set.remove(joint_handle, true); + } +} + +pub struct PhysicsEngine { + pub physics_worlds : Arena, + pub shapes : Arena, +} + +impl PhysicsEngine { + fn new() -> PhysicsEngine { + PhysicsEngine { + physics_worlds: Arena::new(), + shapes: Arena::new(), + } + } + + pub fn insert_world(&mut self, world : PhysicsWorld) -> Handle { + let world_handle = self.physics_worlds.insert(world); + return world_handle_to_handle(world_handle); + } + + pub fn remove_world(&mut self, handle : Handle) { + let world_handle = handle_to_world_handle(handle); + self.physics_worlds.remove(world_handle); + } + + pub fn get_world(&mut self, handle : Handle) -> &mut PhysicsWorld { + let world_handle = handle_to_world_handle(handle); + let world = self.physics_worlds.get_mut(world_handle); + assert!(world.is_some()); + return world.unwrap(); + } + + pub fn insert_shape(&mut self, shape : SharedShape) -> Handle { + let shape_handle = self.shapes.insert(shape); + return shape_handle_to_handle(shape_handle); + } + + pub fn remove_shape(&mut self, handle : Handle) { + let shape_handle = handle_to_shape_handle(handle); + self.shapes.remove(shape_handle); + } + + pub fn get_shape(&mut self, handle : Handle) -> &SharedShape { + let shape_handle = handle_to_shape_handle(handle); + let shape = self.shapes.get(shape_handle); + assert!(shape.is_some()); + return shape.unwrap(); + } +} + +lazy_static! { + pub static ref SINGLETON: Mutex = Mutex::new(PhysicsEngine::new()); +} + +#[no_mangle] +pub extern "C" fn world_create(settings: &WorldSettings) -> Handle { + let physics_world = PhysicsWorld::new(settings); + let mut physics_engine = SINGLETON.lock().unwrap(); + let world_handle = physics_engine.insert_world(physics_world); + let physics_world = physics_engine.get_world(world_handle); + physics_world.handle = world_handle; + return world_handle; +} + +#[no_mangle] +pub extern "C" fn world_destroy(world_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.handle = invalid_handle(); + physics_engine.remove_world(world_handle); +} + +#[no_mangle] +pub extern "C" fn world_step(world_handle : Handle, settings : &SimulationSettings) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.step(settings); +} + +#[no_mangle] +pub extern "C" fn world_set_active_body_callback(world_handle : Handle, callback : ActiveBodyCallback) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world: &mut PhysicsWorld = physics_engine.get_world(world_handle); + physics_world.active_body_callback = callback; +} + +#[no_mangle] +pub extern "C" fn world_set_body_collision_filter_callback(world_handle : Handle, callback : CollisionFilterCallback) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.collision_filter_body_callback = callback; +} + +#[no_mangle] +pub extern "C" fn world_set_sensor_collision_filter_callback(world_handle : Handle, callback : CollisionFilterCallback) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.collision_filter_sensor_callback = callback; +} + +#[no_mangle] +pub extern "C" fn world_set_collision_event_callback(world_handle : Handle, callback : CollisionEventCallback) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.collision_event_callback = callback; +} + +#[no_mangle] +pub extern "C" fn world_set_contact_force_event_callback(world_handle : Handle, callback : ContactForceEventCallback) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.contact_force_event_callback = callback; +} + +#[no_mangle] +pub extern "C" fn world_set_contact_point_callback(world_handle : Handle, callback : ContactPointCallback) { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + physics_world.contact_point_callback = callback; +} diff --git a/src/rapier2d-wrapper/src/query.rs b/src/rapier2d-wrapper/src/query.rs new file mode 100644 index 0000000..31191a3 --- /dev/null +++ b/src/rapier2d-wrapper/src/query.rs @@ -0,0 +1,391 @@ +use rapier2d::parry; +use rapier2d::prelude::*; +use crate::handle::*; +use crate::user_data::*; +use crate::physics_world::*; +use crate::vector::Vector; + +#[repr(C)] +pub struct RayHitInfo { + position: Vector, + normal: Vector, + collider: Handle, + user_data: UserData, +} + +#[repr(C)] +pub struct PointHitInfo { + collider: Handle, + user_data: UserData, +} + +#[repr(C)] +pub struct ShapeCastResult { + collided: bool, + toi : Real, + witness1 : Vector, + witness2 : Vector, + normal1 : Vector, + normal2 : Vector, + collider: Handle, + user_data: UserData +} + +impl ShapeCastResult { + fn new() -> ShapeCastResult { + ShapeCastResult { + collided : false, + toi : 1.0, + collider : invalid_handle(), + witness1 : Vector{ x: 0.0, y: 0.0 }, + witness2 : Vector{ x: 0.0, y: 0.0 }, + normal1 : Vector{ x: 0.0, y: 0.0 }, + normal2 : Vector{ x: 0.0, y: 0.0 }, + user_data: UserData { part1: 0, part2: 0 } + } + } +} + +#[repr(C)] +pub struct ContactResult { + collided: bool, + within_margin: bool, + distance: Real, + point1 : Vector, + point2 : Vector, + normal1 : Vector, + normal2 : Vector +} + +impl ContactResult { + fn new() -> ContactResult { + ContactResult { + collided : false, + within_margin: false, + distance : 0.0, + point1 : Vector{ x: 0.0, y: 0.0 }, + point2 : Vector{ x: 0.0, y: 0.0 }, + normal1 : Vector{ x: 0.0, y: 0.0 }, + normal2 : Vector{ x: 0.0, y: 0.0 } + } + } +} + +#[repr(C)] +pub struct QueryExcludedInfo { + query_collision_layer_mask: u32, + query_canvas_instance_id: u64, + // Pointer to array of objects + query_exclude: * mut Handle, + // Size of query_exclude array + query_exclude_size: u32, + query_exclude_body: i64, +} + +#[no_mangle] +pub extern "C" fn default_query_excluded_info() -> QueryExcludedInfo { + QueryExcludedInfo { + query_collision_layer_mask: 0, + query_canvas_instance_id: 0, + query_exclude: &mut Handle::default(), + query_exclude_size: 0, + query_exclude_body: 0, + } +} + +type QueryHandleExcludedCallback = Option bool>; + +#[no_mangle] +pub extern "C" fn intersect_ray(world_handle : Handle, from : &Vector, dir : &Vector, length: Real, collide_with_body: bool, collide_with_area: bool, hit_from_inside: bool, hit_info : &mut RayHitInfo, handle_excluded_callback: QueryHandleExcludedCallback, handle_excluded_info: &QueryExcludedInfo) -> bool { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + + let ray = Ray::new(point![from.x, from.y], vector![dir.x, dir.y]); + let solid = true; + let mut filter = QueryFilter::new(); + + if !collide_with_body { + filter = filter.exclude_solids(); + } + if !collide_with_area { + filter = filter.exclude_sensors(); + } + + let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { + if handle_excluded_callback.is_some() { + return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle), &handle_excluded_info); + } + return true; + }; + + filter.predicate = Some(&predicate); + + let mut result = false; + physics_world.query_pipeline.intersections_with_ray(&physics_world.rigid_body_set, &physics_world.collider_set, &ray, length, solid, filter, + |handle, intersection| { + // Callback called on each collider hit by the ray. + + if hit_from_inside || intersection.toi != 0.0 { + result = true; + + let hit_point = ray.point_at(intersection.toi); + let hit_normal = intersection.normal; + hit_info.position = Vector { + x: hit_point.x, + y: hit_point.y, + }; + hit_info.normal = Vector { + x: hit_normal.x, + y: hit_normal.y, + }; + hit_info.collider = collider_handle_to_handle(handle); + hit_info.user_data = physics_world.get_collider_user_data(handle); + return false; // We found a collision hit. + } + true // Continue to search. + }, + ); + + return result; +} + +#[no_mangle] +pub extern "C" fn intersect_point(world_handle : Handle, position : &Vector, collide_with_body: bool, collide_with_area: bool, hit_info_array : *mut PointHitInfo, hit_info_length : usize, handle_excluded_callback: QueryHandleExcludedCallback, handle_excluded_info: &QueryExcludedInfo) -> usize { + let mut physics_engine = SINGLETON.lock().unwrap(); + let physics_world = physics_engine.get_world(world_handle); + + let point = Point::new(position.x, position.y); + let mut filter = QueryFilter::new(); + + if !collide_with_body { + filter = filter.exclude_solids(); + } + if !collide_with_area { + filter = filter.exclude_sensors(); + } + + let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { + if handle_excluded_callback.is_some() { + return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle), &handle_excluded_info); + } + return true; + }; + + filter.predicate = Some(&predicate); + + assert!(hit_info_length > 0); + let hit_info_slice_opt; + unsafe { + hit_info_slice_opt = Some(std::slice::from_raw_parts_mut(hit_info_array, hit_info_length)); + } + assert!(hit_info_slice_opt.is_some()); + let hit_info_slice = hit_info_slice_opt.unwrap(); + + let mut cpt_hit = 0; + physics_world.query_pipeline.intersections_with_point( &physics_world.rigid_body_set, &physics_world.collider_set, &point, filter, + |handle| { + // Callback called on each collider hit by the ray. + hit_info_slice[cpt_hit].collider = collider_handle_to_handle(handle); + hit_info_slice[cpt_hit].user_data = physics_world.get_collider_user_data(handle); + cpt_hit += 1; + let keep_searching = cpt_hit < hit_info_length; + keep_searching // Continue to search collisions if we still have space for results. + }); + + return cpt_hit; +} + +#[no_mangle] +pub extern "C" fn shape_casting(world_handle : Handle, motion : &Vector, position: &Vector, rotation: Real, shape_handle : Handle, collide_with_body: bool, collide_with_area: bool, handle_excluded_callback: QueryHandleExcludedCallback, handle_excluded_info: &QueryExcludedInfo) -> ShapeCastResult { + let mut physics_engine = SINGLETON.lock().unwrap(); + + let shared_shape = physics_engine.get_shape(shape_handle).clone(); + + let physics_world = physics_engine.get_world(world_handle); + + let shape_vel = vector![motion.x, motion.y]; + let shape_transform = Isometry::new(vector![position.x, position.y], rotation); + + let mut filter = QueryFilter::new(); + + if !collide_with_body { + filter = filter.exclude_solids(); + } + if !collide_with_area { + filter = filter.exclude_sensors(); + } + + let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { + if handle_excluded_callback.is_some() { + return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle), &handle_excluded_info); + } + return true; + }; + + filter.predicate = Some(&predicate); + + let mut result = ShapeCastResult::new(); + + if let Some((collider_handle, hit)) = physics_world.query_pipeline.cast_shape( + &physics_world.rigid_body_set, &physics_world.collider_set, &shape_transform, &shape_vel, shared_shape.as_ref(), 1.0, false, filter + ) { + result.collided = true; + result.toi = hit.toi; + result.witness1 = Vector{ x: hit.witness1.x, y: hit.witness1.y }; + result.witness2 = Vector{ x: hit.witness2.x, y: hit.witness2.y }; + result.normal1 = Vector{ x: hit.normal1.x, y: hit.normal1.y }; + result.normal2 = Vector{ x: hit.normal2.x, y: hit.normal2.y }; + result.collider = collider_handle_to_handle(collider_handle); + result.user_data = physics_world.get_collider_user_data(collider_handle); + return result; + } + return result; +} + +#[no_mangle] +pub extern "C" fn intersect_shape(world_handle : Handle, position: &Vector, rotation: Real, shape_handle : Handle, collide_with_body: bool, collide_with_area: bool, hit_info_array : *mut PointHitInfo, hit_info_length : usize, handle_excluded_callback: QueryHandleExcludedCallback, handle_excluded_info: &QueryExcludedInfo) -> usize { + let mut physics_engine = SINGLETON.lock().unwrap(); + + let shared_shape = physics_engine.get_shape(shape_handle).clone(); + + let physics_world = physics_engine.get_world(world_handle); + + let shape_transform = Isometry::new(vector![position.x, position.y], rotation); + + let mut filter = QueryFilter::new(); + + if !collide_with_body { + filter = filter.exclude_solids(); + } + if !collide_with_area { + filter = filter.exclude_sensors(); + } + + let predicate = |handle: ColliderHandle, _collider: &Collider| -> bool { + if handle_excluded_callback.is_some() { + return !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(handle), &physics_world.get_collider_user_data(handle), &handle_excluded_info); + } + return true; + }; + + filter.predicate = Some(&predicate); + + assert!(hit_info_length > 0); + let hit_info_slice_opt; + unsafe { + hit_info_slice_opt = Some(std::slice::from_raw_parts_mut(hit_info_array, hit_info_length)); + } + assert!(hit_info_slice_opt.is_some()); + let hit_info_slice = hit_info_slice_opt.unwrap(); + + let mut cpt_hit = 0; + physics_world.query_pipeline.intersections_with_shape ( + &physics_world.rigid_body_set, &physics_world.collider_set, &shape_transform, shared_shape.as_ref(), filter, + |handle| { + // Callback called on each collider hit by the ray. + hit_info_slice[cpt_hit].collider = collider_handle_to_handle(handle); + hit_info_slice[cpt_hit].user_data = physics_world.get_collider_user_data(handle); + cpt_hit += 1; + let keep_searching = cpt_hit < hit_info_length; + keep_searching // Continue to search collisions if we still have space for results. + } + ); + + return cpt_hit; +} + +#[no_mangle] +pub extern "C" fn intersect_aabb(world_handle : Handle, aabb_min : &Vector, aabb_max : &Vector, collide_with_body: bool, collide_with_area: bool, hit_info_array : *mut PointHitInfo, hit_info_length : usize, handle_excluded_callback: QueryHandleExcludedCallback, handle_excluded_info: &QueryExcludedInfo) -> usize { + let mut physics_engine = SINGLETON.lock().unwrap(); + + let physics_world = physics_engine.get_world(world_handle); + + // let aabb_transform = Isometry::new(vector![position.x, position.y], rotation); + let aabb_min_point = Point::new(aabb_min.x, aabb_min.y); + let aabb_max_point = Point::new(aabb_max.x, aabb_max.y); + + // let transformed_aabb_min = aabb_transform * aabb_min_point; + // let transformed_aabb_max = aabb_transform * aabb_max_point; + + let aabb = Aabb { + mins: aabb_min_point, + maxs: aabb_max_point, + }; + + assert!(hit_info_length > 0); + let hit_info_slice_opt; + unsafe { + hit_info_slice_opt = Some(std::slice::from_raw_parts_mut(hit_info_array, hit_info_length)); + } + assert!(hit_info_slice_opt.is_some()); + let hit_info_slice = hit_info_slice_opt.unwrap(); + + let mut cpt_hit = 0; + physics_world.query_pipeline.colliders_with_aabb_intersecting_aabb(&aabb, + |handle| { + + let mut valid_hit = false; + if let Some(collider) = physics_world.collider_set.get(*handle) { + + // type filder + if collider.is_sensor() && collide_with_area { + valid_hit = true; + } else if !collider.is_sensor() && collide_with_body { + valid_hit = true; + } + + if valid_hit && handle_excluded_callback.is_some(){ + valid_hit = !handle_excluded_callback.unwrap()(world_handle, collider_handle_to_handle(*handle), &physics_world.get_collider_user_data(*handle), &handle_excluded_info); + } + } + + if !valid_hit { + return true; // continue + } + + // Callback called on each collider hit by the ray. + hit_info_slice[cpt_hit].collider = collider_handle_to_handle(*handle); + hit_info_slice[cpt_hit].user_data = physics_world.get_collider_user_data(*handle); + cpt_hit += 1; + let keep_searching = cpt_hit < hit_info_length; + keep_searching // Continue to search collisions if we still have space for results. + }); + + return cpt_hit; +} + +#[no_mangle] +pub extern "C" fn shapes_contact(world_handle : Handle, shape_handle1 : Handle, position1: &Vector, rotation1: Real, shape_handle2 : Handle, position2: &Vector, rotation2: Real, margin: Real) -> ContactResult { + let mut physics_engine = SINGLETON.lock().unwrap(); + + let physics_world = physics_engine.get_world(world_handle); + + let prediction = Real::max(physics_world.solver_prediction_distance, margin); + + let shared_shape1 = physics_engine.get_shape(shape_handle1).clone(); + let shared_shape2 = physics_engine.get_shape(shape_handle2).clone(); + + let shape_transform1 = Isometry::new(vector![position1.x, position1.y], rotation1); + let shape_transform2 = Isometry::new(vector![position2.x, position2.y], rotation2); + + let mut result = ContactResult::new(); + if let Ok(Some(contact)) = parry::query::contact( + &shape_transform1, shared_shape1.as_ref(), &shape_transform2, shared_shape2.as_ref(), prediction + ) { + // the distance is negative if there is intersection + // and positive if the objects are separated by distance less than margin + result.distance = contact.dist; + if contact.dist <= 0.0 { + result.within_margin = false; + } else { + result.within_margin = true; + } + result.collided = true; + result.point1 = Vector{ x: contact.point1.x + prediction * contact.normal1.x, y: contact.point1.y + prediction * contact.normal1.y }; + result.point2 = Vector{ x: contact.point2.x, y: contact.point2.y }; + result.normal1 = Vector{ x: contact.normal1.x, y: contact.normal1.y }; + result.normal2 = Vector{ x: contact.normal2.x, y: contact.normal2.y }; + return result; + } + return result; +} diff --git a/src/rapier2d-wrapper/src/settings.rs b/src/rapier2d-wrapper/src/settings.rs new file mode 100644 index 0000000..5782782 --- /dev/null +++ b/src/rapier2d-wrapper/src/settings.rs @@ -0,0 +1,78 @@ +use rapier2d::prelude::*; +use crate::vector::Vector; + +#[repr(C)] +pub struct WorldSettings { + pub sleep_linear_threshold: Real, + pub sleep_angular_threshold: Real, + pub sleep_time_until_sleep: Real, + pub solver_prediction_distance : Real, +} + +#[no_mangle] +pub extern "C" fn default_world_settings() -> WorldSettings { + WorldSettings { + sleep_linear_threshold : 0.1, + sleep_angular_threshold : 0.1, + sleep_time_until_sleep : 1.0, + solver_prediction_distance : 0.002, + } +} + +#[repr(C)] +pub struct SimulationSettings { + /// The timestep length (default: `1.0 / 60.0`) + pub dt: Real, + /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`) + /// + /// When CCD with multiple substeps is enabled, the timestep is subdivided + /// into smaller pieces. This timestep subdivision won't generate timestep + /// lengths smaller than `min_ccd_dt`. + /// + /// Setting this to a large value will reduce the opportunity to performing + /// CCD substepping, resulting in potentially more time dropped by the + /// motion-clamping mechanism. Setting this to an very small value may lead + /// to numerical instabilities. + pub min_ccd_dt: Real, + + /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) + /// will be compensated for during the velocity solve. + /// (default `0.8`). + pub erp: Real, + /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. + /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations + /// before stabilization). + /// (default `0.25`). + pub damping_ratio: Real, + + /// 0-1: multiplier for how much of the joint violation + /// will be compensated for during the velocity solve. + /// (default `1.0`). + pub joint_erp: Real, + + /// The fraction of critical damping applied to the joint for constraints regularization. + /// (default `0.25`). + pub joint_damping_ratio: Real, + + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). + pub allowed_linear_error: Real, + /// Maximum amount of penetration the solver will attempt to resolve in one timestep. + pub max_penetration_correction: Real, + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). + pub prediction_distance: Real, + /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). + pub max_velocity_iterations: usize, + /// Maximum number of iterations performed to solve friction constraints (default: `8`). + pub max_velocity_friction_iterations: usize, + /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). + pub max_stabilization_iterations: usize, + /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, + /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). + pub interleave_restitution_and_friction_resolution: bool, + /// Minimum number of dynamic bodies in each active island (default: `128`). + pub min_island_size: usize, + /// Maximum number of substeps performed by the solver (default: `1`). + pub max_ccd_substeps: usize, + + pub gravity : Vector, +} diff --git a/src/rapier2d-wrapper/src/shape.rs b/src/rapier2d-wrapper/src/shape.rs new file mode 100644 index 0000000..a8cb3e0 --- /dev/null +++ b/src/rapier2d-wrapper/src/shape.rs @@ -0,0 +1,107 @@ +use rapier2d::prelude::*; +use crate::handle::*; +use crate::vector::Vector; +use crate::physics_world::*; + +pub fn point_array_to_vec(data : &Vector, data_count : usize) -> Vec::> { + let mut vec = Vec::>::with_capacity(data_count); + unsafe { + let data_raw = std::slice::from_raw_parts(data, data_count); + for point in data_raw { + vec.push(Point:: { coords : vector![point.x, point.y] }); + } + } + return vec; +} + +#[repr(C)] +pub struct ShapeInfo { + handle: Handle, + position : Vector, + rotation : Real, +} +#[no_mangle] +pub extern "C" fn shape_create_box(size : &Vector) -> Handle { + let shape = SharedShape::cuboid(0.5 * size.x, 0.5 * size.y); + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_create_halfspace(normal : &Vector) -> Handle { + let shape = SharedShape::halfspace(UnitVector::new_normalize(vector![normal.x, normal.y])); + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_create_circle(radius : Real) -> Handle { + let shape = SharedShape::ball(radius); + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_create_capsule(half_height : Real, radius : Real) -> Handle { + let top_circle = SharedShape::ball(radius); + let top_circle_position = Isometry::new(vector![0.0, -half_height], 0.0); + let bottom_circle = SharedShape::ball(radius); + let bottom_circle_position = Isometry::new(vector![0.0, half_height], 0.0); + let square = SharedShape::cuboid(0.5 * radius, 0.5 * (half_height - radius)); + let square_pos = Isometry::new(vector![0.0, 0.0], 0.0); + let mut shapes_vec = Vec::<(Isometry, SharedShape)>::new(); + shapes_vec.push((top_circle_position, top_circle)); + shapes_vec.push((bottom_circle_position, bottom_circle)); + shapes_vec.push((square_pos, square)); + let shape = SharedShape::compound(shapes_vec); + // For now create the shape using circles and squares as the default capsule is buggy + // in case of distance checking(returns invalid distances when close to the ends) + // overall results in a 1.33x decrease in performance + // TODO only do this in case of static objects? + //let shape = SharedShape::capsule_y(half_height, radius); + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_create_convex_polyline(points : &Vector, point_count : usize) -> Handle { + let points_vec = point_array_to_vec(points, point_count); + let shape_data = SharedShape::convex_polyline(points_vec); + if shape_data.is_none() { + return Handle::default(); + } + let shape = shape_data.unwrap(); + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_create_convave_polyline(points : &Vector, point_count : usize) -> Handle { + let points_vec = point_array_to_vec(points, point_count); + let shape = SharedShape::polyline(points_vec, None); + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_create_compound(shapes : &ShapeInfo, shape_count : usize) -> Handle { + let mut shapes_vec = Vec::<(Isometry, SharedShape)>::with_capacity(shape_count); + let mut physics_engine = SINGLETON.lock().unwrap(); + unsafe { + let data_raw = std::slice::from_raw_parts(shapes, shape_count); + for shape_info in data_raw { + let shape = physics_engine.get_shape(shape_info.handle); + let pos = vector![shape_info.position.x, shape_info.position.y]; + shapes_vec.push((Isometry::new(pos, shape_info.rotation), shape.clone())); + } + } + + let shape = SharedShape::compound(shapes_vec); + return physics_engine.insert_shape(shape); +} + +#[no_mangle] +pub extern "C" fn shape_destroy(shape_handle : Handle) { + let mut physics_engine = SINGLETON.lock().unwrap(); + return physics_engine.remove_shape(shape_handle); +} diff --git a/src/rapier2d-wrapper/src/user_data.rs b/src/rapier2d-wrapper/src/user_data.rs new file mode 100644 index 0000000..3047e9b --- /dev/null +++ b/src/rapier2d-wrapper/src/user_data.rs @@ -0,0 +1,40 @@ +#[repr(C)] +#[derive(Copy, Clone, Eq, Hash, PartialEq)] +pub struct UserData { + pub part1 : u64, + pub part2 : u64, +} + +impl UserData { + pub fn new(data : u128) -> UserData { + let data2 : u128 = data >> 64; + let data1 : u128 = data - (data2 << 64); + UserData { + part1 : data1.try_into().unwrap(), + part2 : data2.try_into().unwrap(), + } + } + + pub fn is_valid(&self) -> bool { + return (self.part1 != u64::MAX) && (self.part2 != u64::MAX); + } + + pub fn get_data(&self) -> u128 { + let data1 : u128 = self.part1.into(); + let data2 : u128 = self.part2.into(); + return data1 + (data2 << 64); + } +} + +#[no_mangle] +pub extern "C" fn invalid_user_data() -> UserData { + UserData { + part1 : u64::MAX, + part2 : u64::MAX, + } +} + +#[no_mangle] +pub extern "C" fn is_user_data_valid(user_data : UserData) -> bool { + return user_data.is_valid(); +} diff --git a/src/rapier2d-wrapper/src/vector.rs b/src/rapier2d-wrapper/src/vector.rs new file mode 100644 index 0000000..d1c575b --- /dev/null +++ b/src/rapier2d-wrapper/src/vector.rs @@ -0,0 +1,7 @@ +use rapier2d::prelude::*; + +#[repr(C)] +pub struct Vector { + pub x : Real, + pub y : Real, +} diff --git a/src/rapier_constraint_2d.h b/src/rapier_constraint_2d.h deleted file mode 100644 index d21c404..0000000 --- a/src/rapier_constraint_2d.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef RAPIER_CONSTRAINT_2D_H -#define RAPIER_CONSTRAINT_2D_H - -#include "rapier_body_2d.h" - -using namespace godot; - -class RapierConstraint2D { - RapierBody2D **_body_ptr; - int _body_count; - bool disabled_collisions_between_bodies = true; - - RID rid; - -protected: - RapierConstraint2D(RapierBody2D **p_body_ptr = nullptr, int p_body_count = 0) { - _body_ptr = p_body_ptr; - _body_count = p_body_count; - } - -public: - _FORCE_INLINE_ void set_rid(const RID &p_rid) { rid = p_rid; } - _FORCE_INLINE_ RID get_rid() const { return rid; } - - _FORCE_INLINE_ RapierBody2D **get_body_ptr() const { return _body_ptr; } - _FORCE_INLINE_ int get_body_count() const { return _body_count; } - - _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } - _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } - - virtual ~RapierConstraint2D() {} -}; - -#endif // RAPIER_CONSTRAINT_2D_H diff --git a/src/rapier_joints_2d.cpp b/src/rapier_joints_2d.cpp deleted file mode 100644 index e3686a2..0000000 --- a/src/rapier_joints_2d.cpp +++ /dev/null @@ -1,200 +0,0 @@ -#include "rapier_joints_2d.h" - -#include "rapier_space_2d.h" - -void RapierJoint2D::copy_settings_from(RapierJoint2D *p_joint) { - set_rid(p_joint->get_rid()); - set_max_force(p_joint->get_max_force()); - set_bias(p_joint->get_bias()); - set_max_bias(p_joint->get_max_bias()); - disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); -} - -RapierJoint2D::~RapierJoint2D() { - if (rapier2d::is_handle_valid(handle)) { - ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); - rapier2d::joint_destroy(space_handle, handle); - } - - /*for (int i = 0; i < get_body_count(); i++) { - RapierBody2D *body = get_body_ptr()[i]; - if (body) { - body->remove_constraint(this, i); - } - }*/ -}; - -////////////////////////////////////////////// - -void RapierPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: { - angular_limit_upper = p_value; - } break; - case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: { - angular_limit_lower = p_value; - } break; - case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: { - motor_target_velocity = p_value; - } break; - case PhysicsServer2D::PIN_JOINT_SOFTNESS: { - WARN_PRINT_ONCE("PIN_JOINT_SOFTNESS is unused"); - return; - } - } - ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); - ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); - rapier2d::joint_change_revolute_params(space_handle, handle, angular_limit_lower, angular_limit_upper, angular_limit_enabled, motor_target_velocity, motor_enabled); -} - -real_t RapierPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const { - switch (p_param) { - case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: { - return angular_limit_upper; - } - case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: { - return angular_limit_lower; - } - case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: { - return motor_target_velocity; - } - case PhysicsServer2D::PIN_JOINT_SOFTNESS: { - WARN_PRINT_ONCE("PIN_JOINT_SOFTNESS is unused"); - return 0.0; - } - } - ERR_FAIL_V(0); -} - -void RapierPinJoint2D::set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled) { - switch (p_flag) { - case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: { - angular_limit_enabled = p_enabled; - } break; - case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: { - motor_enabled = p_enabled; - } break; - } - ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); - ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); - rapier2d::joint_change_revolute_params(space_handle, handle, angular_limit_lower, angular_limit_upper, angular_limit_enabled, motor_target_velocity, motor_enabled); -} - -bool RapierPinJoint2D::get_flag(PhysicsServer2D::PinJointFlag p_flag) const { - switch (p_flag) { - case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: { - return angular_limit_enabled; - } - case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: { - return motor_enabled; - } - } - ERR_FAIL_V(0); -} - -RapierPinJoint2D::RapierPinJoint2D(const Vector2 &p_pos, RapierBody2D *p_body_a, RapierBody2D *p_body_b) : - RapierJoint2D(_arr, p_body_b ? 2 : 1) { - A = p_body_a; - B = p_body_b; - - Vector2 anchor_A = p_body_a->get_inv_transform().xform(p_pos); - Vector2 anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; - - rapier2d::Vector rapier_anchor_A = { anchor_A.x, anchor_A.y }; - rapier2d::Vector rapier_anchor_B = { anchor_B.x, anchor_B.y }; - - ERR_FAIL_COND(!p_body_a->get_space()); - ERR_FAIL_COND(p_body_a->get_space() != p_body_b->get_space()); - space_handle = p_body_a->get_space()->get_handle(); - ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); - handle = rapier2d::joint_create_revolute(space_handle, p_body_a->get_body_handle(), p_body_b->get_body_handle(), &rapier_anchor_A, &rapier_anchor_B, angular_limit_lower, angular_limit_upper, angular_limit_enabled, motor_target_velocity, motor_enabled); - ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); - - /*p_body_a->add_constraint(this, 0); - if (p_body_b) { - p_body_b->add_constraint(this, 1); - }*/ -} - -////////////////////////////////////////////// - -RapierGrooveJoint2D::RapierGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RapierBody2D *p_body_a, RapierBody2D *p_body_b) : - RapierJoint2D(_arr, 2) { - A = p_body_a; - B = p_body_b; - - Vector2 point_A_1 = A->get_inv_transform().xform(p_a_groove1); - Vector2 point_A_2 = A->get_inv_transform().xform(p_a_groove2); - - Vector2 anchor_B = B->get_inv_transform().xform(p_b_anchor); - - Vector2 axis = (point_A_2 - point_A_1).normalized(); - real_t length = (point_A_2 - point_A_1).length(); - - rapier2d::Vector rapier_anchor_A = { point_A_1.x, point_A_1.y }; - rapier2d::Vector rapier_anchor_B = { anchor_B.x, anchor_B.y }; - - rapier2d::Vector rapier_axis = { axis.x, axis.y }; - rapier2d::Vector rapier_limits = { 0.0, length }; - - ERR_FAIL_COND(!p_body_a->get_space()); - ERR_FAIL_COND(p_body_a->get_space() != p_body_b->get_space()); - space_handle = p_body_a->get_space()->get_handle(); - ERR_FAIL_COND(!rapier2d::is_handle_valid(space_handle)); - - handle = rapier2d::joint_create_prismatic(space_handle, p_body_a->get_body_handle(), p_body_b->get_body_handle(), &rapier_axis, &rapier_anchor_A, &rapier_anchor_B, &rapier_limits); - ERR_FAIL_COND(!rapier2d::is_handle_valid(handle)); - - //A->add_constraint(this, 0); - //B->add_constraint(this, 1); -} - -////////////////////////////////////////////// - -void RapierDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { - rest_length = p_value; - } break; - case PhysicsServer2D::DAMPED_SPRING_DAMPING: { - damping = p_value; - } break; - case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { - stiffness = p_value; - } break; - } -} - -real_t RapierDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const { - switch (p_param) { - case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { - return rest_length; - } break; - case PhysicsServer2D::DAMPED_SPRING_DAMPING: { - return damping; - } break; - case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { - return stiffness; - } break; - } - - ERR_FAIL_V(0); -} - -RapierDampedSpringJoint2D::RapierDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RapierBody2D *p_body_a, RapierBody2D *p_body_b) : - RapierJoint2D(_arr, 2) { - A = p_body_a; - B = p_body_b; - - Vector2 anchor_A = A->get_inv_transform().xform(p_anchor_a); - Vector2 anchor_B = B->get_inv_transform().xform(p_anchor_b); - - rest_length = p_anchor_a.distance_to(p_anchor_b); - - // TODO: create rapier joint when available - // See https://github.com/dimforge/rapier/issues/241 - ERR_FAIL_MSG("Spring joints not supported for now"); - - //A->add_constraint(this, 0); - //B->add_constraint(this, 1); -} diff --git a/src/rapier_joints_2d.h b/src/rapier_joints_2d.h deleted file mode 100644 index 0395ba8..0000000 --- a/src/rapier_joints_2d.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef RAPIER_JOINTS_2D_H -#define RAPIER_JOINTS_2D_H - -#include "rapier_body_2d.h" -#include "rapier_constraint_2d.h" - -using namespace godot; - -class RapierJoint2D : public RapierConstraint2D { - real_t bias = 0; - real_t max_bias = 3.40282e+38; - real_t max_force = 3.40282e+38; - -protected: - rapier2d::Handle space_handle = rapier2d::invalid_handle(); - rapier2d::Handle handle = rapier2d::invalid_handle(); - -public: - _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } - _FORCE_INLINE_ real_t get_max_force() const { return max_force; } - - _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } - _FORCE_INLINE_ real_t get_bias() const { return bias; } - - _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } - _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } - - void copy_settings_from(RapierJoint2D *p_joint); - - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } - RapierJoint2D(RapierBody2D **p_body_ptr = nullptr, int p_body_count = 0) : - RapierConstraint2D(p_body_ptr, p_body_count) {} - - virtual ~RapierJoint2D(); -}; - -class RapierPinJoint2D : public RapierJoint2D { - union { - struct { - RapierBody2D *A; - RapierBody2D *B; - }; - - RapierBody2D *_arr[2] = { nullptr, nullptr }; - }; - real_t angular_limit_lower = 0.0; - real_t angular_limit_upper = 0.0; - real_t motor_target_velocity = 0.0; - bool motor_enabled = false; - bool angular_limit_enabled = false; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } - - void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::PinJointParam p_param) const; - - void set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled); - bool get_flag(PhysicsServer2D::PinJointFlag p_flag) const; - - RapierPinJoint2D(const Vector2 &p_pos, RapierBody2D *p_body_a, RapierBody2D *p_body_b = nullptr); -}; - -class RapierGrooveJoint2D : public RapierJoint2D { - union { - struct { - RapierBody2D *A; - RapierBody2D *B; - }; - - RapierBody2D *_arr[2] = { nullptr, nullptr }; - }; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } - - RapierGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RapierBody2D *p_body_a, RapierBody2D *p_body_b); -}; - -class RapierDampedSpringJoint2D : public RapierJoint2D { - union { - struct { - RapierBody2D *A; - RapierBody2D *B; - }; - - RapierBody2D *_arr[2] = { nullptr, nullptr }; - }; - - real_t rest_length = 0.0; - real_t damping = 1.5; - real_t stiffness = 20.0; - -public: - virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } - - void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; - - RapierDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RapierBody2D *p_body_a, RapierBody2D *p_body_b); -}; - -#endif // RAPIER_JOINTS_2D_H diff --git a/src/rapier_shape_2d.cpp b/src/rapier_shape_2d.cpp deleted file mode 100644 index ad17224..0000000 --- a/src/rapier_shape_2d.cpp +++ /dev/null @@ -1,440 +0,0 @@ -#include "rapier_shape_2d.h" - -void RapierShape2D::configure(const Rect2 &p_aabb) { - aabb = p_aabb; - configured = true; - for (const KeyValue &E : owners) { - RapierShapeOwner2D *co = const_cast(E.key); - co->_shape_changed(this); - } -} - -void RapierShape2D::destroy_rapier_shape() { - if (rapier2d::is_handle_valid(shape_handle)) { - rapier2d::shape_destroy(shape_handle); - shape_handle = rapier2d::invalid_handle(); - } -} - -rapier2d::Handle RapierShape2D::get_rapier_shape() { - if (!rapier2d::is_handle_valid(shape_handle)) { - shape_handle = create_rapier_shape(); - } - - return shape_handle; -} - -void RapierShape2D::add_owner(RapierShapeOwner2D *p_owner) { - HashMap::Iterator E = owners.find(p_owner); - if (E) { - E->value++; - } else { - owners[p_owner] = 1; - } -} - -void RapierShape2D::remove_owner(RapierShapeOwner2D *p_owner) { - HashMap::Iterator E = owners.find(p_owner); - ERR_FAIL_COND(!E); - E->value--; - if (E->value == 0) { - owners.remove(E); - } -} - -bool RapierShape2D::is_owner(RapierShapeOwner2D *p_owner) const { - return owners.has(p_owner); -} - -const HashMap &RapierShape2D::get_owners() const { - return owners; -} - -RapierShape2D::~RapierShape2D() { - ERR_FAIL_COND(owners.size()); - destroy_rapier_shape(); -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -rapier2d::Handle RapierWorldBoundaryShape2D::create_rapier_shape() const { - rapier2d::Vector v = { normal.x, normal.y }; - return rapier2d::shape_create_halfspace(&v); -} - -void RapierWorldBoundaryShape2D::apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const { - position.x += normal.x * d; - position.y += normal.y * d; -} - -void RapierWorldBoundaryShape2D::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); - Array arr = p_data; - ERR_FAIL_COND(arr.size() != 2); - normal = arr[0]; - d = arr[1]; - configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); -} - -Variant RapierWorldBoundaryShape2D::get_data() const { - Array arr; - arr.resize(2); - arr[0] = normal; - arr[1] = d; - return arr; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -// void RapierSeparationRayShape2D::set_data(const Variant &p_data) { -// Dictionary d = p_data; -// length = d["length"]; -// slide_on_slope = d["slide_on_slope"]; -// configure(Rect2(0, 0, 0.001, length)); -// } - -// Variant RapierSeparationRayShape2D::get_data() const { -// Dictionary d; -// d["length"] = length; -// d["slide_on_slope"] = slide_on_slope; -// return d; -// } - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -rapier2d::Handle RapierSegmentShape2D::create_rapier_shape() const { - Vector2 direction = b - a; - direction.normalize(); - - Vector2 perpendicular = Vector2(-direction.y, direction.x); - float height = 0.1; - - Vector2 p1 = a + perpendicular * height / 2; - Vector2 p2 = a - perpendicular * height / 2; - Vector2 p3 = b + perpendicular * height / 2; - Vector2 p4 = b - perpendicular * height / 2; - - rapier2d::Vector rapier_points[4]; - rapier_points[0] = rapier2d::Vector{ p1.x, p1.y }; - rapier_points[1] = rapier2d::Vector{ p2.x, p2.y }; - rapier_points[2] = rapier2d::Vector{ p3.x, p3.y }; - rapier_points[3] = rapier2d::Vector{ p4.x, p4.y }; - - return rapier2d::shape_create_convex_polyline(rapier_points, 4); -} - -void RapierSegmentShape2D::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); - - Rect2 r = p_data; - a = r.position; - b = r.size; - n = (b - a).orthogonal(); - - Rect2 aabb; - aabb.position = a; - aabb.expand_to(b); - if (aabb.size.x == 0) { - aabb.size.x = 0.001; - } - if (aabb.size.y == 0) { - aabb.size.y = 0.001; - } - configure(aabb); -} - -Variant RapierSegmentShape2D::get_data() const { - Rect2 r; - r.position = a; - r.size = b; - return r; -} - -real_t RapierSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -rapier2d::Handle RapierCircleShape2D::create_rapier_shape() const { - return rapier2d::shape_create_circle(radius); -} - -void RapierCircleShape2D::set_data(const Variant &p_data) { - radius = p_data; - configure(Rect2(-radius, -radius, radius * 2, radius * 2)); -} - -Variant RapierCircleShape2D::get_data() const { - return radius; -} - -real_t RapierCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - real_t a = radius * p_scale.x; - real_t b = radius * p_scale.y; - return p_mass * (a * a + b * b) / 4.0; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -rapier2d::Handle RapierRectangleShape2D::create_rapier_shape() const { - rapier2d::Vector v = { half_extents.x * 2.0f, half_extents.y * 2.0f }; - return rapier2d::shape_create_box(&v); -} - -void RapierRectangleShape2D::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); - - half_extents = p_data; - configure(Rect2(-half_extents, half_extents * 2.0)); -} - -Variant RapierRectangleShape2D::get_data() const { - return half_extents; -} - -real_t RapierRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = half_extents * 2.0 * p_scale; - return p_mass * he2.dot(he2) / 12.0; -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -rapier2d::Handle RapierConvexPolygonShape2D::create_rapier_shape() const { - ERR_FAIL_COND_V(point_count < 3, rapier2d::invalid_handle()); - rapier2d::Vector *rapier_points = (rapier2d::Vector *)alloca(point_count * sizeof(rapier2d::Vector)); - for (int i = 0; i < point_count; i++) { - rapier_points[i] = rapier2d::Vector{ (points[i].pos.x), (points[i].pos.y) }; - } - return rapier2d::shape_create_convex_polyline(rapier_points, point_count); -} - -void RapierConvexPolygonShape2D::set_data(const Variant &p_data) { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); -#else - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); -#endif - - if (points) { - memdelete_arr(points); - } - points = nullptr; - point_count = 0; - - if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - PackedVector2Array arr = p_data; - ERR_FAIL_COND(arr.size() == 0); - point_count = arr.size(); - points = memnew_arr(Point, point_count); - const Vector2 *r = arr.ptr(); - - for (int i = 0; i < point_count; i++) { - points[i].pos = r[i]; - } - - for (int i = 0; i < point_count; i++) { - Vector2 p = points[i].pos; - Vector2 pn = points[(i + 1) % point_count].pos; - points[i].normal = (pn - p).orthogonal().normalized(); - } - } else { -#ifdef REAL_T_IS_DOUBLE - PackedFloat64Array dvr = p_data; -#else - PackedFloat32Array dvr = p_data; -#endif - point_count = dvr.size() / 4; - ERR_FAIL_COND(point_count == 0); - - points = memnew_arr(Point, point_count); - const real_t *r = dvr.ptr(); - - for (int i = 0; i < point_count; i++) { - int idx = i << 2; - points[i].pos.x = r[idx + 0]; - points[i].pos.y = r[idx + 1]; - points[i].normal.x = r[idx + 2]; - points[i].normal.y = r[idx + 3]; - } - } - - ERR_FAIL_COND(point_count == 0); - Rect2 aabb; - aabb.position = points[0].pos; - for (int i = 1; i < point_count; i++) { - aabb.expand_to(points[i].pos); - } - - configure(aabb); -} - -Variant RapierConvexPolygonShape2D::get_data() const { - PackedVector2Array dvr; - - dvr.resize(point_count); - - for (int i = 0; i < point_count; i++) { - dvr.set(i, points[i].pos); - } - - return dvr; -} - -real_t RapierConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); - Rect2 aabb_new; - aabb_new.position = points[0].pos * p_scale; - for (int i = 0; i < point_count; i++) { - aabb_new.expand_to(points[i].pos * p_scale); - } - - return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0; -} - -RapierConvexPolygonShape2D::~RapierConvexPolygonShape2D() { - if (points) { - memdelete_arr(points); - } -} - -/*********************************************************/ -/*********************************************************/ -/*********************************************************/ - -rapier2d::Handle RapierCapsuleShape2D::create_rapier_shape() const { - return rapier2d::shape_create_capsule((height / 2) - radius, radius); -} - -void RapierCapsuleShape2D::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); - - if (p_data.get_type() == Variant::ARRAY) { - Array arr = p_data; - ERR_FAIL_COND(arr.size() != 2); - height = arr[0]; - radius = arr[1]; - } else { - Point2 p = p_data; - radius = p.x; - height = p.y; - } - - Point2 he(radius, height * 0.5); - configure(Rect2(-he, he * 2)); -} - -Variant RapierCapsuleShape2D::get_data() const { - return Point2(height, radius); -} - -real_t RapierCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = Vector2(radius * 2.0, height) * p_scale; - return p_mass * he2.dot(he2) / 12.0; -} - -// /*********************************************************/ -// /*********************************************************/ -// /*********************************************************/ - -rapier2d::Handle RapierConcavePolygonShape2D::create_rapier_shape() const { - int point_count = points.size(); - ERR_FAIL_COND_V(point_count < 3, rapier2d::invalid_handle()); - rapier2d::Vector *rapier_points = (rapier2d::Vector *)alloca(point_count * sizeof(rapier2d::Vector)); - for (int i = 0; i < point_count; i++) { - rapier_points[i] = rapier2d::Vector{ (points[i].x), (points[i].y) }; - } - return rapier2d::shape_create_convave_polyline(rapier_points, point_count); -} - -void RapierConcavePolygonShape2D::set_data(const Variant &p_data) { -#ifdef REAL_T_IS_DOUBLE - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); -#else - ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); -#endif - - Rect2 aabb; - - if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - PackedVector2Array p2arr = p_data; - int len = p2arr.size(); - ERR_FAIL_COND(len % 2); - - segments.clear(); - points.clear(); - - if (len == 0) { - configure(aabb); - return; - } - - const Vector2 *arr = p2arr.ptr(); - - HashMap pointmap; - for (int i = 0; i < len; i += 2) { - Point2 p1 = arr[i]; - Point2 p2 = arr[i + 1]; - int idx_p1, idx_p2; - - if (pointmap.has(p1)) { - idx_p1 = pointmap[p1]; - } else { - idx_p1 = pointmap.size(); - pointmap[p1] = idx_p1; - } - - if (pointmap.has(p2)) { - idx_p2 = pointmap[p2]; - } else { - idx_p2 = pointmap.size(); - pointmap[p2] = idx_p2; - } - - Segment s; - s.points[0] = idx_p1; - s.points[1] = idx_p2; - segments.push_back(s); - } - - points.resize(pointmap.size()); - aabb.position = pointmap.begin()->key; - for (const KeyValue &E : pointmap) { - aabb.expand_to(E.key); - points[E.value] = E.key; - } - } else { - //dictionary with arrays - } - - configure(aabb); -} - -Variant RapierConcavePolygonShape2D::get_data() const { - PackedVector2Array rsegments; - int len = segments.size(); - if (len == 0) { - return rsegments; - } - - rsegments.resize(len * 2); - Vector2 *w = rsegments.ptrw(); - for (int i = 0; i < len; i++) { - w[(i << 1) + 0] = points[segments[i].points[0]]; - w[(i << 1) + 1] = points[segments[i].points[1]]; - } - - return rsegments; -} diff --git a/src/rapier_shape_2d.h b/src/rapier_shape_2d.h deleted file mode 100644 index dfadfeb..0000000 --- a/src/rapier_shape_2d.h +++ /dev/null @@ -1,232 +0,0 @@ -#ifndef RAPIER_SHAPE_2D_H -#define RAPIER_SHAPE_2D_H - -#include -#include -#include - -#include "rapier_include.h" - -using namespace godot; - -class RapierShape2D; - -class RapierShapeOwner2D { -public: - virtual void _shape_changed(RapierShape2D *p_shape) = 0; - virtual void remove_shape(RapierShape2D *p_shape) = 0; - - virtual ~RapierShapeOwner2D() {} -}; - -class RapierShape2D { - RID rid; - Rect2 aabb; - bool configured = false; - real_t custom_bias = 0.0; - - HashMap owners; - - rapier2d::Handle shape_handle = rapier2d::invalid_handle(); - -protected: - void configure(const Rect2 &p_aabb); - - virtual rapier2d::Handle create_rapier_shape() const = 0; - void destroy_rapier_shape(); - -public: - _FORCE_INLINE_ void set_rid(const RID &p_rid) { rid = p_rid; } - _FORCE_INLINE_ RID get_rid() const { return rid; } - - virtual PhysicsServer2D::ShapeType get_type() const = 0; - - virtual void apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const {} - - rapier2d::Handle get_rapier_shape(); - - _FORCE_INLINE_ Rect2 get_aabb(Vector2 origin = Vector2()) const { - Rect2 aabb_clone = aabb; - aabb_clone.position += origin; - return aabb_clone; - } - _FORCE_INLINE_ bool is_configured() const { return configured; } - - void add_owner(RapierShapeOwner2D *p_owner); - void remove_owner(RapierShapeOwner2D *p_owner); - bool is_owner(RapierShapeOwner2D *p_owner) const; - const HashMap &get_owners() const; - - virtual void set_data(const Variant &p_data) = 0; - virtual Variant get_data() const = 0; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; - - RapierShape2D() {} - virtual ~RapierShape2D(); -}; - -class RapierWorldBoundaryShape2D : public RapierShape2D { - Vector2 normal; - real_t d = 0.0; - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; - -public: - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } - - virtual void apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const override; - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0.0; } -}; - -// class RapierSeparationRayShape2D : public RapierShape2D { -// real_t length = 0.0; -// bool slide_on_slope = false; - -// public: - -// virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } - -// virtual bool allows_one_way_collision() const override { return false; } - -// virtual void set_data(const Variant &p_data) override; -// virtual Variant get_data() const override; - -// virtual real_t get_moment_of_inertia(real_t p_mass, const Size2& p_scale) const override; - -// DEFAULT_PROJECT_RANGE_CAST - -// _FORCE_INLINE_ RapierSeparationRayShape2D() {} -// _FORCE_INLINE_ RapierSeparationRayShape2D(real_t p_length) { length = p_length; } -// }; - -class RapierSegmentShape2D : public RapierShape2D { - Vector2 a; - Vector2 b; - Vector2 n; - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; - -public: - _FORCE_INLINE_ const Vector2 &get_a() const { return a; } - _FORCE_INLINE_ const Vector2 &get_b() const { return b; } - _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - _FORCE_INLINE_ RapierSegmentShape2D() {} - _FORCE_INLINE_ RapierSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { - a = p_a; - b = p_b; - n = p_n; - } -}; - -class RapierCircleShape2D : public RapierShape2D { - real_t radius; - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; - -public: - _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; -}; - -class RapierRectangleShape2D : public RapierShape2D { - Vector2 half_extents; - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; - -public: - _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; -}; - -class RapierConvexPolygonShape2D : public RapierShape2D { - struct Point { - Vector2 pos; - Vector2 normal; //normal to next segment - }; - - Point *points = nullptr; - int point_count = 0; - -public: - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - - ~RapierConvexPolygonShape2D(); - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; -}; - -class RapierCapsuleShape2D : public RapierShape2D { - real_t radius = 0.0; - real_t height = 0.0; - -public: - _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - _FORCE_INLINE_ const real_t &get_height() const { return height; } - - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; -}; - -class RapierConcavePolygonShape2D : public RapierShape2D { - struct Segment { - int points[2] = {}; - }; - - LocalVector segments; - LocalVector points; - -protected: - virtual rapier2d::Handle create_rapier_shape() const override; - -public: - virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } - - virtual void set_data(const Variant &p_data) override; - virtual Variant get_data() const override; - - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0.0; } -}; - -#endif // RAPIER_SHAPE_2D_H diff --git a/src/register_types.cpp b/src/register_types.cpp index 7f7d5b1..1b09321 100644 --- a/src/register_types.cpp +++ b/src/register_types.cpp @@ -8,10 +8,11 @@ #include #include -#include "rapier_body_direct_state_2d.h" -#include "rapier_physics_server_2d.h" -#include "rapier_project_settings.h" -#include "rapier_space_2d.h" +#include "bodies/rapier_direct_body_state_2d.h" +#include "servers/rapier_physics_server_2d.h" +#include "servers/rapier_project_settings.h" +#include "spaces/rapier_direct_space_state_2d.h" +#include "spaces/rapier_space_2d.h" #if defined(WINDOWS_ENABLED) // Libs needed to link Rapier @@ -29,8 +30,8 @@ static RapierPhysicsServer2DFactory *rapier_2d_factory = nullptr; void initialize_rapier_2d_module(ModuleInitializationLevel p_level) { switch (p_level) { case MODULE_INITIALIZATION_LEVEL_SERVERS: { - ClassDB::register_class(true); - ClassDB::register_class(true); + ClassDB::register_class(); + ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); diff --git a/src/rapier_body_utils_2d.cpp b/src/servers/rapier_body_utils_2d.cpp similarity index 99% rename from src/rapier_body_utils_2d.cpp rename to src/servers/rapier_body_utils_2d.cpp index 8d421e7..020adcb 100644 --- a/src/rapier_body_utils_2d.cpp +++ b/src/servers/rapier_body_utils_2d.cpp @@ -1,7 +1,7 @@ #include "rapier_body_utils_2d.h" -#include "rapier_body_2d.h" -#include "rapier_shape_2d.h" -#include "rapier_space_2d.h" +#include "../bodies/rapier_body_2d.h" +#include "../shapes/rapier_shape_2d.h" +#include "../spaces/rapier_space_2d.h" #define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 #define BODY_MOTION_RECOVER_ATTEMPTS 4 diff --git a/src/rapier_body_utils_2d.h b/src/servers/rapier_body_utils_2d.h similarity index 100% rename from src/rapier_body_utils_2d.h rename to src/servers/rapier_body_utils_2d.h diff --git a/src/rapier_physics_server_2d.cpp b/src/servers/rapier_physics_server_2d.cpp similarity index 95% rename from src/rapier_physics_server_2d.cpp rename to src/servers/rapier_physics_server_2d.cpp index 91dd3fa..5315b8a 100644 --- a/src/rapier_physics_server_2d.cpp +++ b/src/servers/rapier_physics_server_2d.cpp @@ -1,6 +1,21 @@ #include "rapier_physics_server_2d.h" -#include "rapier_body_direct_state_2d.h" +#include "../bodies/rapier_direct_body_state_2d.h" + +#include "../spaces/rapier_direct_space_state_2d.h" + +#include "../shapes/rapier_capsule_shape_2d.h" +#include "../shapes/rapier_circle_shape_2d.h" +#include "../shapes/rapier_concave_polygon_shape_2d.h" +#include "../shapes/rapier_convex_polygon_shape_2d.h" +#include "../shapes/rapier_rectangle_shape_2d.h" +#include "../shapes/rapier_segment_shape_2d.h" +#include "../shapes/rapier_separation_ray_shape_2d.h" +#include "../shapes/rapier_world_boundary_shape_2d.h" + +#include "../joints/rapier_damped_spring_joint_2d.h" +#include "../joints/rapier_groove_joint_2d.h" +#include "../joints/rapier_pin_joint_2d.h" #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); @@ -11,9 +26,9 @@ RID RapierPhysicsServer2D::_shape_create(ShapeType p_shape) { case SHAPE_WORLD_BOUNDARY: { shape = memnew(RapierWorldBoundaryShape2D); } break; - // case SHAPE_SEPARATION_RAY: { - // shape = memnew(RapierSeparationRayShape2D); - // } break; + case SHAPE_SEPARATION_RAY: { + shape = memnew(RapierSeparationRayShape2D); + } break; case SHAPE_SEGMENT: { shape = memnew(RapierSegmentShape2D); } break; @@ -968,39 +983,37 @@ void RapierPhysicsServer2D::_joint_clear(const RID &p_joint) { } void RapierPhysicsServer2D::_joint_set_param(const RID &p_joint, JointParam p_param, double p_value) { - // RapierJoint2D *joint = joint_owner.get_or_null(p_joint); - // ERR_FAIL_COND(!joint); - - // switch (p_param) { - // case JOINT_PARAM_BIAS: - // joint->set_bias(p_value); - // break; - // case JOINT_PARAM_MAX_BIAS: - // joint->set_max_bias(p_value); - // break; - // case JOINT_PARAM_MAX_FORCE: - // joint->set_max_force(p_value); - // break; - // } + RapierJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!joint); + + switch (p_param) { + case JOINT_PARAM_BIAS: + joint->set_bias(p_value); + break; + case JOINT_PARAM_MAX_BIAS: + joint->set_max_bias(p_value); + break; + case JOINT_PARAM_MAX_FORCE: + joint->set_max_force(p_value); + break; + } } double RapierPhysicsServer2D::_joint_get_param(const RID &p_joint, JointParam p_param) const { - // const RapierJoint2D *joint = joint_owner.get_or_null(p_joint); - // ERR_FAIL_COND_V(!joint, -1); - - // switch (p_param) { - // case JOINT_PARAM_BIAS: - // return joint->get_bias(); - // break; - // case JOINT_PARAM_MAX_BIAS: - // return joint->get_max_bias(); - // break; - // case JOINT_PARAM_MAX_FORCE: - // return joint->get_max_force(); - // break; - // } - - return 0; + const RapierJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!joint, -1); + + switch (p_param) { + case JOINT_PARAM_BIAS: + return joint->get_bias(); + break; + case JOINT_PARAM_MAX_BIAS: + return joint->get_max_bias(); + break; + case JOINT_PARAM_MAX_FORCE: + return joint->get_max_force(); + break; + } } void RapierPhysicsServer2D::_joint_disable_collisions_between_bodies(const RID &p_joint, const bool p_disable) { @@ -1008,19 +1021,6 @@ void RapierPhysicsServer2D::_joint_disable_collisions_between_bodies(const RID & ERR_FAIL_COND(!joint); joint->disable_collisions_between_bodies(p_disable); - - if (2 == joint->get_body_count()) { - RapierBody2D *body_a = *joint->get_body_ptr(); - RapierBody2D *body_b = *(joint->get_body_ptr() + 1); - - if (p_disable) { - body_add_collision_exception(body_a->get_rid(), body_b->get_rid()); - body_add_collision_exception(body_b->get_rid(), body_a->get_rid()); - } else { - body_remove_collision_exception(body_a->get_rid(), body_b->get_rid()); - body_remove_collision_exception(body_b->get_rid(), body_a->get_rid()); - } - } } bool RapierPhysicsServer2D::_joint_is_disabled_collisions_between_bodies(const RID &p_joint) const { @@ -1120,22 +1120,21 @@ double RapierPhysicsServer2D::_pin_joint_get_param(const RID &p_joint, PinJointP } void RapierPhysicsServer2D::_damped_spring_joint_set_param(const RID &p_joint, DampedSpringParam p_param, double p_value) { - // RapierJoint2D *j = joint_owner.get_or_null(p_joint); - // ERR_FAIL_COND(!j); - // ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); + RapierJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); - // RapierDampedSpringJoint2D *dsj = static_cast(j); - // dsj->set_param(p_param, p_value); + RapierDampedSpringJoint2D *dsj = static_cast(j); + dsj->set_param(p_param, p_value); } double RapierPhysicsServer2D::_damped_spring_joint_get_param(const RID &p_joint, DampedSpringParam p_param) const { - // RapierJoint2D *j = joint_owner.get_or_null(p_joint); - // ERR_FAIL_COND_V(!j, 0); - // ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); + RapierJoint2D *j = joint_owner.get_or_null(p_joint); + ERR_FAIL_COND_V(!j, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); - // RapierDampedSpringJoint2D *dsj = static_cast(j); - // return dsj->get_param(p_param); - return 0; + RapierDampedSpringJoint2D *dsj = static_cast(j); + return dsj->get_param(p_param); } PhysicsServer2D::JointType RapierPhysicsServer2D::_joint_get_type(const RID &p_joint) const { diff --git a/src/rapier_physics_server_2d.h b/src/servers/rapier_physics_server_2d.h similarity index 98% rename from src/rapier_physics_server_2d.h rename to src/servers/rapier_physics_server_2d.h index 1943829..8ab09a9 100644 --- a/src/rapier_physics_server_2d.h +++ b/src/servers/rapier_physics_server_2d.h @@ -12,13 +12,13 @@ #include -#include "rapier_area_2d.h" -#include "rapier_body_2d.h" -#include "rapier_joints_2d.h" -#include "rapier_shape_2d.h" -#include "rapier_space_2d.h" +#include "../bodies/rapier_area_2d.h" +#include "../bodies/rapier_body_2d.h" +#include "../joints/rapier_joint_2d.h" +#include "../shapes/rapier_shape_2d.h" +#include "../spaces/rapier_space_2d.h" -#include "rapier_include.h" +#include "../rapier_include.h" using namespace godot; diff --git a/src/rapier_project_settings.cpp b/src/servers/rapier_project_settings.cpp similarity index 75% rename from src/rapier_project_settings.cpp rename to src/servers/rapier_project_settings.cpp index ac93545..d78f349 100644 --- a/src/rapier_project_settings.cpp +++ b/src/servers/rapier_project_settings.cpp @@ -76,17 +76,17 @@ void register_setting_ranged( } void RapierProjectSettings::register_settings() { - register_setting_ranged(SOLVER_MIN_CCD_DT, 1.0f / 60.0f / 100.0f, U"0.0000001,1,0.0000001,suffix:1/s"); - register_setting_ranged(SOLVER_ERP, 0.8f, U"0.00001,1,0.00001,suffix:%"); - register_setting_ranged(SOLVER_DAMPING_RATIO, 0.25f, U"0.00001,1,0.00001,suffix:%"); - register_setting_ranged(SOLVER_JOINT_ERP, 1.0f, U"0.00001,1,0.00001,suffix:%"); - register_setting_ranged(SOLVER_JOINT_DAMPING_RATIO, 0.25f, U"0.0001,1,0.00001,suffix:%"); - register_setting_ranged(SOLVER_ALLOWED_LINEAR_ERROR, 0.001f, U"0,1,0.00001,suffix:m"); - register_setting_ranged(SOLVER_MAX_PENETRATION_CORRECTION, 3.40282e+38f, U"0, 3.40282e+38f,1,suffix:m"); - register_setting_ranged(SOLVER_PREDICTION_DISTANCE, 0.002f, U"0,1,0.00001,suffix:m"); - register_setting_ranged(SOLVER_MAX_VELOCITY_ITERATIONS, 4, U"1,16,or_greater"); - register_setting_ranged(SOLVER_MAX_VELOCITY_FRICTION_ITERATIONS, 8, U"1,16,or_greater"); - register_setting_ranged(SOLVER_MAX_STABILIZATION_ITERATIONS, 1, U"1,16,or_greater"); + register_setting_ranged(SOLVER_MIN_CCD_DT, 1.0 / 60.0 / 100.0, U"0.0000001,1,0.0000001,suffix:1/s"); + register_setting_ranged(SOLVER_ERP, 0.85, U"0.00001,1,0.00001,suffix:%"); + register_setting_ranged(SOLVER_DAMPING_RATIO, 0.3, U"0.00001,1,0.00001,suffix:%"); + register_setting_ranged(SOLVER_JOINT_ERP, 1.0, U"0.00001,1,0.00001,suffix:%"); + register_setting_ranged(SOLVER_JOINT_DAMPING_RATIO, 0.25, U"0.0001,1,0.00001,suffix:%"); + register_setting_ranged(SOLVER_ALLOWED_LINEAR_ERROR, 0.00001, U"0,1,0.00001,suffix:m"); + register_setting_ranged(SOLVER_MAX_PENETRATION_CORRECTION, 3.40282e+38, U"0, 3.40282e+38f,1,suffix:m"); + register_setting_ranged(SOLVER_PREDICTION_DISTANCE, 0.00002, U"0,1,0.00001,suffix:m"); + register_setting_ranged(SOLVER_MAX_VELOCITY_ITERATIONS, 19, U"1,16,or_greater"); + register_setting_ranged(SOLVER_MAX_VELOCITY_FRICTION_ITERATIONS, 29, U"1,16,or_greater"); + register_setting_ranged(SOLVER_MAX_STABILIZATION_ITERATIONS, 9, U"1,16,or_greater"); register_setting_plain(SOLVER_INTERLEAVE_RESTITUTION_AND_FRICTION_RESOLUTION, true); register_setting_plain(SOLVER_MIN_ISLAND_SIZE, 128); register_setting_plain(SOLVER_MAX_CCD_SUBSTEPS, 1); @@ -112,29 +112,29 @@ int RapierProjectSettings::get_max_threads() { return get_setting(MAX_THREADS); } -float RapierProjectSettings::get_solver_min_ccd_dt() { - return get_setting(SOLVER_MIN_CCD_DT); +double RapierProjectSettings::get_solver_min_ccd_dt() { + return get_setting(SOLVER_MIN_CCD_DT); } -float RapierProjectSettings::get_solver_erp() { - return get_setting(SOLVER_ERP); +double RapierProjectSettings::get_solver_erp() { + return get_setting(SOLVER_ERP); } -float RapierProjectSettings::get_solver_damping_ratio() { - return get_setting(SOLVER_DAMPING_RATIO); +double RapierProjectSettings::get_solver_damping_ratio() { + return get_setting(SOLVER_DAMPING_RATIO); } -float RapierProjectSettings::get_solver_joint_erp() { - return get_setting(SOLVER_JOINT_ERP); +double RapierProjectSettings::get_solver_joint_erp() { + return get_setting(SOLVER_JOINT_ERP); } -float RapierProjectSettings::get_solver_joint_damping_ratio() { - return get_setting(SOLVER_JOINT_DAMPING_RATIO); +double RapierProjectSettings::get_solver_joint_damping_ratio() { + return get_setting(SOLVER_JOINT_DAMPING_RATIO); } -float RapierProjectSettings::get_solver_allowed_linear_error() { - return get_setting(SOLVER_ALLOWED_LINEAR_ERROR); +double RapierProjectSettings::get_solver_allowed_linear_error() { + return get_setting(SOLVER_ALLOWED_LINEAR_ERROR); } -float RapierProjectSettings::get_solver_max_penetration_correction() { - return get_setting(SOLVER_MAX_PENETRATION_CORRECTION); +double RapierProjectSettings::get_solver_max_penetration_correction() { + return get_setting(SOLVER_MAX_PENETRATION_CORRECTION); } -float RapierProjectSettings::get_solver_prediction_distance() { - return get_setting(SOLVER_PREDICTION_DISTANCE); +double RapierProjectSettings::get_solver_prediction_distance() { + return get_setting(SOLVER_PREDICTION_DISTANCE); } int RapierProjectSettings::get_solver_max_velocity_iterations() { return get_setting(SOLVER_MAX_VELOCITY_ITERATIONS); diff --git a/src/rapier_project_settings.h b/src/servers/rapier_project_settings.h similarity index 64% rename from src/rapier_project_settings.h rename to src/servers/rapier_project_settings.h index 5e0d05d..28dc5ab 100644 --- a/src/rapier_project_settings.h +++ b/src/servers/rapier_project_settings.h @@ -8,14 +8,14 @@ class RapierProjectSettings { static bool should_run_on_separate_thread(); static int get_max_threads(); - static float get_solver_min_ccd_dt(); - static float get_solver_erp(); - static float get_solver_damping_ratio(); - static float get_solver_joint_erp(); - static float get_solver_joint_damping_ratio(); - static float get_solver_allowed_linear_error(); - static float get_solver_max_penetration_correction(); - static float get_solver_prediction_distance(); + static double get_solver_min_ccd_dt(); + static double get_solver_erp(); + static double get_solver_damping_ratio(); + static double get_solver_joint_erp(); + static double get_solver_joint_damping_ratio(); + static double get_solver_allowed_linear_error(); + static double get_solver_max_penetration_correction(); + static double get_solver_prediction_distance(); static int get_solver_max_velocity_iterations(); static int get_solver_max_velocity_friction_iterations(); static int get_solver_max_stabilization_iterations(); diff --git a/src/shapes/rapier_capsule_shape_2d.cpp b/src/shapes/rapier_capsule_shape_2d.cpp new file mode 100644 index 0000000..38c8a3b --- /dev/null +++ b/src/shapes/rapier_capsule_shape_2d.cpp @@ -0,0 +1,32 @@ +#include "rapier_capsule_shape_2d.h" + +rapier2d::Handle RapierCapsuleShape2D::create_rapier_shape() const { + return rapier2d::shape_create_capsule((height / 2.0) - radius, radius); +} + +void RapierCapsuleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); + + if (p_data.get_type() == Variant::ARRAY) { + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + height = arr[0]; + radius = arr[1]; + } else { + Point2 p = p_data; + radius = p.x; + height = p.y; + } + + Point2 he(radius, height * 0.5); + configure(Rect2(-he, he * 2.0)); +} + +Variant RapierCapsuleShape2D::get_data() const { + return Point2(height, radius); +} + +real_t RapierCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = Vector2(radius * 2.0, height) * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} diff --git a/src/shapes/rapier_capsule_shape_2d.h b/src/shapes/rapier_capsule_shape_2d.h new file mode 100644 index 0000000..157634c --- /dev/null +++ b/src/shapes/rapier_capsule_shape_2d.h @@ -0,0 +1,25 @@ +#ifndef RAPIER_CAPSULE_SHAPE_2D_H +#define RAPIER_CAPSULE_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierCapsuleShape2D : public RapierShape2D { + real_t radius = 0.0; + real_t height = 0.0; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + _FORCE_INLINE_ const real_t &get_height() const { return height; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; +}; + +#endif // RAPIER_CAPSULE_SHAPE_2D_H diff --git a/src/shapes/rapier_circle_shape_2d.cpp b/src/shapes/rapier_circle_shape_2d.cpp new file mode 100644 index 0000000..5cef19e --- /dev/null +++ b/src/shapes/rapier_circle_shape_2d.cpp @@ -0,0 +1,20 @@ +#include "rapier_circle_shape_2d.h" + +rapier2d::Handle RapierCircleShape2D::create_rapier_shape() const { + return rapier2d::shape_create_circle(radius); +} + +void RapierCircleShape2D::set_data(const Variant &p_data) { + radius = p_data; + configure(Rect2(-radius, -radius, radius * 2, radius * 2)); +} + +Variant RapierCircleShape2D::get_data() const { + return radius; +} + +real_t RapierCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + real_t a = radius * p_scale.x; + real_t b = radius * p_scale.y; + return p_mass * (a * a + b * b) / 4.0; +} diff --git a/src/shapes/rapier_circle_shape_2d.h b/src/shapes/rapier_circle_shape_2d.h new file mode 100644 index 0000000..0cf7763 --- /dev/null +++ b/src/shapes/rapier_circle_shape_2d.h @@ -0,0 +1,23 @@ +#ifndef RAPIER_CIRCLE_SHAPE_2D_H +#define RAPIER_CIRCLE_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierCircleShape2D : public RapierShape2D { + real_t radius; + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; +}; + +#endif // RAPIER_CIRCLE_SHAPE_2D_H diff --git a/src/shapes/rapier_concave_polygon_shape_2d.cpp b/src/shapes/rapier_concave_polygon_shape_2d.cpp new file mode 100644 index 0000000..50edd7b --- /dev/null +++ b/src/shapes/rapier_concave_polygon_shape_2d.cpp @@ -0,0 +1,91 @@ +#include "rapier_concave_polygon_shape_2d.h" + +rapier2d::Handle RapierConcavePolygonShape2D::create_rapier_shape() const { + int point_count = points.size(); + ERR_FAIL_COND_V(point_count < 3, rapier2d::invalid_handle()); + rapier2d::Vector *rapier_points = (rapier2d::Vector *)alloca(point_count * sizeof(rapier2d::Vector)); + for (int i = 0; i < point_count; i++) { + rapier_points[i] = rapier2d::Vector{ (points[i].x), (points[i].y) }; + } + return rapier2d::shape_create_convave_polyline(rapier_points, point_count); +} + +void RapierConcavePolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + Rect2 aabb; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + PackedVector2Array p2arr = p_data; + int len = p2arr.size(); + ERR_FAIL_COND(len % 2); + + segments.clear(); + points.clear(); + + if (len == 0) { + configure(aabb); + return; + } + + const Vector2 *arr = p2arr.ptr(); + + HashMap pointmap; + for (int i = 0; i < len; i += 2) { + Point2 p1 = arr[i]; + Point2 p2 = arr[i + 1]; + int idx_p1, idx_p2; + + if (pointmap.has(p1)) { + idx_p1 = pointmap[p1]; + } else { + idx_p1 = pointmap.size(); + pointmap[p1] = idx_p1; + } + + if (pointmap.has(p2)) { + idx_p2 = pointmap[p2]; + } else { + idx_p2 = pointmap.size(); + pointmap[p2] = idx_p2; + } + + Segment s; + s.points[0] = idx_p1; + s.points[1] = idx_p2; + segments.push_back(s); + } + + points.resize(pointmap.size()); + aabb.position = pointmap.begin()->key; + for (const KeyValue &E : pointmap) { + aabb.expand_to(E.key); + points[E.value] = E.key; + } + } else { + //dictionary with arrays + } + + configure(aabb); +} + +Variant RapierConcavePolygonShape2D::get_data() const { + PackedVector2Array rsegments; + int len = segments.size(); + if (len == 0) { + return rsegments; + } + + rsegments.resize(len * 2); + Vector2 *w = rsegments.ptrw(); + for (int i = 0; i < len; i++) { + w[(i << 1) + 0] = points[segments[i].points[0]]; + w[(i << 1) + 1] = points[segments[i].points[1]]; + } + + return rsegments; +} diff --git a/src/shapes/rapier_concave_polygon_shape_2d.h b/src/shapes/rapier_concave_polygon_shape_2d.h new file mode 100644 index 0000000..7fd9954 --- /dev/null +++ b/src/shapes/rapier_concave_polygon_shape_2d.h @@ -0,0 +1,26 @@ +#ifndef RAPIER_CONCAVE_POLYGON_SHAPE_2D_H +#define RAPIER_CONCAVE_POLYGON_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierConcavePolygonShape2D : public RapierShape2D { + struct Segment { + int points[2] = {}; + }; + + LocalVector segments; + LocalVector points; + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0.0; } +}; + +#endif // RAPIER_CONCAVE_POLYGON_SHAPE_2D_H diff --git a/src/shapes/rapier_convex_polygon_shape_2d.cpp b/src/shapes/rapier_convex_polygon_shape_2d.cpp new file mode 100644 index 0000000..baf97fe --- /dev/null +++ b/src/shapes/rapier_convex_polygon_shape_2d.cpp @@ -0,0 +1,99 @@ +#include "rapier_convex_polygon_shape_2d.h" + +rapier2d::Handle RapierConvexPolygonShape2D::create_rapier_shape() const { + ERR_FAIL_COND_V(point_count < 3, rapier2d::invalid_handle()); + rapier2d::Vector *rapier_points = (rapier2d::Vector *)alloca(point_count * sizeof(rapier2d::Vector)); + for (int i = 0; i < point_count; i++) { + rapier_points[i] = rapier2d::Vector{ (points[i].pos.x), (points[i].pos.y) }; + } + return rapier2d::shape_create_convex_polyline(rapier_points, point_count); +} + +void RapierConvexPolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + if (points) { + memdelete_arr(points); + } + points = nullptr; + point_count = 0; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + PackedVector2Array arr = p_data; + ERR_FAIL_COND(arr.size() == 0); + point_count = arr.size(); + points = memnew_arr(Point, point_count); + const Vector2 *r = arr.ptr(); + + for (int i = 0; i < point_count; i++) { + points[i].pos = r[i]; + } + + for (int i = 0; i < point_count; i++) { + Vector2 p = points[i].pos; + Vector2 pn = points[(i + 1) % point_count].pos; + points[i].normal = (pn - p).orthogonal().normalized(); + } + } else { +#ifdef REAL_T_IS_DOUBLE + PackedFloat64Array dvr = p_data; +#else + PackedFloat32Array dvr = p_data; +#endif + point_count = dvr.size() / 4; + ERR_FAIL_COND(point_count == 0); + + points = memnew_arr(Point, point_count); + const real_t *r = dvr.ptr(); + + for (int i = 0; i < point_count; i++) { + int idx = i << 2; + points[i].pos.x = r[idx + 0]; + points[i].pos.y = r[idx + 1]; + points[i].normal.x = r[idx + 2]; + points[i].normal.y = r[idx + 3]; + } + } + + ERR_FAIL_COND(point_count == 0); + Rect2 aabb; + aabb.position = points[0].pos; + for (int i = 1; i < point_count; i++) { + aabb.expand_to(points[i].pos); + } + + configure(aabb); +} + +Variant RapierConvexPolygonShape2D::get_data() const { + PackedVector2Array dvr; + + dvr.resize(point_count); + + for (int i = 0; i < point_count; i++) { + dvr.set(i, points[i].pos); + } + + return dvr; +} + +real_t RapierConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); + Rect2 aabb_new; + aabb_new.position = points[0].pos * p_scale; + for (int i = 0; i < point_count; i++) { + aabb_new.expand_to(points[i].pos * p_scale); + } + + return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0; +} + +RapierConvexPolygonShape2D::~RapierConvexPolygonShape2D() { + if (points) { + memdelete_arr(points); + } +} diff --git a/src/shapes/rapier_convex_polygon_shape_2d.h b/src/shapes/rapier_convex_polygon_shape_2d.h new file mode 100644 index 0000000..489fcf9 --- /dev/null +++ b/src/shapes/rapier_convex_polygon_shape_2d.h @@ -0,0 +1,29 @@ +#ifndef RAPIER_CONVEX_POLYGON_SHAPE_2D_H +#define RAPIER_CONVEX_POLYGON_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierConvexPolygonShape2D : public RapierShape2D { + struct Point { + Vector2 pos; + Vector2 normal; //normal to next segment + }; + + Point *points = nullptr; + int point_count = 0; + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + ~RapierConvexPolygonShape2D(); + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; +}; + +#endif // RAPIER_CONVEX_POLYGON_SHAPE_2D_H diff --git a/src/shapes/rapier_rectangle_shape_2d.cpp b/src/shapes/rapier_rectangle_shape_2d.cpp new file mode 100644 index 0000000..ac499d7 --- /dev/null +++ b/src/shapes/rapier_rectangle_shape_2d.cpp @@ -0,0 +1,22 @@ +#include "rapier_rectangle_shape_2d.h" + +rapier2d::Handle RapierRectangleShape2D::create_rapier_shape() const { + rapier2d::Vector v = { half_extents.x * 2.0f, half_extents.y * 2.0f }; + return rapier2d::shape_create_box(&v); +} + +void RapierRectangleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); + + half_extents = p_data; + configure(Rect2(-half_extents, half_extents * 2.0)); +} + +Variant RapierRectangleShape2D::get_data() const { + return half_extents; +} + +real_t RapierRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = half_extents * 2.0 * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} diff --git a/src/shapes/rapier_rectangle_shape_2d.h b/src/shapes/rapier_rectangle_shape_2d.h new file mode 100644 index 0000000..956464f --- /dev/null +++ b/src/shapes/rapier_rectangle_shape_2d.h @@ -0,0 +1,23 @@ +#ifndef RAPIER_RECTANGLE_SHAPE_2D_H +#define RAPIER_RECTANGLE_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierRectangleShape2D : public RapierShape2D { + Vector2 half_extents; + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; + +public: + _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; +}; + +#endif // RAPIER_RECTANGLE_SHAPE_2D_H diff --git a/src/shapes/rapier_segment_shape_2d.cpp b/src/shapes/rapier_segment_shape_2d.cpp new file mode 100644 index 0000000..d3e3407 --- /dev/null +++ b/src/shapes/rapier_segment_shape_2d.cpp @@ -0,0 +1,53 @@ +#include "rapier_segment_shape_2d.h" + +rapier2d::Handle RapierSegmentShape2D::create_rapier_shape() const { + Vector2 direction = b - a; + direction.normalize(); + + Vector2 perpendicular = Vector2(-direction.y, direction.x); + double height = 0.1; + + Vector2 p1 = a + perpendicular * height / 2.0; + Vector2 p2 = a - perpendicular * height / 2.0; + Vector2 p3 = b + perpendicular * height / 2.0; + Vector2 p4 = b - perpendicular * height / 2.0; + + rapier2d::Vector rapier_points[4]; + rapier_points[0] = rapier2d::Vector{ p1.x, p1.y }; + rapier_points[1] = rapier2d::Vector{ p2.x, p2.y }; + rapier_points[2] = rapier2d::Vector{ p3.x, p3.y }; + rapier_points[3] = rapier2d::Vector{ p4.x, p4.y }; + + return rapier2d::shape_create_convex_polyline(rapier_points, 4); +} + +void RapierSegmentShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); + + Rect2 r = p_data; + a = r.position; + b = r.size; + n = (b - a).orthogonal(); + + Rect2 aabb; + aabb.position = a; + aabb.expand_to(b); + if (aabb.size.x == 0) { + aabb.size.x = 0.001; + } + if (aabb.size.y == 0) { + aabb.size.y = 0.001; + } + configure(aabb); +} + +Variant RapierSegmentShape2D::get_data() const { + Rect2 r; + r.position = a; + r.size = b; + return r; +} + +real_t RapierSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12.0; +} diff --git a/src/shapes/rapier_segment_shape_2d.h b/src/shapes/rapier_segment_shape_2d.h new file mode 100644 index 0000000..a7c8bd5 --- /dev/null +++ b/src/shapes/rapier_segment_shape_2d.h @@ -0,0 +1,27 @@ +#ifndef RAPIER_SEGMENT_SHAPE_2D_H +#define RAPIER_SEGMENT_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierSegmentShape2D : public RapierShape2D { + Vector2 a; + Vector2 b; + Vector2 n; + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; + +public: + _FORCE_INLINE_ const Vector2 &get_a() const { return a; } + _FORCE_INLINE_ const Vector2 &get_b() const { return b; } + _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; +}; + +#endif // RAPIER_SEGMENT_SHAPE_2D_H diff --git a/src/shapes/rapier_separation_ray_shape_2d.cpp b/src/shapes/rapier_separation_ray_shape_2d.cpp new file mode 100644 index 0000000..6f4610b --- /dev/null +++ b/src/shapes/rapier_separation_ray_shape_2d.cpp @@ -0,0 +1,15 @@ +#include "rapier_separation_ray_shape_2d.h" + +void RapierSeparationRayShape2D::set_data(const Variant &p_data) { + Dictionary d = p_data; + length = d["length"]; + slide_on_slope = d["slide_on_slope"]; + configure(Rect2(0, 0, 0.001, length)); +} + +Variant RapierSeparationRayShape2D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} diff --git a/src/shapes/rapier_separation_ray_shape_2d.h b/src/shapes/rapier_separation_ray_shape_2d.h new file mode 100644 index 0000000..d32dbb1 --- /dev/null +++ b/src/shapes/rapier_separation_ray_shape_2d.h @@ -0,0 +1,24 @@ +#ifndef RAPIER_SEPARATION_RAY_SHAPE_2D_H +#define RAPIER_SEPARATION_RAY_SHAPE_2D_H + +#include "rapier_segment_shape_2d.h" + +class RapierSeparationRayShape2D : public RapierSegmentShape2D { + real_t length = 0.0; + bool slide_on_slope = false; + +protected: + //virtual rapier2d::Handle create_rapier_shape() const override; + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } + + //virtual void apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + //virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; +}; + +#endif // RAPIER_SEPARATION_RAY_SHAPE_2D_H diff --git a/src/shapes/rapier_shape_2d.cpp b/src/shapes/rapier_shape_2d.cpp new file mode 100644 index 0000000..1bed777 --- /dev/null +++ b/src/shapes/rapier_shape_2d.cpp @@ -0,0 +1,56 @@ +#include "rapier_shape_2d.h" + +void RapierShape2D::configure(const Rect2 &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue &E : owners) { + RapierShapeOwner2D *co = const_cast(E.key); + co->_shape_changed(this); + } +} + +void RapierShape2D::destroy_rapier_shape() { + if (rapier2d::is_handle_valid(shape_handle)) { + rapier2d::shape_destroy(shape_handle); + shape_handle = rapier2d::invalid_handle(); + } +} + +rapier2d::Handle RapierShape2D::get_rapier_shape() { + if (!rapier2d::is_handle_valid(shape_handle)) { + shape_handle = create_rapier_shape(); + } + + return shape_handle; +} + +void RapierShape2D::add_owner(RapierShapeOwner2D *p_owner) { + HashMap::Iterator E = owners.find(p_owner); + if (E) { + E->value++; + } else { + owners[p_owner] = 1; + } +} + +void RapierShape2D::remove_owner(RapierShapeOwner2D *p_owner) { + HashMap::Iterator E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->value--; + if (E->value == 0) { + owners.remove(E); + } +} + +bool RapierShape2D::is_owner(RapierShapeOwner2D *p_owner) const { + return owners.has(p_owner); +} + +const HashMap &RapierShape2D::get_owners() const { + return owners; +} + +RapierShape2D::~RapierShape2D() { + ERR_FAIL_COND(owners.size()); + destroy_rapier_shape(); +} diff --git a/src/shapes/rapier_shape_2d.h b/src/shapes/rapier_shape_2d.h new file mode 100644 index 0000000..7e1155c --- /dev/null +++ b/src/shapes/rapier_shape_2d.h @@ -0,0 +1,68 @@ +#ifndef RAPIER_SHAPE_2D_H +#define RAPIER_SHAPE_2D_H + +#include +#include +#include + +#include "../rapier_include.h" + +using namespace godot; + +class RapierShape2D; + +class RapierShapeOwner2D { +public: + virtual void _shape_changed(RapierShape2D *p_shape) = 0; + virtual void remove_shape(RapierShape2D *p_shape) = 0; + + virtual ~RapierShapeOwner2D() {} +}; + +class RapierShape2D { + RID rid; + Rect2 aabb; + bool configured = false; + + HashMap owners; + + rapier2d::Handle shape_handle = rapier2d::invalid_handle(); + +protected: + void configure(const Rect2 &p_aabb); + + virtual rapier2d::Handle create_rapier_shape() const = 0; + void destroy_rapier_shape(); + +public: + _FORCE_INLINE_ void set_rid(const RID &p_rid) { rid = p_rid; } + _FORCE_INLINE_ RID get_rid() const { return rid; } + + virtual PhysicsServer2D::ShapeType get_type() const = 0; + + virtual void apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const {} + + rapier2d::Handle get_rapier_shape(); + + _FORCE_INLINE_ Rect2 get_aabb(Vector2 origin = Vector2()) const { + Rect2 aabb_clone = aabb; + aabb_clone.position += origin; + return aabb_clone; + } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + void add_owner(RapierShapeOwner2D *p_owner); + void remove_owner(RapierShapeOwner2D *p_owner); + bool is_owner(RapierShapeOwner2D *p_owner) const; + const HashMap &get_owners() const; + + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; + + RapierShape2D() {} + virtual ~RapierShape2D(); +}; + +#endif // RAPIER_SHAPE_2D_H diff --git a/src/shapes/rapier_world_boundary_shape_2d.cpp b/src/shapes/rapier_world_boundary_shape_2d.cpp new file mode 100644 index 0000000..f1b3570 --- /dev/null +++ b/src/shapes/rapier_world_boundary_shape_2d.cpp @@ -0,0 +1,28 @@ +#include "rapier_world_boundary_shape_2d.h" + +rapier2d::Handle RapierWorldBoundaryShape2D::create_rapier_shape() const { + rapier2d::Vector v = { normal.x, normal.y }; + return rapier2d::shape_create_halfspace(&v); +} + +void RapierWorldBoundaryShape2D::apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const { + position.x += normal.x * d; + position.y += normal.y * d; +} + +void RapierWorldBoundaryShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + normal = arr[0]; + d = arr[1]; + configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2.0, 1e4 * 2.0))); +} + +Variant RapierWorldBoundaryShape2D::get_data() const { + Array arr; + arr.resize(2); + arr[0] = normal; + arr[1] = d; + return arr; +} diff --git a/src/shapes/rapier_world_boundary_shape_2d.h b/src/shapes/rapier_world_boundary_shape_2d.h new file mode 100644 index 0000000..4441ae0 --- /dev/null +++ b/src/shapes/rapier_world_boundary_shape_2d.h @@ -0,0 +1,24 @@ +#ifndef RAPIER_WORLD_BOUNDARY_SHAPE_2D_H +#define RAPIER_WORLD_BOUNDARY_SHAPE_2D_H + +#include "rapier_shape_2d.h" + +class RapierWorldBoundaryShape2D : public RapierShape2D { + Vector2 normal; + real_t d = 0.0; + +protected: + virtual rapier2d::Handle create_rapier_shape() const override; + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } + + virtual void apply_rapier_transform(rapier2d::Vector &position, real_t &angle) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0.0; } +}; + +#endif // RAPIER_WORLD_BOUNDARY_SHAPE_2D_H diff --git a/src/spaces/rapier_direct_space_state_2d.cpp b/src/spaces/rapier_direct_space_state_2d.cpp new file mode 100644 index 0000000..5733ffb --- /dev/null +++ b/src/spaces/rapier_direct_space_state_2d.cpp @@ -0,0 +1,192 @@ +#include "rapier_direct_space_state_2d.h" + +int RapierDirectSpaceState2D::_intersect_point(const Vector2 &position, uint64_t canvas_instance_id, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) { + ERR_FAIL_COND_V(space->locked, 0); + ERR_FAIL_COND_V(!is_handle_valid(space->handle), 0); + ERR_FAIL_COND_V(p_result_max < 0, 0); + + rapier2d::Vector rapier_pos = { position.x, position.y }; + + rapier2d::PointHitInfo *hit_info_array = (rapier2d::PointHitInfo *)alloca(p_result_max * sizeof(rapier2d::PointHitInfo)); + + rapier2d::QueryExcludedInfo query_excluded_info = rapier2d::default_query_excluded_info(); + query_excluded_info.query_canvas_instance_id = canvas_instance_id; + query_excluded_info.query_collision_layer_mask = collision_mask; + + uint32_t result_count = rapier2d::intersect_point(space->handle, &rapier_pos, collide_with_bodies, collide_with_areas, hit_info_array, p_result_max, RapierSpace2D::_is_handle_excluded_callback, &query_excluded_info); + ERR_FAIL_COND_V(result_count > (uint32_t)p_result_max, 0); + + for (uint32_t i = 0; i < result_count; i++) { + rapier2d::PointHitInfo &hit_info = hit_info_array[i]; + PhysicsServer2DExtensionShapeResult &result = r_results[i]; + ERR_CONTINUE(!rapier2d::is_user_data_valid(hit_info.user_data)); + + uint32_t shape_index = 0; + RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(hit_info.user_data, shape_index); + ERR_CONTINUE(collision_object_2d == nullptr); + + result.shape = shape_index; + result.collider_id = collision_object_2d->get_instance_id(); + result.rid = collision_object_2d->get_rid(); + + if (result.collider_id.is_valid()) { + result.collider = RapierSpace2D::_get_object_instance_hack(result.collider_id); + } + } + + return result_count; +} + +bool RapierDirectSpaceState2D::_intersect_ray(const Vector2 &from, const Vector2 &to, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, bool hit_from_inside, PhysicsServer2DExtensionRayResult *r_result) { + ERR_FAIL_COND_V(space->locked, false); + ERR_FAIL_COND_V(!is_handle_valid(space->handle), false); + + // Raycast Info + Vector2 begin, end, dir; + begin = from; + end = (to - from); + dir = end.normalized(); + real_t length = end.length(); + + rapier2d::Vector rapier_from = { begin.x, begin.y }; + rapier2d::Vector rapier_dir = { dir.x, dir.y }; + + rapier2d::QueryExcludedInfo query_excluded_info = rapier2d::default_query_excluded_info(); + query_excluded_info.query_collision_layer_mask = collision_mask; + + rapier2d::RayHitInfo hit_info; + bool collide = rapier2d::intersect_ray(space->handle, + &rapier_from, + &rapier_dir, + length, + collide_with_bodies, + collide_with_areas, + hit_from_inside, + &hit_info, + RapierSpace2D::_is_handle_excluded_callback, + &query_excluded_info); + + if (collide) { + r_result->position = Vector2(hit_info.position.x, hit_info.position.y); + r_result->normal = Vector2(hit_info.normal.x, hit_info.normal.y); + + ERR_FAIL_COND_V(!rapier2d::is_user_data_valid(hit_info.user_data), false); + uint32_t shape_index = 0; + RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(hit_info.user_data, shape_index); + ERR_FAIL_NULL_V(collision_object_2d, false); + + r_result->shape = shape_index; + r_result->collider_id = collision_object_2d->get_instance_id(); + r_result->rid = collision_object_2d->get_rid(); + + if (r_result->collider_id.is_valid()) { + r_result->collider = RapierSpace2D::_get_object_instance_hack(r_result->collider_id); + } + + return true; + } + + return false; +} + +bool RapierDirectSpaceState2D::_cast_motion(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, float *p_closest_safe, float *p_closest_unsafe) { + RapierShape2D *shape = space->get_shape_from_rid(shape_rid); + ERR_FAIL_COND_V(!shape, false); + rapier2d::Handle shape_handle = shape->get_rapier_shape(); + ERR_FAIL_COND_V(!rapier2d::is_handle_valid(shape_handle), false); + + rapier2d::Vector rapier_motion = { motion.x, motion.y }; + rapier2d::Vector rapier_pos = { transform.get_origin().x, transform.get_origin().y }; + real_t rotation = transform.get_rotation(); + + rapier2d::QueryExcludedInfo query_excluded_info = rapier2d::default_query_excluded_info(); + query_excluded_info.query_collision_layer_mask = collision_mask; + real_t hit = rapier2d::shape_casting(space->handle, &rapier_motion, &rapier_pos, rotation, shape_handle, collide_with_bodies, collide_with_areas, RapierSpace2D::_is_handle_excluded_callback, &query_excluded_info).toi; + *p_closest_safe = hit; + *p_closest_unsafe = hit; + return true; +} + +bool RapierDirectSpaceState2D::_collide_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, void *results, int32_t max_results, int32_t *result_count) { + RapierShape2D *shape = space->get_shape_from_rid(shape_rid); + ERR_FAIL_COND_V(!shape, false); + rapier2d::Handle shape_handle = shape->get_rapier_shape(); + ERR_FAIL_COND_V(!rapier2d::is_handle_valid(shape_handle), false); + + rapier2d::Vector rapier_motion{ motion.x, motion.y }; + rapier2d::Vector rapier_pos{ transform.get_origin().x, transform.get_origin().y }; + real_t rotation = transform.get_rotation(); + + Vector2 *results_out = static_cast(results); + + rapier2d::QueryExcludedInfo query_excluded_info = rapier2d::default_query_excluded_info(); + query_excluded_info.query_collision_layer_mask = collision_mask; + query_excluded_info.query_exclude = (rapier2d::Handle *)alloca((max_results) * sizeof(rapier2d::Handle)); + query_excluded_info.query_exclude_size = 0; + + int cpt = 0; + int array_idx = 0; + do { + rapier2d::ShapeCastResult result = rapier2d::shape_casting(space->handle, &rapier_motion, &rapier_pos, rotation, shape_handle, collide_with_bodies, collide_with_areas, RapierSpace2D::_is_handle_excluded_callback, &query_excluded_info); + if (!result.collided) { + break; + } + (*result_count)++; + query_excluded_info.query_exclude[query_excluded_info.query_exclude_size++] = result.collider; + + results_out[array_idx++] = Vector2(result.witness1.x, result.witness1.y); + results_out[array_idx++] = Vector2(result.witness2.x, result.witness2.y); + + cpt++; + } while (cpt < max_results); + + return array_idx > 0; +} + +int RapierDirectSpaceState2D::_intersect_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) { + RapierShape2D *shape = space->get_shape_from_rid(shape_rid); + ERR_FAIL_COND_V(!shape, false); + rapier2d::Handle shape_handle = shape->get_rapier_shape(); + ERR_FAIL_COND_V(!rapier2d::is_handle_valid(shape_handle), false); + + rapier2d::Vector rapier_motion{ motion.x, motion.y }; + rapier2d::Vector rapier_pos{ transform.get_origin().x, transform.get_origin().y }; + real_t rotation = transform.get_rotation(); + + // Exclude BODY RID + rapier2d::Handle *rapier_exclude = nullptr; + + rapier2d::QueryExcludedInfo query_excluded_info = rapier2d::default_query_excluded_info(); + query_excluded_info.query_collision_layer_mask = collision_mask; + query_excluded_info.query_exclude = (rapier2d::Handle *)alloca((p_result_max) * sizeof(rapier2d::Handle)); + query_excluded_info.query_exclude_size = 0; + + int cpt = 0; + do { + rapier2d::ShapeCastResult result = rapier2d::shape_casting(space->handle, &rapier_motion, &rapier_pos, rotation, shape_handle, collide_with_bodies, collide_with_areas, RapierSpace2D::_is_handle_excluded_callback, &query_excluded_info); + if (!result.collided) { + break; + } + query_excluded_info.query_exclude[query_excluded_info.query_exclude_size++] = result.collider; + + ERR_CONTINUE_MSG(!rapier2d::is_user_data_valid(result.user_data), "Invalid user data"); + uint32_t shape_index = 0; + RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(result.user_data, shape_index); + ERR_CONTINUE_MSG(!collision_object_2d, "Invalid collision object"); + + PhysicsServer2DExtensionShapeResult &result_out = r_results[cpt]; + + result_out.shape = shape_index; + result_out.rid = collision_object_2d->get_rid(); + result_out.collider_id = collision_object_2d->get_instance_id(); + + if (result_out.collider_id.is_valid()) { + result_out.collider = RapierSpace2D::_get_object_instance_hack(result_out.collider_id); + } + + cpt++; + + } while (cpt < p_result_max); + + return cpt; +} diff --git a/src/spaces/rapier_direct_space_state_2d.h b/src/spaces/rapier_direct_space_state_2d.h new file mode 100644 index 0000000..a3f4552 --- /dev/null +++ b/src/spaces/rapier_direct_space_state_2d.h @@ -0,0 +1,39 @@ +#ifndef RAPIER_DIRECT_SPACE_STATE_2D_H +#define RAPIER_DIRECT_SPACE_STATE_2D_H + +#include "../bodies/rapier_area_2d.h" +#include "../bodies/rapier_body_2d.h" +#include "rapier_space_2d.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace godot; + +class RapierDirectSpaceState2D : public PhysicsDirectSpaceState2DExtension { + GDCLASS(RapierDirectSpaceState2D, PhysicsDirectSpaceState2DExtension); + +protected: + static void _bind_methods() {} + +public: + RapierSpace2D *space = nullptr; + virtual int _intersect_point(const Vector2 &position, uint64_t canvas_instance_id, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) override; + virtual bool _intersect_ray(const Vector2 &from, const Vector2 &to, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, bool hit_from_inside, PhysicsServer2DExtensionRayResult *r_result) override; + virtual int _intersect_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) override; + virtual bool _cast_motion(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, float *p_closest_safe, float *p_closest_unsafe) override; + virtual bool _collide_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, void *results, int32_t max_results, int32_t *result_count) override; + virtual bool _rest_info(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeRestInfo *r_info) override { return false; } + + RapierDirectSpaceState2D() {} +}; + +#endif // RAPIER_DIRECT_SPACE_STATE_2D_H diff --git a/src/rapier_space_2d.cpp b/src/spaces/rapier_space_2d.cpp similarity index 69% rename from src/rapier_space_2d.cpp rename to src/spaces/rapier_space_2d.cpp index d8ca858..96cf05f 100644 --- a/src/rapier_space_2d.cpp +++ b/src/spaces/rapier_space_2d.cpp @@ -1,11 +1,14 @@ #include "rapier_space_2d.h" +#include "rapier_direct_space_state_2d.h" -#include "rapier_body_utils_2d.h" -#include "rapier_physics_server_2d.h" -#include "rapier_project_settings.h" +#include "../servers/rapier_body_utils_2d.h" +#include "../servers/rapier_physics_server_2d.h" +#include "../servers/rapier_project_settings.h" #include +#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 + void RapierSpace2D::body_add_to_active_list(SelfList *p_body) { active_list.add(p_body); } @@ -112,8 +115,6 @@ bool RapierSpace2D::collision_filter_sensor_callback(rapier2d::Handle world_hand return collision_filter_common_callback(world_handle, filter_info, colliders_info); } -#define COLLISION_EVENT_LOG 0 - void RapierSpace2D::collision_event_callback(rapier2d::Handle world_handle, const rapier2d::CollisionEventInfo *event_info) { RapierSpace2D *space = RapierPhysicsServer2D::singleton->get_active_space(world_handle); ERR_FAIL_COND(!space); @@ -166,20 +167,6 @@ void RapierSpace2D::collision_event_callback(rapier2d::Handle world_handle, cons type2 = pObject2->get_type(); } -#if COLLISION_EVENT_LOG - String frame_str(itos(RapierPhysicsServer2D::singleton->get_frame())); - String str_area("A"); - String str_body("B"); - String str_invalid("*/"); - String str_valid("/"); - String id1_str = (type1 == RapierCollisionObject2D::TYPE_AREA ? str_area : str_body) + (pObject1 ? str_valid : str_invalid) + itos(rid1.get_id()) + ":" + itos(shape1); - String id2_str = (type2 == RapierCollisionObject2D::TYPE_AREA ? str_area : str_body) + (pObject2 ? str_valid : str_invalid) + itos(rid2.get_id()) + ":" + itos(shape2); - String event_str(event_info->is_started ? "[COLLISION_ENTER]" : "[COLLISION_EXIT]"); - String remove_str(event_info->is_removed ? " [REMOVED]" : ""); - String sensor_str(event_info->is_sensor ? "SENSOR" : "CONTACT"); - print_line("[" + frame_str + "] " + event_str + remove_str + " [" + sensor_str + "] " + itos(collider_handle1.id) + "(" + id1_str + ")" + " | " + itos(collider_handle2.id) + "(" + id2_str + ")"); -#endif - if (event_info->is_sensor) { if (!instanceId1.is_valid()) { ERR_FAIL_COND_MSG(pObject2, "Should be able to get info about a removed object if the other one is still valid."); @@ -256,18 +243,10 @@ void RapierSpace2D::collision_event_callback(rapier2d::Handle world_handle, cons } } -RapierSpace2D *g_contact_events_space = nullptr; -RapierBody2D *g_contact_events_body1 = nullptr; -RapierBody2D *g_contact_events_body2 = nullptr; -uint32_t g_contact_events_shape1 = 0; -uint32_t g_contact_events_shape2 = 0; - bool RapierSpace2D::contact_force_event_callback(rapier2d::Handle world_handle, const rapier2d::ContactForceEventInfo *event_info) { RapierSpace2D *space = RapierPhysicsServer2D::singleton->get_active_space(world_handle); ERR_FAIL_COND_V(!space, false); - g_contact_events_space = space; - bool send_contacts = false; #ifdef DEBUG_ENABLED @@ -283,8 +262,6 @@ bool RapierSpace2D::contact_force_event_callback(rapier2d::Handle world_handle, } ERR_FAIL_COND_V(!pObject1, false); ERR_FAIL_COND_V(pObject1->get_type() != RapierCollisionObject2D::TYPE_BODY, false); - g_contact_events_body1 = static_cast(pObject1); - g_contact_events_shape1 = shape1; uint32_t shape2 = 0; RapierCollisionObject2D *pObject2 = nullptr; @@ -293,22 +270,20 @@ bool RapierSpace2D::contact_force_event_callback(rapier2d::Handle world_handle, } ERR_FAIL_COND_V(!pObject2, false); ERR_FAIL_COND_V(pObject2->get_type() != RapierCollisionObject2D::TYPE_BODY, false); - g_contact_events_body2 = static_cast(pObject2); - g_contact_events_shape2 = shape2; - if (g_contact_events_body1->can_report_contacts()) { + if (static_cast(pObject1)->can_report_contacts()) { send_contacts = true; } - if (g_contact_events_body2->can_report_contacts()) { + if (static_cast(pObject2)->can_report_contacts()) { send_contacts = true; } return send_contacts; } -bool RapierSpace2D::contact_point_callback(rapier2d::Handle world_handle, const rapier2d::ContactPointInfo *contact_info) { - RapierSpace2D *space = g_contact_events_space; +bool RapierSpace2D::contact_point_callback(rapier2d::Handle world_handle, const rapier2d::ContactPointInfo *contact_info, const rapier2d::ContactForceEventInfo *event_info) { + RapierSpace2D *space = RapierPhysicsServer2D::singleton->get_active_space(world_handle); ERR_FAIL_COND_V(!space, false); ERR_FAIL_COND_V(space->get_handle().id != world_handle.id, false); @@ -324,11 +299,26 @@ bool RapierSpace2D::contact_point_callback(rapier2d::Handle world_handle, const space->add_debug_contact(pos2); } #endif - - ERR_FAIL_COND_V(!g_contact_events_body1, false); - RapierBody2D *body1 = g_contact_events_body1; - ERR_FAIL_COND_V(!g_contact_events_body2, false); - RapierBody2D *body2 = g_contact_events_body2; + // body and shape 1 + uint32_t shape1 = 0; + RapierCollisionObject2D *pObject1 = nullptr; + if (rapier2d::is_user_data_valid(event_info->user_data1)) { + pObject1 = RapierCollisionObject2D::get_collider_user_data(event_info->user_data1, shape1); + } + ERR_FAIL_COND_V(!pObject1, false); + ERR_FAIL_COND_V(pObject1->get_type() != RapierCollisionObject2D::TYPE_BODY, false); + ERR_FAIL_COND_V(!pObject1, false); + RapierBody2D *body1 = static_cast(pObject1); + // body and shape 2 + uint32_t shape2 = 0; + RapierCollisionObject2D *pObject2 = nullptr; + if (rapier2d::is_user_data_valid(event_info->user_data2)) { + pObject2 = RapierCollisionObject2D::get_collider_user_data(event_info->user_data2, shape2); + } + ERR_FAIL_COND_V(!pObject2, false); + ERR_FAIL_COND_V(pObject2->get_type() != RapierCollisionObject2D::TYPE_BODY, false); + ERR_FAIL_COND_V(!pObject2, false); + RapierBody2D *body2 = static_cast(pObject2); real_t depth = MAX(0.0, -contact_info->distance); // negative distance means penetration @@ -339,13 +329,13 @@ bool RapierSpace2D::contact_point_callback(rapier2d::Handle world_handle, const if (body1->can_report_contacts()) { keep_sending_contacts = true; Vector2 vel_pos2(contact_info->velocity_pos_2.x, contact_info->velocity_pos_2.y); - body1->add_contact(pos1, -normal, depth, (int)g_contact_events_shape1, pos2, (int)g_contact_events_shape2, body2->get_instance_id(), body2->get_rid(), vel_pos2, impulse); + body1->add_contact(pos1, -normal, depth, (int)shape1, pos2, (int)shape2, body2->get_instance_id(), body2->get_rid(), vel_pos2, impulse); } if (body2->can_report_contacts()) { keep_sending_contacts = true; Vector2 vel_pos1(contact_info->velocity_pos_1.x, contact_info->velocity_pos_1.y); - body2->add_contact(pos2, normal, depth, (int)g_contact_events_shape2, pos1, (int)g_contact_events_shape1, body1->get_instance_id(), body1->get_rid(), vel_pos1, impulse); + body2->add_contact(pos2, normal, depth, (int)shape2, pos1, (int)shape1, body1->get_instance_id(), body1->get_rid(), vel_pos1, impulse); } return keep_sending_contacts; @@ -387,7 +377,7 @@ void RapierSpace2D::step(real_t p_step) { body->update_gravity(p_step); } - rapier2d::SimulationSettings settings = rapier2d::default_simulation_settings(); + rapier2d::SimulationSettings settings; settings.dt = p_step; settings.gravity.x = default_gravity_dir.x * default_gravity_value; settings.gravity.y = default_gravity_dir.y * default_gravity_value; @@ -419,30 +409,10 @@ void RapierSpace2D::step(real_t p_step) { } } -uint32_t g_query_collision_layer_mask = 0; -ObjectID g_query_canvas_instance_id; -rapier2d::Handle *g_query_exclude = nullptr; -uint32_t g_query_exclude_size = 0; -RID *g_query_exclude_body = nullptr; - -struct QueryCallbackScope { - QueryCallbackScope(uint32_t collision_layer_mask) { - g_query_collision_layer_mask = collision_layer_mask; - } - - ~QueryCallbackScope() { - g_query_collision_layer_mask = 0; - g_query_canvas_instance_id = ObjectID(); - g_query_exclude = nullptr; - g_query_exclude_size = 0; - g_query_exclude_body = nullptr; - } -}; - // Returns true to ignore the collider -bool RapierSpace2D::_is_handle_excluded_callback(const rapier2d::Handle world_handle, const rapier2d::Handle collider_handle, const rapier2d::UserData *user_data) { - for (uint32_t exclude_index = 0; exclude_index < g_query_exclude_size; ++exclude_index) { - if (rapier2d::are_handles_equal(g_query_exclude[exclude_index], collider_handle)) { +bool RapierSpace2D::_is_handle_excluded_callback(const rapier2d::Handle world_handle, const rapier2d::Handle collider_handle, const rapier2d::UserData *user_data, const rapier2d::QueryExcludedInfo *handle_excluded_info) { + for (uint32_t exclude_index = 0; exclude_index < handle_excluded_info->query_exclude_size; ++exclude_index) { + if (rapier2d::are_handles_equal(handle_excluded_info->query_exclude[exclude_index], collider_handle)) { return true; } } @@ -453,17 +423,15 @@ bool RapierSpace2D::_is_handle_excluded_callback(const rapier2d::Handle world_ha RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(*user_data, shape_index); ERR_FAIL_COND_V(!collision_object_2d, false); - if (g_query_canvas_instance_id.is_valid()) { - if (g_query_canvas_instance_id != collision_object_2d->get_canvas_instance_id()) { - return true; - } + if (handle_excluded_info->query_canvas_instance_id != ((uint64_t)collision_object_2d->get_canvas_instance_id())) { + return true; } - if (0 == (collision_object_2d->get_collision_layer() & g_query_collision_layer_mask)) { + if (0 == (collision_object_2d->get_collision_layer() & handle_excluded_info->query_collision_layer_mask)) { return true; } - if (g_query_exclude_body && *g_query_exclude_body == collision_object_2d->get_rid()) { + if (handle_excluded_info->query_exclude_body == collision_object_2d->get_rid().get_id()) { return true; } @@ -651,193 +619,6 @@ RapierSpace2D::~RapierSpace2D() { memdelete(direct_access); } -int RapierDirectSpaceState2D::_intersect_point(const Vector2 &position, uint64_t canvas_instance_id, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) { - ERR_FAIL_COND_V(space->locked, 0); - ERR_FAIL_COND_V(!is_handle_valid(space->handle), 0); - ERR_FAIL_COND_V(p_result_max < 0, 0); - - rapier2d::Vector rapier_pos = { position.x, position.y }; - - rapier2d::PointHitInfo *hit_info_array = (rapier2d::PointHitInfo *)alloca(p_result_max * sizeof(rapier2d::PointHitInfo)); - - QueryCallbackScope queryCallback(collision_mask); - g_query_canvas_instance_id = ObjectID(canvas_instance_id); - - uint32_t result_count = rapier2d::intersect_point(space->handle, &rapier_pos, collide_with_bodies, collide_with_areas, hit_info_array, p_result_max, RapierSpace2D::_is_handle_excluded_callback); - ERR_FAIL_COND_V(result_count > (uint32_t)p_result_max, 0); - - for (uint32_t i = 0; i < result_count; i++) { - rapier2d::PointHitInfo &hit_info = hit_info_array[i]; - PhysicsServer2DExtensionShapeResult &result = r_results[i]; - ERR_CONTINUE(!rapier2d::is_user_data_valid(hit_info.user_data)); - - uint32_t shape_index = 0; - RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(hit_info.user_data, shape_index); - ERR_CONTINUE(collision_object_2d == nullptr); - - result.shape = shape_index; - result.collider_id = collision_object_2d->get_instance_id(); - result.rid = collision_object_2d->get_rid(); - - if (result.collider_id.is_valid()) { - result.collider = RapierSpace2D::_get_object_instance_hack(result.collider_id); - } - } - - return result_count; -} - -bool RapierDirectSpaceState2D::_intersect_ray(const Vector2 &from, const Vector2 &to, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, bool hit_from_inside, PhysicsServer2DExtensionRayResult *r_result) { - ERR_FAIL_COND_V(space->locked, false); - ERR_FAIL_COND_V(!is_handle_valid(space->handle), false); - - // Raycast Info - Vector2 begin, end, dir; - begin = from; - end = (to - from); - dir = end.normalized(); - real_t length = end.length(); - - rapier2d::Vector rapier_from = { begin.x, begin.y }; - rapier2d::Vector rapier_dir = { dir.x, dir.y }; - - QueryCallbackScope queryCallback(collision_mask); - - rapier2d::RayHitInfo hit_info; - bool collide = rapier2d::intersect_ray(space->handle, - &rapier_from, - &rapier_dir, - length, - collide_with_bodies, - collide_with_areas, - hit_from_inside, - &hit_info, - RapierSpace2D::_is_handle_excluded_callback); - - if (collide) { - r_result->position = Vector2(hit_info.position.x, hit_info.position.y); - r_result->normal = Vector2(hit_info.normal.x, hit_info.normal.y); - - ERR_FAIL_COND_V(!rapier2d::is_user_data_valid(hit_info.user_data), false); - uint32_t shape_index = 0; - RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(hit_info.user_data, shape_index); - ERR_FAIL_NULL_V(collision_object_2d, false); - - r_result->shape = shape_index; - r_result->collider_id = collision_object_2d->get_instance_id(); - r_result->rid = collision_object_2d->get_rid(); - - if (r_result->collider_id.is_valid()) { - r_result->collider = RapierSpace2D::_get_object_instance_hack(r_result->collider_id); - } - - return true; - } - - return false; -} - -bool RapierDirectSpaceState2D::_cast_motion(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, real_t *p_closest_safe, real_t *p_closest_unsafe) { - RapierShape2D *shape = space->get_shape_from_rid(shape_rid); - ERR_FAIL_COND_V(!shape, false); - rapier2d::Handle shape_handle = shape->get_rapier_shape(); - ERR_FAIL_COND_V(!rapier2d::is_handle_valid(shape_handle), false); - - rapier2d::Vector rapier_motion = { motion.x, motion.y }; - rapier2d::Vector rapier_pos = { transform.get_origin().x, transform.get_origin().y }; - real_t rotation = transform.get_rotation(); - - QueryCallbackScope queryCallback(collision_mask); - real_t hit = rapier2d::shape_casting(space->handle, &rapier_motion, &rapier_pos, rotation, shape_handle, collide_with_bodies, collide_with_areas, RapierSpace2D::_is_handle_excluded_callback).toi; - *p_closest_safe = hit; - *p_closest_unsafe = hit; - return true; -} - -bool RapierDirectSpaceState2D::_collide_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, void *results, int32_t max_results, int32_t *result_count) { - RapierShape2D *shape = space->get_shape_from_rid(shape_rid); - ERR_FAIL_COND_V(!shape, false); - rapier2d::Handle shape_handle = shape->get_rapier_shape(); - ERR_FAIL_COND_V(!rapier2d::is_handle_valid(shape_handle), false); - - rapier2d::Vector rapier_motion{ motion.x, motion.y }; - rapier2d::Vector rapier_pos{ transform.get_origin().x, transform.get_origin().y }; - real_t rotation = transform.get_rotation(); - - Vector2 *results_out = static_cast(results); - - QueryCallbackScope queryCallback(collision_mask); - g_query_exclude = (rapier2d::Handle *)alloca((max_results) * sizeof(rapier2d::Handle)); - g_query_exclude_size = 0; - - int cpt = 0; - int array_idx = 0; - do { - rapier2d::ShapeCastResult result = rapier2d::shape_casting(space->handle, &rapier_motion, &rapier_pos, rotation, shape_handle, collide_with_bodies, collide_with_areas, RapierSpace2D::_is_handle_excluded_callback); - if (!result.collided) { - break; - } - (*result_count)++; - g_query_exclude[g_query_exclude_size++] = result.collider; - - results_out[array_idx++] = Vector2(result.witness1.x, result.witness1.y); - results_out[array_idx++] = Vector2(result.witness2.x, result.witness2.y); - - cpt++; - } while (cpt < max_results); - - return array_idx > 0; -} - -int RapierDirectSpaceState2D::_intersect_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) { - RapierShape2D *shape = space->get_shape_from_rid(shape_rid); - ERR_FAIL_COND_V(!shape, false); - rapier2d::Handle shape_handle = shape->get_rapier_shape(); - ERR_FAIL_COND_V(!rapier2d::is_handle_valid(shape_handle), false); - - rapier2d::Vector rapier_motion{ motion.x, motion.y }; - rapier2d::Vector rapier_pos{ transform.get_origin().x, transform.get_origin().y }; - real_t rotation = transform.get_rotation(); - - // Exclude BODY RID - rapier2d::Handle *rapier_exclude = nullptr; - - QueryCallbackScope queryCallback(collision_mask); - g_query_exclude = (rapier2d::Handle *)alloca((p_result_max) * sizeof(rapier2d::Handle)); - g_query_exclude_size = 0; - - int cpt = 0; - do { - rapier2d::ShapeCastResult result = rapier2d::shape_casting(space->handle, &rapier_motion, &rapier_pos, rotation, shape_handle, collide_with_bodies, collide_with_areas, RapierSpace2D::_is_handle_excluded_callback); - if (!result.collided) { - break; - } - g_query_exclude[g_query_exclude_size++] = result.collider; - - ERR_CONTINUE(!rapier2d::is_user_data_valid(result.user_data)); - uint32_t shape_index = 0; - RapierCollisionObject2D *collision_object_2d = RapierCollisionObject2D::get_collider_user_data(result.user_data, shape_index); - ERR_CONTINUE(!collision_object_2d); - - PhysicsServer2DExtensionShapeResult &result_out = r_results[cpt]; - - result_out.shape = shape_index; - result_out.rid = collision_object_2d->get_rid(); - result_out.collider_id = collision_object_2d->get_instance_id(); - - if (result_out.collider_id.is_valid()) { - result_out.collider = RapierSpace2D::_get_object_instance_hack(result_out.collider_id); - } - - cpt++; - - } while (cpt < p_result_max); - - return cpt; -} - -#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 - bool RapierSpace2D::test_body_motion(RapierBody2D *p_body, const Transform2D &p_from, const Vector2 &p_motion, double p_margin, bool p_collide_separation_ray, bool p_recovery_as_collision, PhysicsServer2DExtensionMotionResult *r_result) const { Transform2D body_transform = p_from; // Because body_transform needs to be modified during recovery // Step 1: recover motion. @@ -896,20 +677,21 @@ bool RapierSpace2D::rapier_shape_cast(rapier2d::Handle p_shape_handle, const Tra rapier2d::Vector rapier_pos{ p_transform.get_origin().x, p_transform.get_origin().y }; real_t rotation = p_transform.get_rotation(); - QueryCallbackScope queryCallback(p_collision_mask); - g_query_exclude = (rapier2d::Handle *)alloca((p_max_results) * sizeof(rapier2d::Handle)); - g_query_exclude_size = 0; + rapier2d::QueryExcludedInfo handle_excluded_info = rapier2d::default_query_excluded_info(); + handle_excluded_info.query_exclude = (rapier2d::Handle *)alloca((p_max_results) * sizeof(rapier2d::Handle)); + handle_excluded_info.query_collision_layer_mask = p_collision_mask; + handle_excluded_info.query_exclude_size = 0; int cpt = 0; int array_idx = 0; do { rapier2d::ShapeCastResult &result = p_results[cpt]; - result = rapier2d::shape_casting(handle, &rapier_motion, &rapier_pos, rotation, p_shape_handle, p_collide_with_bodies, p_collide_with_areas, RapierSpace2D::_is_handle_excluded_callback); + result = rapier2d::shape_casting(handle, &rapier_motion, &rapier_pos, rotation, p_shape_handle, p_collide_with_bodies, p_collide_with_areas, RapierSpace2D::_is_handle_excluded_callback, &handle_excluded_info); if (!result.collided) { break; } (*p_result_count)++; - g_query_exclude[g_query_exclude_size++] = result.collider; + handle_excluded_info.query_exclude[handle_excluded_info.query_exclude_size++] = result.collider; cpt++; } while (cpt < p_max_results); @@ -924,12 +706,13 @@ int RapierSpace2D::rapier_intersect_shape(rapier2d::Handle p_shape_handle, const rapier2d::Vector rapier_pos{ p_transform.get_origin().x, p_transform.get_origin().y }; real_t rotation = p_transform.get_rotation(); - QueryCallbackScope queryCallback(p_collision_mask); - g_query_exclude = (rapier2d::Handle *)alloca((p_max_results) * sizeof(rapier2d::Handle)); - g_query_exclude_size = 0; - g_query_exclude_body = &p_exclude_body; + rapier2d::QueryExcludedInfo handle_excluded_info = rapier2d::default_query_excluded_info(); + handle_excluded_info.query_exclude = (rapier2d::Handle *)alloca((p_max_results) * sizeof(rapier2d::Handle)); + handle_excluded_info.query_collision_layer_mask = p_collision_mask; + handle_excluded_info.query_exclude_size = 0; + handle_excluded_info.query_exclude_body = p_exclude_body.get_id(); - return rapier2d::intersect_shape(handle, &rapier_pos, rotation, p_shape_handle, p_collide_with_bodies, p_collide_with_areas, p_results, p_max_results, RapierSpace2D::_is_handle_excluded_callback); + return rapier2d::intersect_shape(handle, &rapier_pos, rotation, p_shape_handle, p_collide_with_bodies, p_collide_with_areas, p_results, p_max_results, RapierSpace2D::_is_handle_excluded_callback, &handle_excluded_info); } int RapierSpace2D::rapier_intersect_aabb(Rect2 p_aabb, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, rapier2d::PointHitInfo *p_results, int32_t p_max_results, int32_t *p_result_count, RID p_exclude_body) const { @@ -937,10 +720,11 @@ int RapierSpace2D::rapier_intersect_aabb(Rect2 p_aabb, uint32_t p_collision_mask rapier2d::Vector rect_begin{ p_aabb.position.x, p_aabb.position.y }; rapier2d::Vector rect_end{ p_aabb.get_end().x, p_aabb.get_end().y }; - QueryCallbackScope queryCallback(p_collision_mask); - g_query_exclude = (rapier2d::Handle *)alloca((p_max_results) * sizeof(rapier2d::Handle)); - g_query_exclude_size = 0; - g_query_exclude_body = &p_exclude_body; + rapier2d::QueryExcludedInfo handle_excluded_info = rapier2d::default_query_excluded_info(); + handle_excluded_info.query_exclude = (rapier2d::Handle *)alloca((p_max_results) * sizeof(rapier2d::Handle)); + handle_excluded_info.query_collision_layer_mask = p_collision_mask; + handle_excluded_info.query_exclude_size = 0; + handle_excluded_info.query_exclude_body = p_exclude_body.get_id(); - return rapier2d::intersect_aabb(handle, &rect_begin, &rect_end, p_collide_with_bodies, p_collide_with_areas, p_results, p_max_results, RapierSpace2D::_is_handle_excluded_callback); + return rapier2d::intersect_aabb(handle, &rect_begin, &rect_end, p_collide_with_bodies, p_collide_with_areas, p_results, p_max_results, RapierSpace2D::_is_handle_excluded_callback, &handle_excluded_info); } diff --git a/src/rapier_space_2d.h b/src/spaces/rapier_space_2d.h similarity index 78% rename from src/rapier_space_2d.h rename to src/spaces/rapier_space_2d.h index 8e2c8d9..a5416ff 100644 --- a/src/rapier_space_2d.h +++ b/src/spaces/rapier_space_2d.h @@ -1,8 +1,8 @@ #ifndef RAPIER_SPACE_2D_H #define RAPIER_SPACE_2D_H -#include "rapier_area_2d.h" -#include "rapier_body_2d.h" +#include "../bodies/rapier_area_2d.h" +#include "../bodies/rapier_body_2d.h" #include #include @@ -18,29 +18,7 @@ using namespace godot; class RapierSpace2D; - -class RapierDirectSpaceState2D : public PhysicsDirectSpaceState2DExtension { - GDCLASS(RapierDirectSpaceState2D, PhysicsDirectSpaceState2DExtension); - -protected: - static void _bind_methods() {} - -private: - enum RID_TYPE { RID_TYPE_BODY, - RID_TYPE_SHAPE }; - void _hashset_rid_to_handle(rapier2d::Handle *r_rapier_exclude, uint32_t *r_rapier_exclude_size, const HashSet &exclude, RID_TYPE p_rid_type = RID_TYPE_BODY); - -public: - RapierSpace2D *space = nullptr; - virtual int _intersect_point(const Vector2 &position, uint64_t canvas_instance_id, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) override; - virtual bool _intersect_ray(const Vector2 &from, const Vector2 &to, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, bool hit_from_inside, PhysicsServer2DExtensionRayResult *r_result) override; - virtual int _intersect_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeResult *r_results, int32_t p_result_max) override; - virtual bool _cast_motion(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, real_t *p_closest_safe, real_t *p_closest_unsafe) override; - virtual bool _collide_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, void *results, int32_t max_results, int32_t *result_count) override; - virtual bool _rest_info(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, PhysicsServer2DExtensionShapeRestInfo *r_info) override { return false; } - - RapierDirectSpaceState2D() {} -}; +class RapierDirectSpaceState2D; class RapierSpace2D { RapierDirectSpaceState2D *direct_access = nullptr; @@ -113,9 +91,9 @@ class RapierSpace2D { static void collision_event_callback(rapier2d::Handle world_handle, const rapier2d::CollisionEventInfo *event_info); static bool contact_force_event_callback(rapier2d::Handle world_handle, const rapier2d::ContactForceEventInfo *event_info); - static bool contact_point_callback(rapier2d::Handle world_handle, const rapier2d::ContactPointInfo *contact_info); + static bool contact_point_callback(rapier2d::Handle world_handle, const rapier2d::ContactPointInfo *contact_info, const rapier2d::ContactForceEventInfo *event_info); - static bool _is_handle_excluded_callback(const rapier2d::Handle world_handle, const rapier2d::Handle collider_handle, const rapier2d::UserData *collider); + static bool _is_handle_excluded_callback(const rapier2d::Handle world_handle, const rapier2d::Handle collider_handle, const rapier2d::UserData *collider, const rapier2d::QueryExcludedInfo *handle_excluded_info); static Object *_get_object_instance_hack(uint64_t p_object_id) { return reinterpret_cast((GodotObject *)(internal::gdextension_interface_object_get_instance_from_id(p_object_id)));