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.cpp242
1 files changed, 143 insertions, 99 deletions
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 1659664ff9..d0515e7c97 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -63,9 +63,9 @@ BulletPhysicsDirectSpaceState::BulletPhysicsDirectSpaceState(SpaceBullet *p_spac
space(p_space) {}
int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, 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)
+ if (p_result_max <= 0) {
return 0;
+ }
btVector3 bt_point;
G_TO_B(p_point, bt_point);
@@ -86,7 +86,6 @@ int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, Shape
}
bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) {
-
btVector3 btVec_from;
btVector3 btVec_to;
@@ -119,14 +118,16 @@ 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) {
- if (p_result_max <= 0)
+ if (p_result_max <= 0) {
return 0;
+ }
ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->getornull(p_shape);
+ ERR_FAIL_COND_V(!shape, 0);
btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin);
if (!btShape->isConvex()) {
- bulletdelete(btShape);
+ shape->destroy_bt_shape(btShape);
ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
return 0;
}
@@ -146,25 +147,28 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
btQuery.m_closestDistanceThreshold = 0;
space->dynamicsWorld->contactTest(&collision_object, btQuery);
- bulletdelete(btConvex);
+ shape->destroy_bt_shape(btShape);
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) {
+ 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);
+ ERR_FAIL_COND_V(!shape, false);
btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
if (!btShape->isConvex()) {
- bulletdelete(btShape);
+ shape->destroy_bt_shape(btShape);
ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
return false;
}
btConvexShape *bt_convex_shape = static_cast<btConvexShape *>(btShape);
- btVector3 bt_motion;
- G_TO_B(p_motion, bt_motion);
-
btTransform bt_xform_from;
G_TO_B(p_xform, bt_xform_from);
UNSCALE_BT_BASIS(bt_xform_from);
@@ -172,15 +176,17 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
btTransform bt_xform_to(bt_xform_from);
bt_xform_to.getOrigin() += bt_motion;
+ if ((bt_xform_to.getOrigin() - bt_xform_from.getOrigin()).fuzzyZero()) {
+ shape->destroy_bt_shape(btShape);
+ return false;
+ }
+
GodotClosestConvexResultCallback btResult(bt_xform_from.getOrigin(), bt_xform_to.getOrigin(), &p_exclude, p_collide_with_bodies, p_collide_with_areas);
btResult.m_collisionFilterGroup = 0;
btResult.m_collisionFilterMask = p_collision_mask;
space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, space->dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
- r_closest_unsafe = 1.0;
- r_closest_safe = 1.0;
-
if (btResult.hasHit()) {
const btScalar l = bt_motion.length();
r_closest_unsafe = btResult.m_closestHitFraction;
@@ -196,24 +202,29 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
r_info->collider_id = collision_object->get_instance_id();
r_info->shape = btResult.m_shapeId;
}
+ } else {
+ r_closest_safe = 1.0f;
+ r_closest_unsafe = 1.0f;
}
- bulletdelete(bt_convex_shape);
+ shape->destroy_bt_shape(btShape);
return true; // Mean success
}
/// 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) {
- if (p_result_max <= 0)
- return 0;
+ if (p_result_max <= 0) {
+ return false;
+ }
ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->getornull(p_shape);
+ ERR_FAIL_COND_V(!shape, false);
btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
if (!btShape->isConvex()) {
- bulletdelete(btShape);
+ shape->destroy_bt_shape(btShape);
ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
- return 0;
+ return false;
}
btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
@@ -232,20 +243,20 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
space->dynamicsWorld->contactTest(&collision_object, btQuery);
r_result_count = btQuery.m_count;
- bulletdelete(btConvex);
+ shape->destroy_bt_shape(btShape);
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);
+ ERR_FAIL_COND_V(!shape, false);
btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
if (!btShape->isConvex()) {
- bulletdelete(btShape);
+ shape->destroy_bt_shape(btShape);
ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
- return 0;
+ return false;
}
btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
@@ -263,7 +274,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
btQuery.m_closestDistanceThreshold = 0;
space->dynamicsWorld->contactTest(&collision_object, btQuery);
- bulletdelete(btConvex);
+ shape->destroy_bt_shape(btShape);
if (btQuery.m_collided) {
if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) {
@@ -276,8 +287,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
}
Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
-
- RigidCollisionObjectBullet *rigid_object = space->get_physics_server()->get_rigid_collisin_object(p_object);
+ RigidCollisionObjectBullet *rigid_object = space->get_physics_server()->get_rigid_collision_object(p_object);
ERR_FAIL_COND_V(!rigid_object, Vector3());
btVector3 out_closest_point(0, 0, 0);
@@ -320,31 +330,16 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_
}
if (shapes_found) {
-
Vector3 out;
B_TO_G(out_closest_point, out);
return out;
} else {
-
// no shapes found, use distance to origin.
return rigid_object->get_transform().get_origin();
}
}
-SpaceBullet::SpaceBullet() :
- broadphase(nullptr),
- collisionConfiguration(nullptr),
- dispatcher(nullptr),
- solver(nullptr),
- dynamicsWorld(nullptr),
- soft_body_world_info(nullptr),
- ghostPairCallback(nullptr),
- godotFilterCallback(nullptr),
- gravityDirection(0, -1, 0),
- gravityMagnitude(10),
- contactDebugCount(0),
- delta_time(0.) {
-
+SpaceBullet::SpaceBullet() {
create_empty_world(GLOBAL_DEF("physics/3d/active_soft_world", true));
direct_access = memnew(BulletPhysicsDirectSpaceState(this));
}
@@ -354,14 +349,46 @@ SpaceBullet::~SpaceBullet() {
destroy_world();
}
+void SpaceBullet::add_to_pre_flush_queue(CollisionObjectBullet *p_co) {
+ if (p_co->is_in_flush_queue == false) {
+ p_co->is_in_flush_queue = true;
+ queue_pre_flush.push_back(p_co);
+ }
+}
+
+void SpaceBullet::add_to_flush_queue(CollisionObjectBullet *p_co) {
+ if (p_co->is_in_flush_queue == false) {
+ p_co->is_in_flush_queue = true;
+ queue_flush.push_back(p_co);
+ }
+}
+
+void SpaceBullet::remove_from_any_queue(CollisionObjectBullet *p_co) {
+ if (p_co->is_in_flush_queue) {
+ p_co->is_in_flush_queue = false;
+ queue_pre_flush.erase(p_co);
+ queue_flush.erase(p_co);
+ }
+}
+
void SpaceBullet::flush_queries() {
- const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray();
- for (int i = colObjArray.size() - 1; 0 <= i; --i) {
- static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->dispatch_callbacks();
+ for (uint32_t i = 0; i < queue_pre_flush.size(); i += 1) {
+ queue_pre_flush[i]->dispatch_callbacks();
+ queue_pre_flush[i]->is_in_flush_queue = false;
+ }
+ for (uint32_t i = 0; i < queue_flush.size(); i += 1) {
+ queue_flush[i]->dispatch_callbacks();
+ queue_flush[i]->is_in_flush_queue = false;
}
+ queue_pre_flush.clear();
+ queue_flush.clear();
}
void SpaceBullet::step(real_t p_delta_time) {
+ for (uint32_t i = 0; i < collision_objects.size(); i += 1) {
+ collision_objects[i]->pre_process();
+ }
+
delta_time = p_delta_time;
dynamicsWorld->stepSimulation(p_delta_time, 0, 0);
}
@@ -379,8 +406,11 @@ void SpaceBullet::set_param(PhysicsServer3D::AreaParameter p_param, const Varian
update_gravity();
break;
case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
+ linear_damp = p_value;
+ break;
case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
- break; // No damp
+ angular_damp = p_value;
+ break;
case PhysicsServer3D::AREA_PARAM_PRIORITY:
// Priority is always 0, the lower
break;
@@ -401,8 +431,9 @@ Variant SpaceBullet::get_param(PhysicsServer3D::AreaParameter p_param) {
case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
return gravityDirection;
case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
+ return linear_damp;
case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
- return 0; // No damp
+ return angular_damp;
case PhysicsServer3D::AREA_PARAM_PRIORITY:
return 0; // Priority is always 0, the lower
case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
@@ -450,16 +481,30 @@ real_t SpaceBullet::get_param(PhysicsServer3D::SpaceParameter p_param) {
}
void SpaceBullet::add_area(AreaBullet *p_area) {
+#ifdef TOOLS_ENABLED
+ // This never happen, and there is no way for the user to trigger it.
+ // If in future a bug is introduced into this bullet integration and this
+ // function is called twice, the crash will notify the developer that will
+ // fix it even before do the eventual PR.
+ CRASH_COND(p_area->is_in_world);
+#endif
areas.push_back(p_area);
dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
+ p_area->is_in_world = true;
}
void SpaceBullet::remove_area(AreaBullet *p_area) {
- areas.erase(p_area);
- dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
+ if (p_area->is_in_world) {
+ areas.erase(p_area);
+ dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
+ p_area->is_in_world = false;
+ }
}
void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
+ if (p_area->is_in_world == false) {
+ return;
+ }
btGhostObject *ghost_object = p_area->get_bt_ghost();
btBroadphaseProxy *ghost_proxy = ghost_object->getBroadphaseHandle();
@@ -469,24 +514,47 @@ void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
dynamicsWorld->refreshBroadphaseProxy(ghost_object);
}
+void SpaceBullet::register_collision_object(CollisionObjectBullet *p_object) {
+ collision_objects.push_back(p_object);
+}
+
+void SpaceBullet::unregister_collision_object(CollisionObjectBullet *p_object) {
+ remove_from_any_queue(p_object);
+ collision_objects.erase(p_object);
+}
+
void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
+#ifdef TOOLS_ENABLED
+ // This never happen, and there is no way for the user to trigger it.
+ // If in future a bug is introduced into this bullet integration and this
+ // function is called twice, the crash will notify the developer that will
+ // fix it even before do the eventual PR.
+ CRASH_COND(p_body->is_in_world);
+#endif
if (p_body->is_static()) {
dynamicsWorld->addCollisionObject(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
} else {
dynamicsWorld->addRigidBody(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
p_body->scratch_space_override_modificator();
}
+ p_body->is_in_world = true;
}
void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
- if (p_body->is_static()) {
- dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
- } else {
- dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
+ if (p_body->is_in_world) {
+ if (p_body->is_static()) {
+ dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
+ } else {
+ dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
+ }
+ p_body->is_in_world = false;
}
}
void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) {
+ if (p_body->is_in_world == false) {
+ return;
+ }
btRigidBody *rigid_body = p_body->get_bt_rigid_body();
btBroadphaseProxy *body_proxy = rigid_body->getBroadphaseProxy();
@@ -543,12 +611,7 @@ void SpaceBullet::remove_all_collision_objects() {
}
}
-void onBulletPreTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
- static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo())->flush_queries();
-}
-
void onBulletTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
-
const btCollisionObjectArray &colObjArray = p_dynamicsWorld->getCollisionObjectArray();
// Notify all Collision objects the collision checker is started
@@ -570,17 +633,14 @@ BulletPhysicsDirectSpaceState *SpaceBullet::get_direct_state() {
}
btScalar calculateGodotCombinedRestitution(const btCollisionObject *body0, const btCollisionObject *body1) {
-
return CLAMP(body0->getRestitution() + body1->getRestitution(), 0, 1);
}
btScalar calculateGodotCombinedFriction(const btCollisionObject *body0, const btCollisionObject *body1) {
-
return ABS(MIN(body0->getFriction(), body1->getFriction()));
}
void SpaceBullet::create_empty_world(bool p_create_soft_world) {
-
gjk_epa_pen_solver = bulletnew(btGjkEpaPenetrationDepthSolver);
gjk_simplex_solver = bulletnew(btVoronoiSimplexSolver);
@@ -618,7 +678,6 @@ void SpaceBullet::create_empty_world(bool p_create_soft_world) {
dynamicsWorld->setWorldUserInfo(this);
- dynamicsWorld->setInternalTickCallback(onBulletPreTickCallback, this, true);
dynamicsWorld->setInternalTickCallback(onBulletTickCallback, this, false);
dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(ghostPairCallback); // Setup ghost check
dynamicsWorld->getPairCache()->setOverlapFilterCallback(godotFilterCallback);
@@ -633,7 +692,6 @@ void SpaceBullet::create_empty_world(bool p_create_soft_world) {
}
void SpaceBullet::destroy_world() {
-
/// The world elements (like: Collision Objects, Constraints, Shapes) are managed by godot
dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(nullptr);
@@ -657,7 +715,6 @@ void SpaceBullet::destroy_world() {
}
void SpaceBullet::check_ghost_overlaps() {
-
/// Algorithm support variables
btCollisionShape *other_body_shape;
btConvexShape *area_shape;
@@ -671,12 +728,13 @@ void SpaceBullet::check_ghost_overlaps() {
btVector3 area_scale(area->get_bt_body_scale());
- if (!area->is_monitoring())
+ if (!area->is_monitoring()) {
continue;
+ }
/// 1. Reset all states
for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) {
- AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects.write[i];
+ AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects[i];
// This check prevent the overwrite of ENTER state
// if this function is called more times before dispatchCallbacks
if (otherObj.state != AreaBullet::OVERLAP_STATE_ENTER) {
@@ -690,7 +748,6 @@ void SpaceBullet::check_ghost_overlaps() {
// For each overlapping
for (i = ghostOverlaps.size() - 1; 0 <= i; --i) {
-
bool hasOverlap = false;
btCollisionObject *overlapped_bt_co = ghostOverlaps[i];
RigidCollisionObjectBullet *otherObject = static_cast<RigidCollisionObjectBullet *>(overlapped_bt_co->getUserPointer());
@@ -702,15 +759,18 @@ void SpaceBullet::check_ghost_overlaps() {
}
if (overlapped_bt_co->getUserIndex() == CollisionObjectBullet::TYPE_AREA) {
- if (!static_cast<AreaBullet *>(overlapped_bt_co->getUserPointer())->is_monitorable())
+ if (!static_cast<AreaBullet *>(overlapped_bt_co->getUserPointer())->is_monitorable()) {
continue;
- } else if (overlapped_bt_co->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY)
+ }
+ } else if (overlapped_bt_co->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY) {
continue;
+ }
// For each area shape
for (y = area->get_shape_count() - 1; 0 <= y; --y) {
- if (!area->get_bt_shape(y)->isConvex())
+ if (!area->get_bt_shape(y)->isConvex()) {
continue;
+ }
btTransform area_shape_treansform(area->get_bt_shape_transform(y));
area_shape_treansform.getOrigin() *= area_scale;
@@ -723,7 +783,6 @@ void SpaceBullet::check_ghost_overlaps() {
// For each other object shape
for (z = otherObject->get_shape_count() - 1; 0 <= z; --z) {
-
other_body_shape = static_cast<btCollisionShape *>(otherObject->get_bt_shape(z));
btTransform other_shape_transform(otherObject->get_bt_shape_transform(z));
@@ -734,7 +793,6 @@ void SpaceBullet::check_ghost_overlaps() {
other_shape_transform;
if (other_body_shape->isConvex()) {
-
btPointCollector result;
btGjkPairDetector gjk_pair_detector(
area_shape,
@@ -749,14 +807,14 @@ void SpaceBullet::check_ghost_overlaps() {
}
} else {
-
btCollisionObjectWrapper obA(nullptr, area_shape, area->get_bt_ghost(), gjk_input.m_transformA, -1, y);
btCollisionObjectWrapper obB(nullptr, other_body_shape, otherObject->get_bt_collision_object(), gjk_input.m_transformB, -1, z);
btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, nullptr, BT_CONTACT_POINT_ALGORITHMS);
- if (!algorithm)
+ if (!algorithm) {
continue;
+ }
GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB);
algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult);
@@ -774,8 +832,9 @@ void SpaceBullet::check_ghost_overlaps() {
} // ~For each area shape
collision_found:
- if (!hasOverlap)
+ if (!hasOverlap) {
continue;
+ }
indexOverlap = area->find_overlapping_object(otherObject);
if (-1 == indexOverlap) {
@@ -832,7 +891,6 @@ void SpaceBullet::check_body_collision() {
pt.getDistance() <= 0.0 ||
bodyA->was_colliding(bodyB) ||
bodyB->was_colliding(bodyA)) {
-
Vector3 collisionWorldPosition;
Vector3 collisionLocalPosition;
Vector3 normalOnB;
@@ -892,7 +950,6 @@ 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) {
-
#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
@@ -957,7 +1014,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
motionVec->end();
#endif
- for (int shIndex = 0; shIndex < shape_count && !motion.fuzzyZero(); ++shIndex) {
+ for (int shIndex = 0; shIndex < shape_count; ++shIndex) {
if (p_body->is_shape_disabled(shIndex)) {
continue;
}
@@ -979,6 +1036,11 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
btTransform shape_world_to(shape_world_from);
shape_world_to.getOrigin() += motion;
+ if ((shape_world_to.getOrigin() - shape_world_from.getOrigin()).fuzzyZero()) {
+ motion = btVector3(0, 0, 0);
+ break;
+ }
+
GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia);
btResult.m_collisionFilterGroup = p_body->get_collision_layer();
btResult.m_collisionFilterMask = p_body->get_collision_mask();
@@ -1009,7 +1071,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
B_TO_G(motion + initial_recover_motion + __rec, r_result->motion);
if (has_penetration) {
-
const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
@@ -1043,7 +1104,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
}
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) {
-
btTransform body_transform;
G_TO_B(p_transform, body_transform);
UNSCALE_BT_BASIS(body_transform);
@@ -1108,14 +1168,12 @@ public:
self_collision_object(p_self_collision_object),
collision_layer(p_collision_layer),
collision_mask(p_collision_mask) {
-
bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max);
}
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)) {
@@ -1159,13 +1217,11 @@ 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) {
-
// Calculate the cumulative AABB of all shapes of the kinematic body
btVector3 aabb_min, aabb_max;
bool shapes_found = false;
for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-
const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
if (!kin_shape.is_active()) {
continue;
@@ -1210,7 +1266,6 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
// Perform narrowphase per shape
for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-
const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
if (!kin_shape.is_active()) {
continue;
@@ -1229,8 +1284,9 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
otherObject->activate(); // Force activation of hitten rigid, soft body
continue;
- } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()))
+ } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) {
continue;
+ }
if (otherObject->getCollisionShape()->isCompound()) {
const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
@@ -1239,23 +1295,19 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
if (cs->getChildShape(shape_idx)->isConvex()) {
if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(shape_idx)), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-
penetration = true;
}
} else {
if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-
penetration = true;
}
}
} else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape
if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-
penetration = true;
}
} else {
if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-
penetration = true;
}
}
@@ -1266,7 +1318,6 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
}
bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
-
// Initialize GJK input
btGjkPairDetector::ClosestPointInput gjk_input;
gjk_input.m_transformA = p_transformA;
@@ -1297,7 +1348,6 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
}
bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
-
/// Contact test
btTransform tA(p_transformA);
@@ -1334,7 +1384,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
}
int SpaceBullet::add_separation_result(PhysicsServer3D::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const {
-
// optimize results (ignore non-colliding)
if (p_recover_result.penetration_distance < 0.0) {
const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
@@ -1356,13 +1405,11 @@ int SpaceBullet::add_separation_result(PhysicsServer3D::SeparationResult *r_resu
}
int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer3D::SeparationResult *r_results) {
-
// Calculate the cumulative AABB of all shapes of the kinematic body
btVector3 aabb_min, aabb_max;
bool shapes_found = false;
for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-
const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
if (!kin_shape.is_active()) {
continue;
@@ -1406,7 +1453,6 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
// Perform narrowphase per shape
for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-
if (ray_count >= p_result_max) {
break;
}
@@ -1428,8 +1474,9 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
otherObject->activate(); // Force activation of hitten rigid, soft body
continue;
- } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()))
+ } else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) {
continue;
+ }
if (otherObject->getCollisionShape()->isCompound()) {
const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
@@ -1438,14 +1485,11 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
RecoverResult recover_result;
if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
-
ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
}
} else {
-
RecoverResult recover_result;
if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
-
ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
}
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-04-16 01:58:06 +0000