summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--modules/bullet/bullet_physics_server.cpp26
-rw-r--r--modules/bullet/bullet_physics_server.h5
-rw-r--r--modules/bullet/collision_object_bullet.cpp2
-rw-r--r--modules/bullet/godot_result_callbacks.cpp47
-rw-r--r--modules/bullet/godot_result_callbacks.h37
-rw-r--r--modules/bullet/rigid_body_bullet.cpp46
-rw-r--r--modules/bullet/rigid_body_bullet.h10
-rw-r--r--modules/bullet/space_bullet.cpp380
-rw-r--r--modules/bullet/space_bullet.h21
-rw-r--r--scene/3d/physics_body.cpp5
-rw-r--r--servers/physics/body_sw.cpp5
-rw-r--r--servers/physics/body_sw.h4
-rw-r--r--servers/physics/physics_server_sw.cpp17
-rw-r--r--servers/physics/physics_server_sw.h5
-rw-r--r--servers/physics_server.cpp3
-rw-r--r--servers/physics_server.h5
16 files changed, 267 insertions, 351 deletions
diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index 2656074296..3fbbdf50b3 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 e66e8a5ac1..c1859ca149 100644
--- a/modules/bullet/bullet_physics_server.h
+++ b/modules/bullet/bullet_physics_server.h
@@ -209,6 +209,9 @@ public:
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 @@ public:
// 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 5739568d91..449d4322d4 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 409d12f080..bc60c9cb6b 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<btCollisionObject *>(proxy0->m_clientObject);
- CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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 d505bd0de1..68dff5b12a 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 @@ public:
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 5d0513db76..98ae82bc5f 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<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(m_owner->get_shapes_wrappers());
+ const Vector<CollisionObjectBullet::ShapeWrapper> &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<SphereShapeBullet *>(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<BoxShapeBullet *>(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<CapsuleShapeBullet *>(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<ConvexPolygonShapeBullet *>(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<RayShapeBullet *>(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 0cf3f9f605..ab3c3e58b2 100644
--- a/modules/bullet/rigid_body_bullet.h
+++ b/modules/bullet/rigid_body_bullet.h
@@ -162,17 +162,15 @@ public:
};
struct KinematicUtilities {
- RigidBodyBullet *m_owner;
- btScalar m_margin;
- btManifoldArray m_manifoldArray; ///keep track of the contact manifolds
- class btPairCachingGhostObject *m_ghostObject;
- Vector<KinematicShape> m_shapes;
+ RigidBodyBullet *owner;
+ btScalar safe_margin;
+ Vector<KinematicShape> 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 af7f511fab..9df01aee3e 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -50,9 +50,6 @@
#include "ustring.h"
#include <assert.h>
-// 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<RigidBodyBullet *>(obA->getUserPointer());
- RigidBodyBullet *bodyB = static_cast<RigidBodyBullet *>(obB->getUserPointer());
+ RigidBodyBullet *bodyA = static_cast<RigidBodyBullet *>(contactManifold->getBody0()->getUserPointer());
+ RigidBodyBullet *bodyB = static_cast<RigidBodyBullet *>(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<SpatialMaterial> red_mat;
@@ -825,10 +800,10 @@ static Ref<SpatialMaterial> 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<const btRigidBody *>(result_callabck.m_pointCollisionObject);
+ if (hasPenetration) {
+ const btRigidBody *btRigid = static_cast<const btRigidBody *>(recover_result.other_collision_object);
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(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<btCollisionObject *> 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<btCollisionObject *>(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<btCollisionObject *>(collisionPair->m_pProxy0->m_clientObject);
- btCollisionObject *obj1 = static_cast<btCollisionObject *>(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<btCompoundShape *>(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<const btConvexShape *>(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<const btConvexShape *>(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 cbbfdac1d7..c0acef4f86 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 @@ private:
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 @@ public:
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 @@ private:
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 4c661e6a88..df137f1908 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 6ced004118..f8cd6ca858 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 782bf14a4b..fc64ca5817 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 5ba935d47c..ce63d84617 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 f9eb8fa454..fa754a1c8f 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -187,6 +187,9 @@ public:
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 @@ public:
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 a3efe76e13..76fb5bc46b 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 7cccd9b4cb..64c67eae2a 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -411,6 +411,9 @@ public:
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 @@ public:
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 */
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-23 21:21:27 +0000