From 10f879bf883ed364a9b0eafe40aba03c59b6fbfb Mon Sep 17 00:00:00 2001 From: AndreaCatania Date: Tue, 7 Nov 2017 15:22:09 +0100 Subject: [PATCH] Rewritten kinematic system --- modules/bullet/bullet_physics_server.cpp | 26 +- modules/bullet/bullet_physics_server.h | 5 +- modules/bullet/collision_object_bullet.cpp | 2 +- modules/bullet/godot_result_callbacks.cpp | 47 --- modules/bullet/godot_result_callbacks.h | 37 -- modules/bullet/rigid_body_bullet.cpp | 46 +-- modules/bullet/rigid_body_bullet.h | 10 +- modules/bullet/space_bullet.cpp | 380 +++++++++------------ modules/bullet/space_bullet.h | 21 +- scene/3d/physics_body.cpp | 5 +- servers/physics/body_sw.cpp | 5 + servers/physics/body_sw.h | 4 + servers/physics/physics_server_sw.cpp | 17 +- servers/physics/physics_server_sw.h | 5 +- servers/physics_server.cpp | 3 + servers/physics_server.h | 5 +- 16 files changed, 267 insertions(+), 351 deletions(-) diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp index 26560742962e..3fbbdf50b35a 100644 --- a/modules/bullet/bullet_physics_server.cpp +++ b/modules/bullet/bullet_physics_server.cpp @@ -634,6 +634,28 @@ float BulletPhysicsServer::body_get_param(RID p_body, BodyParameter p_param) con return body->get_param(p_param); } +void BulletPhysicsServer::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { + RigidBodyBullet *body = rigid_body_owner.get(p_body); + ERR_FAIL_COND(!body); + + if (body->get_kinematic_utilities()) { + + body->get_kinematic_utilities()->setSafeMargin(p_margin); + } +} + +real_t BulletPhysicsServer::body_get_kinematic_safe_margin(RID p_body) const { + RigidBodyBullet *body = rigid_body_owner.get(p_body); + ERR_FAIL_COND_V(!body, 0); + + if (body->get_kinematic_utilities()) { + + return body->get_kinematic_utilities()->safe_margin; + } + + return 0; +} + void BulletPhysicsServer::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { RigidBodyBullet *body = rigid_body_owner.get(p_body); ERR_FAIL_COND(!body); @@ -796,12 +818,12 @@ PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) { return BulletPhysicsDirectBodyState::get_singleton(body); } -bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) { +bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result) { RigidBodyBullet *body = rigid_body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, r_result); } RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) { diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h index e66e8a5ac1b2..c1859ca149d1 100644 --- a/modules/bullet/bullet_physics_server.h +++ b/modules/bullet/bullet_physics_server.h @@ -209,6 +209,9 @@ class BulletPhysicsServer : public PhysicsServer { virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); virtual float body_get_param(RID p_body, BodyParameter p_param) const; + virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin); + virtual real_t body_get_kinematic_safe_margin(RID p_body) const; + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant); virtual Variant body_get_state(RID p_body, BodyState p_state) const; @@ -246,7 +249,7 @@ class BulletPhysicsServer : public PhysicsServer { // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL); + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result = NULL); /* SOFT BODY API */ diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp index 5739568d91f5..449d4322d402 100644 --- a/modules/bullet/collision_object_bullet.cpp +++ b/modules/bullet/collision_object_bullet.cpp @@ -101,7 +101,7 @@ void CollisionObjectBullet::remove_collision_exception(const CollisionObjectBull } bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const { - return !bt_collision_object->checkCollideWithOverride(p_otherCollisionObject->bt_collision_object); + return !bt_collision_object->checkCollideWith(p_otherCollisionObject->bt_collision_object); } void CollisionObjectBullet::set_collision_enabled(bool p_enabled) { diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp index 409d12f08069..bc60c9cb6bcd 100644 --- a/modules/bullet/godot_result_callbacks.cpp +++ b/modules/bullet/godot_result_callbacks.cpp @@ -242,50 +242,3 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp return cp.getDistance(); } - -bool GodotRecoverAndClosestContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const { - const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask); - if (needs) { - btCollisionObject *btObj = static_cast(proxy0->m_clientObject); - CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); - if (gObj == m_self_object) { - return false; - } else { - if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) { - return false; - } else if (m_self_object->has_collision_exception(gObj)) { - return false; - } - } - return true; - } else { - return false; - } -} - -btScalar GodotRecoverAndClosestContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) { - - if (cp.getDistance() < -MAX_PENETRATION_DEPTH) { - if (m_most_penetrated_distance > cp.getDistance()) { - m_most_penetrated_distance = cp.getDistance(); - - // take other object - btScalar sign(1); - if (m_self_object == colObj0Wrap->getCollisionObject()->getUserPointer()) { - m_pointCollisionObject = colObj1Wrap->getCollisionObject(); - m_other_compound_shape_index = cp.m_index1; - } else { - m_pointCollisionObject = colObj0Wrap->getCollisionObject(); - sign = -1; - m_other_compound_shape_index = cp.m_index0; - } - - m_pointNormalWorld = cp.m_normalWorldOnB * sign; - m_pointWorld = cp.getPositionWorldOnB(); - m_penetration_distance = cp.getDistance(); - - m_recover_penetration -= cp.m_normalWorldOnB * sign * (cp.getDistance() + MAX_PENETRATION_DEPTH); - } - } - return 1; -} diff --git a/modules/bullet/godot_result_callbacks.h b/modules/bullet/godot_result_callbacks.h index d505bd0de181..68dff5b12a96 100644 --- a/modules/bullet/godot_result_callbacks.h +++ b/modules/bullet/godot_result_callbacks.h @@ -36,8 +36,6 @@ #include "btBulletDynamicsCommon.h" #include "servers/physics_server.h" -#define MAX_PENETRATION_DEPTH 0.005 - class RigidBodyBullet; /// This class is required to implement custom collision behaviour in the broadphase @@ -151,39 +149,4 @@ struct GodotRestInfoContactResultCallback : public btCollisionWorld::ContactResu virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1); }; -struct GodotRecoverAndClosestContactResultCallback : public btCollisionWorld::ContactResultCallback { -public: - btVector3 m_pointNormalWorld; - btVector3 m_pointWorld; - btScalar m_penetration_distance; - int m_other_compound_shape_index; - const btCollisionObject *m_pointCollisionObject; - - const RigidBodyBullet *m_self_object; - bool m_ignore_areas; - - btScalar m_most_penetrated_distance; - btVector3 m_recover_penetration; - - GodotRecoverAndClosestContactResultCallback() - : m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(NULL), m_ignore_areas(true), m_most_penetrated_distance(1e20), m_recover_penetration(0, 0, 0) {} - - GodotRecoverAndClosestContactResultCallback(const RigidBodyBullet *p_self_object, bool p_ignore_areas) - : m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(p_self_object), m_ignore_areas(p_ignore_areas), m_most_penetrated_distance(9999999999), m_recover_penetration(0, 0, 0) {} - - void reset() { - m_pointCollisionObject = NULL; - m_most_penetrated_distance = 1e20; - m_recover_penetration.setZero(); - } - - bool hasHit() { - return m_pointCollisionObject; - } - - virtual bool needsCollision(btBroadphaseProxy *proxy0) const; - - virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1); -}; - #endif // GODOT_RESULT_CALLBACKS_H diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp index 5d0513db76f6..98ae82bc5fcc 100644 --- a/modules/bullet/rigid_body_bullet.cpp +++ b/modules/bullet/rigid_body_bullet.cpp @@ -177,31 +177,21 @@ PhysicsDirectSpaceState *BulletPhysicsDirectBodyState::get_space_state() { } RigidBodyBullet::KinematicUtilities::KinematicUtilities(RigidBodyBullet *p_owner) - : m_owner(p_owner), m_margin(0.01) // Godot default margin 0.001 -{ - m_ghostObject = bulletnew(btPairCachingGhostObject); - - int clearedCurrentFlags = m_ghostObject->getCollisionFlags(); - clearedCurrentFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT); - - m_ghostObject->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_KINEMATIC_OBJECT); - m_ghostObject->setUserPointer(p_owner); - m_ghostObject->setUserIndex(TYPE_KINEMATIC_GHOST_BODY); - - resetDefShape(); + : owner(p_owner), + safe_margin(0.001) { } RigidBodyBullet::KinematicUtilities::~KinematicUtilities() { - just_delete_shapes(m_shapes.size()); // don't need to resize - bulletdelete(m_ghostObject); + just_delete_shapes(shapes.size()); // don't need to resize } -void RigidBodyBullet::KinematicUtilities::resetDefShape() { - m_ghostObject->setCollisionShape(BulletPhysicsServer::get_empty_shape()); +void RigidBodyBullet::KinematicUtilities::setSafeMargin(btScalar p_margin) { + safe_margin = p_margin; + copyAllOwnerShapes(); } void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { - const Vector &shapes_wrappers(m_owner->get_shapes_wrappers()); + const Vector &shapes_wrappers(owner->get_shapes_wrappers()); const int shapes_count = shapes_wrappers.size(); just_delete_shapes(shapes_count); @@ -213,35 +203,35 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { if (!shape_wrapper->active) { continue; } - m_shapes[i].transform = shape_wrapper->transform; + shapes[i].transform = shape_wrapper->transform; - btConvexShape *&kin_shape_ref = m_shapes[i].shape; + btConvexShape *&kin_shape_ref = shapes[i].shape; switch (shape_wrapper->shape->get_type()) { case PhysicsServer::SHAPE_SPHERE: { SphereShapeBullet *sphere = static_cast(shape_wrapper->shape); - kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * m_owner->body_scale[0] + m_margin); + kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * owner->body_scale[0] + safe_margin); break; } case PhysicsServer::SHAPE_BOX: { BoxShapeBullet *box = static_cast(shape_wrapper->shape); - kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * m_owner->body_scale) + btVector3(m_margin, m_margin, m_margin)); + kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * owner->body_scale) + btVector3(safe_margin, safe_margin, safe_margin)); break; } case PhysicsServer::SHAPE_CAPSULE: { CapsuleShapeBullet *capsule = static_cast(shape_wrapper->shape); - kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * m_owner->body_scale[0] + m_margin, capsule->get_height() * m_owner->body_scale[1] + m_margin); + kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * owner->body_scale[0] + safe_margin, capsule->get_height() * owner->body_scale[1] + safe_margin); break; } case PhysicsServer::SHAPE_CONVEX_POLYGON: { ConvexPolygonShapeBullet *godot_convex = static_cast(shape_wrapper->shape); kin_shape_ref = ShapeBullet::create_shape_convex(godot_convex->vertices); - kin_shape_ref->setLocalScaling(m_owner->body_scale + btVector3(m_margin, m_margin, m_margin)); + kin_shape_ref->setLocalScaling(owner->body_scale + btVector3(safe_margin, safe_margin, safe_margin)); break; } case PhysicsServer::SHAPE_RAY: { RayShapeBullet *godot_ray = static_cast(shape_wrapper->shape); - kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * m_owner->body_scale[1] + m_margin); + kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * owner->body_scale[1] + safe_margin); break; } default: @@ -252,12 +242,12 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { } void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) { - for (int i = m_shapes.size() - 1; 0 <= i; --i) { - if (m_shapes[i].shape) { - bulletdelete(m_shapes[i].shape); + for (int i = shapes.size() - 1; 0 <= i; --i) { + if (shapes[i].shape) { + bulletdelete(shapes[i].shape); } } - m_shapes.resize(new_size); + shapes.resize(new_size); } RigidBodyBullet::RigidBodyBullet() diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h index 0cf3f9f60558..ab3c3e58b2ad 100644 --- a/modules/bullet/rigid_body_bullet.h +++ b/modules/bullet/rigid_body_bullet.h @@ -162,17 +162,15 @@ class RigidBodyBullet : public RigidCollisionObjectBullet { }; struct KinematicUtilities { - RigidBodyBullet *m_owner; - btScalar m_margin; - btManifoldArray m_manifoldArray; ///keep track of the contact manifolds - class btPairCachingGhostObject *m_ghostObject; - Vector m_shapes; + RigidBodyBullet *owner; + btScalar safe_margin; + Vector shapes; KinematicUtilities(RigidBodyBullet *p_owner); ~KinematicUtilities(); + void setSafeMargin(btScalar p_margin); /// Used to set the default shape to ghost - void resetDefShape(); void copyAllOwnerShapes(); private: diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index af7f511fabdf..9df01aee3eb8 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -50,9 +50,6 @@ #include "ustring.h" #include -// test only -//#include "scene/3d/immediate_geometry.h" - BulletPhysicsDirectSpaceState::BulletPhysicsDirectSpaceState(SpaceBullet *p_space) : PhysicsDirectSpaceState(), space(p_space) {} @@ -174,7 +171,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf btResult.m_collisionFilterGroup = p_collision_layer; btResult.m_collisionFilterMask = p_object_type_mask; - space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult); + space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, 0.002); if (btResult.hasHit()) { if (btCollisionObject::CO_RIGID_BODY == btResult.m_hitCollisionObject->getInternalType()) { @@ -281,10 +278,6 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_ btVector3 bt_point; G_TO_B(p_point, bt_point); - btGjkEpaPenetrationDepthSolver gjk_epa_pen_solver; - btVoronoiSimplexSolver gjk_simplex_solver; - gjk_simplex_solver.setEqualVertexThreshold(0.); - btSphereShape point_shape(0.); btCollisionShape *shape; @@ -308,7 +301,7 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_ input.m_transformB = body_transform * child_transform; btPointCollector result; - btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, &gjk_simplex_solver, &gjk_epa_pen_solver); + btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, space->gjk_simplex_solver, space->gjk_epa_pen_solver); gjk_pair_detector.getClosestPoints(input, result, 0); if (out_distance > result.m_distance) { @@ -558,13 +551,10 @@ btScalar calculateGodotCombinedRestitution(const btCollisionObject *body0, const } void SpaceBullet::create_empty_world(bool p_create_soft_world) { - assert(NULL == broadphase); - assert(NULL == dispatcher); - assert(NULL == solver); - assert(NULL == collisionConfiguration); - assert(NULL == dynamicsWorld); - assert(NULL == ghostPairCallback); - assert(NULL == godotFilterCallback); + + gjk_epa_pen_solver = bulletnew(btGjkEpaPenetrationDepthSolver); + gjk_simplex_solver = bulletnew(btVoronoiSimplexSolver); + gjk_simplex_solver->setEqualVertexThreshold(0.f); void *world_mem; if (p_create_soft_world) { @@ -611,13 +601,6 @@ void SpaceBullet::create_empty_world(bool p_create_soft_world) { } void SpaceBullet::destroy_world() { - assert(NULL != broadphase); - assert(NULL != dispatcher); - assert(NULL != solver); - assert(NULL != collisionConfiguration); - assert(NULL != dynamicsWorld); - assert(NULL != ghostPairCallback); - assert(NULL != godotFilterCallback); /// The world elements (like: Collision Objects, Constraints, Shapes) are managed by godot @@ -637,14 +620,13 @@ void SpaceBullet::destroy_world() { bulletdelete(dispatcher); bulletdelete(collisionConfiguration); bulletdelete(soft_body_world_info); + bulletdelete(gjk_simplex_solver); + bulletdelete(gjk_epa_pen_solver); } void SpaceBullet::check_ghost_overlaps() { /// Algorith support variables - btGjkEpaPenetrationDepthSolver gjk_epa_pen_solver; - btVoronoiSimplexSolver gjk_simplex_solver; - gjk_simplex_solver.setEqualVertexThreshold(0.f); btConvexShape *other_body_shape; btConvexShape *area_shape; btGjkPairDetector::ClosestPointInput gjk_input; @@ -701,7 +683,7 @@ void SpaceBullet::check_ghost_overlaps() { gjk_input.m_transformB = otherObject->get_transform__bullet() * otherObject->get_compound_shape()->getChildTransform(z); btPointCollector result; - btGjkPairDetector gjk_pair_detector(area_shape, other_body_shape, &gjk_simplex_solver, &gjk_epa_pen_solver); + btGjkPairDetector gjk_pair_detector(area_shape, other_body_shape, gjk_simplex_solver, gjk_epa_pen_solver); gjk_pair_detector.getClosestPoints(gjk_input, result, 0); if (0 >= result.m_distance) { @@ -743,23 +725,11 @@ void SpaceBullet::check_body_collision() { const int numManifolds = dynamicsWorld->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; ++i) { btPersistentManifold *contactManifold = dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i); - const btCollisionObject *obA = contactManifold->getBody0(); - const btCollisionObject *obB = contactManifold->getBody1(); - - if (btCollisionObject::CO_RIGID_BODY != obA->getInternalType() || btCollisionObject::CO_RIGID_BODY != obB->getInternalType()) { - // This checks is required to be sure the ghost object is skipped - // The ghost object "getUserPointer" return the BodyBullet owner so this check is required - continue; - } - - // Asserts all Godot objects are assigned - assert(NULL != obA->getUserPointer()); - assert(NULL != obB->getUserPointer()); // I know this static cast is a bit risky. But I'm checking its type just after it. // This allow me to avoid a lot of other cast and checks - RigidBodyBullet *bodyA = static_cast(obA->getUserPointer()); - RigidBodyBullet *bodyB = static_cast(obB->getUserPointer()); + RigidBodyBullet *bodyA = static_cast(contactManifold->getBody0()->getUserPointer()); + RigidBodyBullet *bodyB = static_cast(contactManifold->getBody1()->getUserPointer()); if (CollisionObjectBullet::TYPE_RIGID_BODY == bodyA->getType() && CollisionObjectBullet::TYPE_RIGID_BODY == bodyB->getType()) { if (!bodyA->can_add_collision() && !bodyB->can_add_collision()) { @@ -784,13 +754,13 @@ void SpaceBullet::check_body_collision() { if (bodyA->can_add_collision()) { B_TO_G(pt.getPositionWorldOnB(), collisionWorldPosition); /// pt.m_localPointB Doesn't report the exact point in local space - B_TO_G(pt.getPositionWorldOnB() - obB->getWorldTransform().getOrigin(), collisionLocalPosition); + B_TO_G(pt.getPositionWorldOnB() - contactManifold->getBody1()->getWorldTransform().getOrigin(), collisionLocalPosition); bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, pt.m_index1, pt.m_index0); } if (bodyB->can_add_collision()) { B_TO_G(pt.getPositionWorldOnA(), collisionWorldPosition); /// pt.m_localPointA Doesn't report the exact point in local space - B_TO_G(pt.getPositionWorldOnA() - obA->getWorldTransform().getOrigin(), collisionLocalPosition); + B_TO_G(pt.getPositionWorldOnA() - contactManifold->getBody0()->getWorldTransform().getOrigin(), collisionLocalPosition); bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, pt.m_index0, pt.m_index1); } @@ -817,7 +787,12 @@ void SpaceBullet::update_gravity() { /// I'm leaving this here just for future tests. /// Debug motion and normal vector drawing #define debug_test_motion 0 +#define PERFORM_INITIAL_UNSTACK 1 + #if debug_test_motion + +#include "scene/3d/immediate_geometry.h" + static ImmediateGeometry *motionVec(NULL); static ImmediateGeometry *normalLine(NULL); static Ref red_mat; @@ -825,10 +800,10 @@ static Ref blue_mat; #endif #define IGNORE_AREAS_TRUE true -bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) { +bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, PhysicsServer::MotionResult *r_result) { #if debug_test_motion - /// Yes I know this is not good, but I've used it as fast debugging. + /// Yes I know this is not good, but I've used it as fast debugging hack. /// I'm leaving it here just for speedup the other eventual debugs if (!normalLine) { motionVec = memnew(ImmediateGeometry); @@ -866,43 +841,21 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f // } //} - btVector3 recover_initial_position; - recover_initial_position.setZero(); + btVector3 recover_initial_position(0, 0, 0); -/// I'm performing the unstack at the end of movement so I'm sure the player is unstacked even after the movement. -/// I've removed the initial unstack because this is useful just for the first tick since after the first -/// the real unstack is performed at the end of process. -/// However I'm leaving here the old code. -/// Note: It has a bug when two shapes touches something simultaneously the body is moved too much away (I'm not fixing it for the reason written above). -#define INITIAL_UNSTACK 0 -#if !INITIAL_UNSTACK btTransform body_safe_position; G_TO_B(p_from, body_safe_position); -//btTransform body_unsafe_positino; -//G_TO_B(p_from, body_unsafe_positino); -#else - btTransform body_safe_position; - btTransform body_unsafe_positino; - { /// Phase one - multi shapes depenetration using margin - G_TO_B(p_from, body_safe_position); - G_TO_B(p_from, body_unsafe_positino); - - // MAX_PENETRATION_DEPTH Is useful have the ghost a bit penetrated so I can detect the floor easily - recover_from_penetration(p_body, body_safe_position, MAX_PENETRATION_DEPTH, /* p_depenetration_speed */ 1, recover_initial_position); - /// Not required if I put p_depenetration_speed = 1 - //for(int t = 0; t<4; ++t){ - // if(!recover_from_penetration(p_body, body_safe_position, MAX_PENETRATION_DEPTH, /* p_depenetration_speed */0.2, recover_initial_position)){ - // break; - // } - //} + { /// Phase one - multi shapes depenetration using margin +#if PERFORM_INITIAL_UNSTACK + if (recover_from_penetration(p_body, body_safe_position, recover_initial_position)) { - // Add recover position to "From" and "To" transforms - body_safe_position.getOrigin() += recover_initial_position; - } + // Add recover position to "From" and "To" transforms + body_safe_position.getOrigin() += recover_initial_position; + } #endif + } - int shape_most_recovered(-1); btVector3 recovered_motion; G_TO_B(p_motion, recovered_motion); const int shape_count(p_body->get_shape_count()); @@ -930,127 +883,53 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f continue; } - btTransform shape_xform_from; - G_TO_B(p_body->get_shape_transform(shIndex), shape_xform_from); - //btTransform shape_xform_to(shape_xform_from); + btTransform shape_world_from; + G_TO_B(p_body->get_shape_transform(shIndex), shape_world_from); // Add local shape transform - shape_xform_from.getOrigin() += body_safe_position.getOrigin(); - shape_xform_from.getBasis() *= body_safe_position.getBasis(); + shape_world_from = body_safe_position * shape_world_from; - btTransform shape_xform_to(shape_xform_from); - //shape_xform_to.getOrigin() += body_unsafe_positino.getOrigin(); - //shape_xform_to.getBasis() *= body_unsafe_positino.getBasis(); - shape_xform_to.getOrigin() += recovered_motion; + btTransform shape_world_to(shape_world_from); + shape_world_to.getOrigin() += recovered_motion; - GodotKinClosestConvexResultCallback btResult(shape_xform_from.getOrigin(), shape_xform_to.getOrigin(), p_body, IGNORE_AREAS_TRUE); + GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, IGNORE_AREAS_TRUE); btResult.m_collisionFilterGroup = p_body->get_collision_layer(); btResult.m_collisionFilterMask = p_body->get_collision_mask(); - dynamicsWorld->convexSweepTest(convex_shape_test, shape_xform_from, shape_xform_to, btResult); + dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, 0.002); if (btResult.hasHit()) { - //recovered_motion *= btResult.m_closestHitFraction; /// Since for each sweep test I fix the motion of new shapes in base the recover result, - /// if another shape will hit something it means that has a deepest recovering respect the previous shape - shape_most_recovered = shIndex; + /// if another shape will hit something it means that has a deepest penetration respect the previous shape + recovered_motion *= btResult.m_closestHitFraction; } } } - bool hasHit = false; - - { /// Phase three - contact test with margin - - btGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject; - - GodotRecoverAndClosestContactResultCallback result_callabck; - - if (false && 0 <= shape_most_recovered) { - result_callabck.m_self_object = p_body; - result_callabck.m_ignore_areas = IGNORE_AREAS_TRUE; - result_callabck.m_collisionFilterGroup = p_body->get_collision_layer(); - result_callabck.m_collisionFilterMask = p_body->get_collision_mask(); - - const RigidBodyBullet::KinematicShape &kin(p_body->get_kinematic_utilities()->m_shapes[shape_most_recovered]); - ghost->setCollisionShape(kin.shape); - ghost->setWorldTransform(body_safe_position); - - ghost->getWorldTransform().getOrigin() += recovered_motion; - ghost->getWorldTransform().getOrigin() += kin.transform.getOrigin(); - ghost->getWorldTransform().getBasis() *= kin.transform.getBasis(); + bool hasPenetration = false; - dynamicsWorld->contactTest(ghost, result_callabck); - - recovered_motion += result_callabck.m_recover_penetration; // Required to avoid all kind of penetration - - } else { - // The sweep result does not return a penetrated shape, so I've to check all shapes - // Then return the most penetrated shape - - GodotRecoverAndClosestContactResultCallback iter_result_callabck(p_body, IGNORE_AREAS_TRUE); - iter_result_callabck.m_collisionFilterGroup = p_body->get_collision_layer(); - iter_result_callabck.m_collisionFilterMask = p_body->get_collision_mask(); - - btScalar max_penetration(99999999999); - for (int i = 0; i < shape_count; ++i) { - - const RigidBodyBullet::KinematicShape &kin(p_body->get_kinematic_utilities()->m_shapes[i]); - if (!kin.is_active()) { - continue; - } + { /// Phase three - Recover + contact test with margin - // reset callback each function - iter_result_callabck.reset(); - - ghost->setCollisionShape(kin.shape); - ghost->setWorldTransform(body_safe_position); - ghost->getWorldTransform().getOrigin() += recovered_motion; - ghost->getWorldTransform().getOrigin() += kin.transform.getOrigin(); - ghost->getWorldTransform().getBasis() *= kin.transform.getBasis(); - - dynamicsWorld->contactTest(ghost, iter_result_callabck); - - if (iter_result_callabck.hasHit()) { - if (max_penetration > iter_result_callabck.m_penetration_distance) { - max_penetration = iter_result_callabck.m_penetration_distance; - shape_most_recovered = i; - // This is more penetrated - result_callabck.m_pointCollisionObject = iter_result_callabck.m_pointCollisionObject; - result_callabck.m_pointNormalWorld = iter_result_callabck.m_pointNormalWorld; - result_callabck.m_pointWorld = iter_result_callabck.m_pointWorld; - result_callabck.m_penetration_distance = iter_result_callabck.m_penetration_distance; - result_callabck.m_other_compound_shape_index = iter_result_callabck.m_other_compound_shape_index; - - recovered_motion += iter_result_callabck.m_recover_penetration; // Required to avoid all kind of penetration - } - } - } - } + RecoverResult recover_result; - hasHit = result_callabck.hasHit(); + hasPenetration = recover_from_penetration(p_body, body_safe_position, recovered_motion, &recover_result); if (r_result) { B_TO_G(recovered_motion + recover_initial_position, r_result->motion); - if (hasHit) { - - if (btCollisionObject::CO_RIGID_BODY != result_callabck.m_pointCollisionObject->getInternalType()) { - ERR_PRINT("The collision is not against a rigid body. Please check what's going on."); - goto EndExecution; - } - const btRigidBody *btRigid = static_cast(result_callabck.m_pointCollisionObject); + if (hasPenetration) { + const btRigidBody *btRigid = static_cast(recover_result.other_collision_object); CollisionObjectBullet *collisionObject = static_cast(btRigid->getUserPointer()); r_result->remainder = p_motion - r_result->motion; // is the remaining movements - B_TO_G(result_callabck.m_pointWorld, r_result->collision_point); - B_TO_G(result_callabck.m_pointNormalWorld, r_result->collision_normal); - B_TO_G(btRigid->getVelocityInLocalPoint(result_callabck.m_pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot + B_TO_G(recover_result.pointWorld, r_result->collision_point); + B_TO_G(recover_result.pointNormalWorld, r_result->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot r_result->collider = collisionObject->get_self(); r_result->collider_id = collisionObject->get_instance_id(); - r_result->collider_shape = result_callabck.m_other_compound_shape_index; - r_result->collision_local_shape = shape_most_recovered; + r_result->collider_shape = recover_result.other_compound_shape_index; + r_result->collision_local_shape = recover_result.local_shape_most_recovered; //{ /// Add manifold point to manage collisions // btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid); @@ -1079,85 +958,144 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f } } -EndExecution: - p_body->get_kinematic_utilities()->resetDefShape(); - return hasHit; + return hasPenetration; } -/// Note: It has a bug when two shapes touches something simultaneously the body is moved too much away -/// (I'm not fixing it because I don't use it). -bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_maxPenetrationDepth, btScalar p_depenetration_speed, btVector3 &out_recover_position) { +struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { +private: + const btCollisionObject *self_collision_object; + uint32_t collision_layer; + uint32_t collision_mask; - bool penetration = false; - btPairCachingGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject; +public: + Vector result_collision_objects; - for (int kinIndex = p_body->get_kinematic_utilities()->m_shapes.size() - 1; 0 <= kinIndex; --kinIndex) { - const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->m_shapes[kinIndex]); - if (!kin_shape.is_active()) { - continue; +public: + RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask) + : self_collision_object(p_self_collision_object), + collision_layer(p_collision_layer), + collision_mask(p_collision_mask) {} + + virtual ~RecoverPenetrationBroadPhaseCallback() {} + + virtual bool process(const btBroadphaseProxy *proxy) { + + btCollisionObject *co = static_cast(proxy->m_clientObject); + if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) { + if (self_collision_object != proxy->m_clientObject && GodotFilterCallback::test_collision_filters(collision_layer, collision_mask, proxy->m_collisionFilterGroup, proxy->m_collisionFilterMask)) { + result_collision_objects.push_back(co); + return true; + } } + return false; + } + + void reset() { + result_collision_objects.empty(); + } +}; + +bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btVector3 &out_recover_position, RecoverResult *recover_result) { - btConvexShape *convexShape = kin_shape.shape; - btTransform shape_xform(kin_shape.transform); + RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask()); - // from local to world - shape_xform.getOrigin() += p_from.getOrigin(); - shape_xform.getBasis() *= p_from.getBasis(); + btTransform body_shape_position; + btTransform body_shape_position_recovered; - // Apply last recovery to avoid doubling the recovering - shape_xform.getOrigin() += out_recover_position; + // Broad phase support + btVector3 minAabb, maxAabb; + + // GJK support + btGjkPairDetector::ClosestPointInput gjk_input; - ghost->setCollisionShape(convexShape); - ghost->setWorldTransform(shape_xform); + bool penetration = false; - btVector3 minAabb, maxAabb; - convexShape->getAabb(shape_xform, minAabb, maxAabb); - dynamicsWorld->getBroadphase()->setAabb(ghost->getBroadphaseHandle(), - minAabb, - maxAabb, - dynamicsWorld->getDispatcher()); + // For each shape + for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { - dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs(ghost->getOverlappingPairCache(), dynamicsWorld->getDispatchInfo(), dynamicsWorld->getDispatcher()); + recover_broad_result.reset(); - for (int i = 0; i < ghost->getOverlappingPairCache()->getNumOverlappingPairs(); ++i) { - p_body->get_kinematic_utilities()->m_manifoldArray.resize(0); + const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]); + if (!kin_shape.is_active()) { + continue; + } - btBroadphasePair *collisionPair = &ghost->getOverlappingPairCache()->getOverlappingPairArray()[i]; + body_shape_position = p_body_position * kin_shape.transform; + body_shape_position_recovered = body_shape_position; + body_shape_position_recovered.getOrigin() += out_recover_position; - btCollisionObject *obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject *obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb); + dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result); - if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) + for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) { + btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i]; + if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) continue; - // This is not required since the dispatched does all the job - //if (!needsCollision(obj0, obj1)) - // continue; + if (otherObject->getCollisionShape()->isCompound()) { /// Execute GJK test against all shapes - if (collisionPair->m_algorithm) - collisionPair->m_algorithm->getAllContactManifolds(p_body->get_kinematic_utilities()->m_manifoldArray); + // Each convex shape + btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); + for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { - for (int j = 0; j < p_body->get_kinematic_utilities()->m_manifoldArray.size(); ++j) { + if (!cs->getChildShape(x)->isConvex()) + continue; - btPersistentManifold *manifold = p_body->get_kinematic_utilities()->m_manifoldArray[j]; - btScalar directionSign = manifold->getBody0() == ghost ? btScalar(-1.0) : btScalar(1.0); - for (int p = 0; p < manifold->getNumContacts(); ++p) { - const btManifoldPoint &pt = manifold->getContactPoint(p); + // Initialize GJK input + gjk_input.m_transformA = body_shape_position; + gjk_input.m_transformA.getOrigin() += out_recover_position; + gjk_input.m_transformB = otherObject->getWorldTransform() * cs->getChildTransform(x); - btScalar dist = pt.getDistance(); - if (dist < -p_maxPenetrationDepth) { + // Perform GJK test + btPointCollector result; + btGjkPairDetector gjk_pair_detector(kin_shape.shape, static_cast(cs->getChildShape(x)), gjk_simplex_solver, gjk_epa_pen_solver); + gjk_pair_detector.getClosestPoints(gjk_input, result, 0); + if (0 > result.m_distance) { + // Has penetration + out_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1); penetration = true; - out_recover_position += pt.m_normalWorldOnB * directionSign * (dist + p_maxPenetrationDepth) * p_depenetration_speed; - //print_line("penetrate distance: " + rtos(dist)); + + if (recover_result) { + + recover_result->hasPenetration = true; + recover_result->other_collision_object = otherObject; + recover_result->other_compound_shape_index = x; + recover_result->penetration_distance = result.m_distance; + recover_result->pointNormalWorld = result.m_normalOnBInWorld; + recover_result->pointWorld = result.m_pointInWorld; + } + } + } + + } else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape + + // Initialize GJK input + gjk_input.m_transformA = body_shape_position; + gjk_input.m_transformA.getOrigin() += out_recover_position; + gjk_input.m_transformB = otherObject->getWorldTransform(); + + // Perform GJK test + btPointCollector result; + btGjkPairDetector gjk_pair_detector(kin_shape.shape, static_cast(otherObject->getCollisionShape()), gjk_simplex_solver, gjk_epa_pen_solver); + gjk_pair_detector.getClosestPoints(gjk_input, result, 0); + if (0 > result.m_distance) { + // Has penetration + out_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1); + penetration = true; + + if (recover_result) { + + recover_result->hasPenetration = true; + recover_result->other_collision_object = otherObject; + recover_result->other_compound_shape_index = 0; + recover_result->penetration_distance = result.m_distance; + recover_result->pointNormalWorld = result.m_normalOnBInWorld; + recover_result->pointWorld = result.m_pointInWorld; } - //else { - // print_line("touching distance: " + rtos(dist)); - //} } } } } - p_body->get_kinematic_utilities()->resetDefShape(); return penetration; } diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index cbbfdac1d7d6..c0acef4f869f 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -59,6 +59,7 @@ class CollisionObjectBullet; class RigidBodyBullet; class SpaceBullet; class SoftBodyBullet; +class btGjkEpaPenetrationDepthSolver; class BulletPhysicsDirectSpaceState : public PhysicsDirectSpaceState { GDCLASS(BulletPhysicsDirectSpaceState, PhysicsDirectSpaceState) @@ -93,6 +94,9 @@ class SpaceBullet : public RIDBullet { GodotFilterCallback *godotFilterCallback; btSoftBodyWorldInfo *soft_body_world_info; + btGjkEpaPenetrationDepthSolver *gjk_epa_pen_solver; + btVoronoiSimplexSolver *gjk_simplex_solver; + BulletPhysicsDirectSpaceState *direct_access; Vector3 gravityDirection; real_t gravityMagnitude; @@ -163,7 +167,7 @@ class SpaceBullet : public RIDBullet { void update_gravity(); - bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result); + bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, PhysicsServer::MotionResult *r_result); private: void create_empty_world(bool p_create_soft_world); @@ -171,6 +175,19 @@ class SpaceBullet : public RIDBullet { void check_ghost_overlaps(); void check_body_collision(); - bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_maxPenetrationDepth, btScalar p_depenetration_speed, btVector3 &out_recover_position); + struct RecoverResult { + bool hasPenetration; + btVector3 pointNormalWorld; + btVector3 pointWorld; + btScalar penetration_distance; // Negative is penetration + int other_compound_shape_index; + const btCollisionObject *other_collision_object; + int local_shape_most_recovered; + + RecoverResult() + : hasPenetration(false) {} + }; + + bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btVector3 &out_recover_position, RecoverResult *recover_result = NULL); }; #endif diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 4c661e6a8882..df137f1908e3 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -938,7 +938,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, Collision &r_colli Transform gt = get_global_transform(); PhysicsServer::MotionResult result; - bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, margin, &result); + bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, &result); if (colliding) { r_collision.collider_metadata = result.collider_metadata; @@ -1041,12 +1041,13 @@ bool KinematicBody::test_move(const Transform &p_from, const Vector3 &p_motion) ERR_FAIL_COND_V(!is_inside_tree(), false); - return PhysicsServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, margin); + return PhysicsServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion); } void KinematicBody::set_safe_margin(float p_margin) { margin = p_margin; + PhysicsServer::get_singleton()->body_set_kinematic_safe_margin(get_rid(), margin); } float KinematicBody::get_safe_margin() const { diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 6ced004118c5..f8cd6ca8584c 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -736,6 +736,10 @@ void BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_m } } +void BodySW::set_kinematic_margin(real_t p_margin) { + kinematic_safe_margin = p_margin; +} + BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { @@ -743,6 +747,7 @@ BodySW::BodySW() active = true; mass = 1; + kinematic_safe_margin = 0.01; //_inv_inertia=Transform(); _inv_mass = 1; bounce = 0; diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 782bf14a4b3d..fc64ca5817e6 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -55,6 +55,7 @@ class BodySW : public CollisionObjectSW { PhysicsServer::BodyAxisLock axis_lock; + real_t kinematic_safe_margin; real_t _inv_mass; Vector3 _inv_inertia; // Relative to the principal axes of inertia @@ -149,6 +150,9 @@ class BodySW : public CollisionObjectSW { public: void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); + void set_kinematic_margin(real_t p_margin); + _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } + _FORCE_INLINE_ void add_area(AreaSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index 5ba935d47c16..ce63d8461758 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -695,6 +695,19 @@ real_t PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const return body->get_param(p_param); }; +void PhysicsServerSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->set_kinematic_margin(p_margin); +} + +real_t PhysicsServerSW::body_get_kinematic_safe_margin(RID p_body) const { + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_kinematic_margin(); +} + void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { BodySW *body = body_owner.get(p_body); @@ -888,7 +901,7 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const { return body->is_ray_pickable(); } -bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) { +bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); @@ -897,7 +910,7 @@ bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, cons _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result); + return body->get_space()->test_body_motion(body, p_from, p_motion, body->get_kinematic_margin(), r_result); } PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) { diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h index f9eb8fa454e5..fa754a1c8fef 100644 --- a/servers/physics/physics_server_sw.h +++ b/servers/physics/physics_server_sw.h @@ -187,6 +187,9 @@ class PhysicsServerSW : public PhysicsServer { virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value); virtual real_t body_get_param(RID p_body, BodyParameter p_param) const; + virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin); + virtual real_t body_get_kinematic_safe_margin(RID p_body) const; + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant); virtual Variant body_get_state(RID p_body, BodyState p_state) const; @@ -221,7 +224,7 @@ class PhysicsServerSW : public PhysicsServer { virtual void body_set_ray_pickable(RID p_body, bool p_enable); virtual bool body_is_ray_pickable(RID p_body) const; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL); + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result = NULL); // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body); diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index a3efe76e13f7..76fb5bc46b4b 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -481,6 +481,9 @@ void PhysicsServer::_bind_methods() { ClassDB::bind_method(D_METHOD("body_set_param", "body", "param", "value"), &PhysicsServer::body_set_param); ClassDB::bind_method(D_METHOD("body_get_param", "body", "param"), &PhysicsServer::body_get_param); + ClassDB::bind_method(D_METHOD("body_set_kinematic_safe_margin", "body", "margin"), &PhysicsServer::body_set_kinematic_safe_margin); + ClassDB::bind_method(D_METHOD("body_get_kinematic_safe_margin", "body"), &PhysicsServer::body_get_kinematic_safe_margin); + ClassDB::bind_method(D_METHOD("body_set_state", "body", "state", "value"), &PhysicsServer::body_set_state); ClassDB::bind_method(D_METHOD("body_get_state", "body", "state"), &PhysicsServer::body_get_state); diff --git a/servers/physics_server.h b/servers/physics_server.h index 7cccd9b4cb88..64c67eae2a15 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -411,6 +411,9 @@ class PhysicsServer : public Object { virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value) = 0; virtual float body_get_param(RID p_body, BodyParameter p_param) const = 0; + virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) = 0; + virtual real_t body_get_kinematic_safe_margin(RID p_body) const = 0; + //state enum BodyState { BODY_STATE_TRANSFORM, @@ -482,7 +485,7 @@ class PhysicsServer : public Object { Variant collider_metadata; }; - virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) = 0; + virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result = NULL) = 0; /* JOINT API */