summaryrefslogtreecommitdiff
path: root/modules/bullet/space_bullet.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'modules/bullet/space_bullet.cpp')
-rw-r--r--modules/bullet/space_bullet.cpp121
1 files changed, 85 insertions, 36 deletions
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index f4bca8eef5..485fb8d3bc 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -5,8 +5,8 @@
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
-/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -81,7 +81,7 @@ int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, Shape
btResult.m_collisionFilterMask = p_collision_mask;
space->dynamicsWorld->contactTest(&collision_object_point, btResult);
- // The results is already populated by GodotAllConvexResultCallback
+ // The results are already populated by GodotAllConvexResultCallback
return btResult.m_count;
}
@@ -117,12 +117,12 @@ bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const V
}
}
-int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (p_result_max <= 0) {
return 0;
}
- ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->getornull(p_shape);
+ ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin);
@@ -152,13 +152,13 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
return btQuery.m_count;
}
-bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &r_closest_safe, float &r_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) {
+bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &r_closest_safe, real_t &r_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) {
r_closest_safe = 0.0f;
r_closest_unsafe = 0.0f;
btVector3 bt_motion;
G_TO_B(p_motion, bt_motion);
- ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->getornull(p_shape);
+ ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false);
btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
@@ -177,8 +177,10 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
bt_xform_to.getOrigin() += bt_motion;
if ((bt_xform_to.getOrigin() - bt_xform_from.getOrigin()).fuzzyZero()) {
+ r_closest_safe = 1.0f;
+ r_closest_unsafe = 1.0f;
bulletdelete(btShape);
- return false;
+ return true;
}
GodotClosestConvexResultCallback btResult(bt_xform_from.getOrigin(), bt_xform_to.getOrigin(), &p_exclude, p_collide_with_bodies, p_collide_with_areas);
@@ -212,12 +214,12 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
}
/// Returns the list of contacts pairs in this order: Local contact, other body contact
-bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (p_result_max <= 0) {
return false;
}
- ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->getornull(p_shape);
+ ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false);
btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
@@ -248,8 +250,8 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
return btQuery.m_count;
}
-bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
- ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->getornull(p_shape);
+bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+ ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false);
btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
@@ -443,7 +445,7 @@ real_t SpaceBullet::get_param(PhysicsServer3D::SpaceParameter p_param) {
case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO:
case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
default:
- WARN_PRINT("The SpaceBullet doesn't support this get parameter (" + itos(p_param) + "), 0 is returned.");
+ WARN_PRINT("The SpaceBullet doesn't support this get parameter (" + itos(p_param) + "), 0 is returned.");
return 0.f;
}
}
@@ -477,16 +479,20 @@ void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
}
}
-void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
+void SpaceBullet::remove_rigid_body_constraints(RigidBodyBullet *p_body) {
btRigidBody *btBody = p_body->get_bt_rigid_body();
int constraints = btBody->getNumConstraintRefs();
if (constraints > 0) {
- WARN_PRINT("A body connected to joints was removed. Ensure bodies are disconnected from joints before removing them.");
+ ERR_PRINT("A body connected to joints was removed.");
for (int i = 0; i < constraints; i++) {
dynamicsWorld->removeConstraint(btBody->getConstraintRef(i));
}
}
+}
+
+void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
+ btRigidBody *btBody = p_body->get_bt_rigid_body();
if (p_body->is_static()) {
dynamicsWorld->removeCollisionObject(btBody);
@@ -835,20 +841,32 @@ void SpaceBullet::check_body_collision() {
Vector3 collisionWorldPosition;
Vector3 collisionLocalPosition;
Vector3 normalOnB;
- float appliedImpulse = pt.m_appliedImpulse;
+ real_t appliedImpulse = pt.m_appliedImpulse;
B_TO_G(pt.m_normalWorldOnB, normalOnB);
+ // The pt.m_index only contains the shape index when more than one collision shape is used
+ // and only if the collision shape is not a concave collision shape.
+ // A value of -1 in pt.m_partId indicates the pt.m_index is a shape index.
+ int shape_index_a = 0;
+ if (bodyA->get_shape_count() > 1 && pt.m_partId0 == -1) {
+ shape_index_a = pt.m_index0;
+ }
+ int shape_index_b = 0;
+ if (bodyB->get_shape_count() > 1 && pt.m_partId1 == -1) {
+ shape_index_b = pt.m_index1;
+ }
+
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() - contactManifold->getBody1()->getWorldTransform().getOrigin(), collisionLocalPosition);
- bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, appliedImpulse, pt.m_index1, pt.m_index0);
+ bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, appliedImpulse, shape_index_b, shape_index_a);
}
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() - contactManifold->getBody0()->getWorldTransform().getOrigin(), collisionLocalPosition);
- bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, appliedImpulse * -1, pt.m_index0, pt.m_index1);
+ bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, appliedImpulse * -1, shape_index_a, shape_index_b);
}
#ifdef DEBUG_ENABLED
@@ -890,7 +908,7 @@ static Ref<StandardMaterial3D> red_mat;
static Ref<StandardMaterial3D> blue_mat;
#endif
-bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) {
+bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude) {
#if debug_test_motion
/// 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
@@ -927,10 +945,14 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
G_TO_B(p_from, body_transform);
UNSCALE_BT_BASIS(body_transform);
+ if (!p_body->get_kinematic_utilities()) {
+ p_body->init_kinematic_utilities();
+ }
+
btVector3 initial_recover_motion(0, 0, 0);
{ /// Phase one - multi shapes depenetration using margin
for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
- if (!recover_from_penetration(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, initial_recover_motion)) {
+ if (!recover_from_penetration(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, initial_recover_motion, nullptr, p_exclude)) {
break;
}
}
@@ -940,6 +962,9 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
btVector3 motion;
G_TO_B(p_motion, motion);
+ real_t total_length = motion.length();
+ real_t unsafe_fraction = 1.0;
+ real_t safe_fraction = 1.0;
{
// Phase two - sweep test, from a secure position without margin
@@ -982,13 +1007,22 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
break;
}
- GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia);
+ GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia, &p_exclude);
btResult.m_collisionFilterGroup = p_body->get_collision_layer();
btResult.m_collisionFilterMask = p_body->get_collision_mask();
dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
if (btResult.hasHit()) {
+ if (total_length > CMP_EPSILON) {
+ real_t hit_fraction = btResult.m_closestHitFraction * motion.length() / total_length;
+ if (hit_fraction < unsafe_fraction) {
+ unsafe_fraction = hit_fraction;
+ real_t margin = p_body->get_kinematic_utilities()->safe_margin;
+ safe_fraction = MAX(hit_fraction - (1 - ((total_length - margin) / total_length)), 0);
+ }
+ }
+
/// 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 penetration respect the previous shape
motion *= btResult.m_closestHitFraction;
@@ -1005,7 +1039,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
btVector3 __rec(0, 0, 0);
RecoverResult r_recover_result;
- has_penetration = recover_from_penetration(p_body, body_transform, 1, p_infinite_inertia, __rec, &r_recover_result);
+ has_penetration = recover_from_penetration(p_body, body_transform, 1, p_infinite_inertia, __rec, &r_recover_result, p_exclude);
// Parse results
if (r_result) {
@@ -1025,6 +1059,9 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
r_result->collider_id = collisionObject->get_instance_id();
r_result->collider_shape = r_recover_result.other_compound_shape_index;
r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
+ r_result->collision_depth = Math::abs(r_recover_result.penetration_distance);
+ r_result->collision_safe_fraction = safe_fraction;
+ r_result->collision_unsafe_fraction = unsafe_fraction;
#if debug_test_motion
Vector3 sup_line2;
@@ -1044,11 +1081,15 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
return has_penetration;
}
-int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, float p_margin) {
+int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) {
btTransform body_transform;
G_TO_B(p_transform, body_transform);
UNSCALE_BT_BASIS(body_transform);
+ if (!p_body->get_kinematic_utilities()) {
+ p_body->init_kinematic_utilities();
+ }
+
btVector3 recover_motion(0, 0, 0);
int rays_found = 0;
@@ -1074,13 +1115,12 @@ private:
btDbvtVolume bounds;
const btCollisionObject *self_collision_object;
- uint32_t collision_layer;
- uint32_t collision_mask;
+ uint32_t collision_layer = 0;
struct CompoundLeafCallback : btDbvt::ICollide {
private:
- RecoverPenetrationBroadPhaseCallback *parent_callback;
- btCollisionObject *collision_object;
+ RecoverPenetrationBroadPhaseCallback *parent_callback = nullptr;
+ btCollisionObject *collision_object = nullptr;
public:
CompoundLeafCallback(RecoverPenetrationBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) :
@@ -1098,17 +1138,16 @@ private:
public:
struct BroadphaseResult {
- btCollisionObject *collision_object;
- int compound_child_index;
+ btCollisionObject *collision_object = nullptr;
+ int compound_child_index = 0;
};
Vector<BroadphaseResult> results;
public:
- RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask, btVector3 p_aabb_min, btVector3 p_aabb_max) :
+ RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, btVector3 p_aabb_min, btVector3 p_aabb_max) :
self_collision_object(p_self_collision_object),
- collision_layer(p_collision_layer),
- collision_mask(p_collision_mask) {
+ collision_layer(p_collision_layer) {
bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max);
}
@@ -1117,7 +1156,7 @@ public:
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)) {
+ if (self_collision_object != proxy->m_clientObject && (proxy->collision_layer & m_collisionFilterMask)) {
if (co->getCollisionShape()->isCompound()) {
const btCompoundShape *cs = static_cast<btCompoundShape *>(co->getCollisionShape());
@@ -1157,7 +1196,7 @@ public:
}
};
-bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
+bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result, const Set<RID> &p_exclude) {
// Calculate the cumulative AABB of all shapes of the kinematic body
btVector3 aabb_min, aabb_max;
bool shapes_found = false;
@@ -1200,7 +1239,7 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
}
// Perform broadphase test
- RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max);
+ RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), aabb_min, aabb_max);
dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result);
bool penetration = false;
@@ -1217,11 +1256,21 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
continue;
}
+ if (kin_shape.shape->getShapeType() == EMPTY_SHAPE_PROXYTYPE) {
+ continue;
+ }
+
btTransform shape_transform = p_body_position * kin_shape.transform;
shape_transform.getOrigin() += r_delta_recover_movement;
for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
+
+ CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(otherObject->getUserPointer());
+ if (p_exclude.has(gObj->get_self())) {
+ continue;
+ }
+
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
otherObject->activate(); // Force activation of hitten rigid, soft body
continue;
@@ -1387,7 +1436,7 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
}
// Perform broadphase test
- RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max);
+ RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), aabb_min, aabb_max);
dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result);
int ray_count = 0;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-08 22:16:02 +0000