diff options
Diffstat (limited to 'modules/bullet/space_bullet.cpp')
| -rw-r--r-- | modules/bullet/space_bullet.cpp | 268 |
1 files changed, 152 insertions, 116 deletions
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 1659664ff9..a9a811c445 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 */ @@ -34,8 +34,8 @@ #include "bullet_types_converter.h" #include "bullet_utilities.h" #include "constraint_bullet.h" -#include "core/project_settings.h" -#include "core/ustring.h" +#include "core/config/project_settings.h" +#include "core/string/ustring.h" #include "godot_collision_configuration.h" #include "godot_collision_dispatcher.h" #include "rigid_body_bullet.h" @@ -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); @@ -81,12 +81,11 @@ 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; } 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; @@ -118,11 +117,13 @@ 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) +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); + ERR_FAIL_COND_V(!shape, 0); btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { @@ -151,8 +152,14 @@ 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); + ERR_FAIL_COND_V(!shape, false); btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin); if (!btShape->isConvex()) { @@ -162,9 +169,6 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf } 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,19 @@ 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()) { + r_closest_safe = 1.0f; + r_closest_unsafe = 1.0f; + bulletdelete(btShape); + return true; + } + 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,6 +204,9 @@ 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); @@ -203,17 +214,19 @@ 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) { - if (p_result_max <= 0) - return 0; +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); + 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); 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); @@ -237,15 +250,15 @@ 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) { - +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()->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); 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); @@ -276,8 +289,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 +332,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)); } @@ -379,8 +376,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 +401,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: @@ -444,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; } } @@ -478,11 +479,25 @@ void SpaceBullet::add_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) { + 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(p_body->get_bt_rigid_body()); + dynamicsWorld->removeCollisionObject(btBody); } else { - dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body()); + dynamicsWorld->removeRigidBody(btBody); } } @@ -543,12 +558,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 +580,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 +625,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 +639,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 +662,6 @@ void SpaceBullet::destroy_world() { } void SpaceBullet::check_ghost_overlaps() { - /// Algorithm support variables btCollisionShape *other_body_shape; btConvexShape *area_shape; @@ -671,8 +675,9 @@ 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) { @@ -690,7 +695,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 +706,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 +730,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 +740,6 @@ void SpaceBullet::check_ghost_overlaps() { other_shape_transform; if (other_body_shape->isConvex()) { - btPointCollector result; btGjkPairDetector gjk_pair_detector( area_shape, @@ -749,14 +754,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 +779,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,24 +838,35 @@ void SpaceBullet::check_body_collision() { pt.getDistance() <= 0.0 || bodyA->was_colliding(bodyB) || bodyB->was_colliding(bodyA)) { - 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 @@ -891,8 +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 @@ -902,8 +918,8 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f SceneTree::get_singleton()->get_current_scene()->add_child(motionVec); SceneTree::get_singleton()->get_current_scene()->add_child(normalLine); - motionVec->set_as_toplevel(true); - normalLine->set_as_toplevel(true); + motionVec->set_as_top_level(true); + normalLine->set_as_top_level(true); red_mat = Ref<StandardMaterial3D>(memnew(StandardMaterial3D)); red_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); @@ -929,10 +945,15 @@ 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(); + p_body->reload_kinematic_shapes(); + } + 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; } } @@ -942,6 +963,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 @@ -957,7 +981,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,13 +1003,27 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f btTransform shape_world_to(shape_world_from); shape_world_to.getOrigin() += motion; - GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia); + 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, &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; @@ -1002,14 +1040,13 @@ 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) { 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()); @@ -1023,6 +1060,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; @@ -1042,12 +1082,16 @@ 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(); + p_body->reload_kinematic_shapes(); + } + btVector3 recover_motion(0, 0, 0); int rays_found = 0; @@ -1073,13 +1117,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) : @@ -1097,28 +1140,25 @@ 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); } 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)) { + if (self_collision_object != proxy->m_clientObject && (proxy->collision_layer & m_collisionFilterMask)) { if (co->getCollisionShape()->isCompound()) { const btCompoundShape *cs = static_cast<btCompoundShape *>(co->getCollisionShape()); @@ -1158,14 +1198,12 @@ 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; 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; @@ -1203,14 +1241,13 @@ 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; // 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; @@ -1221,16 +1258,27 @@ 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; - } 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 +1287,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 +1310,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 +1340,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 +1376,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 +1397,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; @@ -1399,14 +1438,13 @@ 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; // 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 +1466,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 +1477,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); } } |