KinematicBody performance and quality improvements
With this change finally one can use compound collisions (like those created by Gridmaps) without serious performance issues. The previous KinematicBody code for Bullet was practically doing a whole bunch of unnecessary calculations. Gridmaps with fairly large octant sizes (in my case 32) can get up to 10000x speedup with this change (literally!). I expect the FPS demo to get a fair speedup as well. List of fixes and improvements: - Fixed a general bug in move_and_slide that affects both GodotPhysics and Bullet, where ray shapes would be ignored unless the stop_on_slope parameter is disabled. Not sure where that came from, but looking at the 2D physics code it was obvious there's a difference. - Enabled the dynamic AABB tree that Bullet uses to allow broadphase collision tests against individual shapes of compound shapes. This is crucial to get good performance with Gridmaps and in general improves the performance whenever a KinematicBody collides with compound collision shapes. - Added code to the broadphase collision detection code used by the Bullet module for KinematicBodies to also do broadphase on the sub-shapes of compound collision shapes. This is possible thanks to the dynamic AABB tree that was previously disabled and it's the change that provides the biggest performance boost. - Now broadphase test is only done once per KinematicBody in Bullet instead of once per each of its shapes which was completely unnecessary. - Fixed the way how the ray separation results are populated in Bullet which was completely broken previously, overwriting previous results and similar non-sense. - Fixed ray shapes for good now. Previously the margin set in the editor was not respected at all, and the KinematicBody code for ray separation was complete bogus, thus all previous attempts to fix it were mislead. - Fixed an obvious bug also in GodotPhysics where an out-of-bounds index was used in the ray result array. There are a whole set of other problems with the KinematicBody code of Bullet which cost performance and may cause unexpected behavior, but those are not addressed in this change (need to keep it "simple"). Not sure whether this fixes any outstanding Github issues but I wouldn't be surprised.
This commit is contained in:
Daniel Rakos
@ -1043,23 +1043,16 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p
|
||||
btVector3 recover_motion(0, 0, 0);
|
||||
|
||||
int rays_found = 0;
|
||||
int rays_found_this_round = 0;
|
||||
|
||||
for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
|
||||
int last_ray_index = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max, recover_motion, r_results);
|
||||
PhysicsServer::SeparationResult *next_results = &r_results[rays_found];
|
||||
rays_found_this_round = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max - rays_found, recover_motion, next_results);
|
||||
|
||||
rays_found = MAX(last_ray_index, rays_found);
|
||||
if (!rays_found) {
|
||||
break;
|
||||
} else {
|
||||
rays_found += rays_found_this_round;
|
||||
if (rays_found_this_round == 0) {
|
||||
body_transform.getOrigin() += recover_motion;
|
||||
}
|
||||
}
|
||||
|
||||
//optimize results (remove non colliding)
|
||||
for (int i = 0; i < rays_found; i++) {
|
||||
if (r_results[i].collision_depth >= 0) {
|
||||
rays_found--;
|
||||
SWAP(r_results[i], r_results[rays_found]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1069,18 +1062,47 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p
|
||||
|
||||
struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
|
||||
private:
|
||||
btDbvtVolume bounds;
|
||||
|
||||
const btCollisionObject *self_collision_object;
|
||||
uint32_t collision_layer;
|
||||
uint32_t collision_mask;
|
||||
|
||||
public:
|
||||
Vector<btCollisionObject *> result_collision_objects;
|
||||
struct CompoundLeafCallback : btDbvt::ICollide {
|
||||
private:
|
||||
RecoverPenetrationBroadPhaseCallback *parent_callback;
|
||||
btCollisionObject *collision_object;
|
||||
|
||||
public:
|
||||
CompoundLeafCallback(RecoverPenetrationBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) :
|
||||
parent_callback(p_parent_callback),
|
||||
collision_object(p_collision_object) {
|
||||
}
|
||||
|
||||
void Process(const btDbvtNode *leaf) {
|
||||
BroadphaseResult result;
|
||||
result.collision_object = collision_object;
|
||||
result.compound_child_index = leaf->dataAsInt;
|
||||
parent_callback->results.push_back(result);
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask) :
|
||||
struct BroadphaseResult {
|
||||
btCollisionObject *collision_object;
|
||||
int compound_child_index;
|
||||
};
|
||||
|
||||
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) :
|
||||
self_collision_object(p_self_collision_object),
|
||||
collision_layer(p_collision_layer),
|
||||
collision_mask(p_collision_mask) {}
|
||||
collision_mask(p_collision_mask) {
|
||||
|
||||
bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max);
|
||||
}
|
||||
|
||||
virtual ~RecoverPenetrationBroadPhaseCallback() {}
|
||||
|
||||
@ -1089,35 +1111,53 @@ public:
|
||||
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);
|
||||
if (co->getCollisionShape()->isCompound()) {
|
||||
const btCompoundShape *cs = static_cast<btCompoundShape *>(co->getCollisionShape());
|
||||
|
||||
if (cs->getNumChildShapes() > 1) {
|
||||
const btDbvt *tree = cs->getDynamicAabbTree();
|
||||
ERR_FAIL_COND_V(tree == NULL, true);
|
||||
|
||||
// Transform bounds into compound shape local space
|
||||
const btTransform other_in_compound_space = co->getWorldTransform().inverse();
|
||||
const btMatrix3x3 abs_b = other_in_compound_space.getBasis().absolute();
|
||||
const btVector3 local_center = other_in_compound_space(bounds.Center());
|
||||
const btVector3 local_extent = bounds.Extents().dot3(abs_b[0], abs_b[1], abs_b[2]);
|
||||
const btVector3 local_aabb_min = local_center - local_extent;
|
||||
const btVector3 local_aabb_max = local_center + local_extent;
|
||||
const btDbvtVolume local_bounds = btDbvtVolume::FromMM(local_aabb_min, local_aabb_max);
|
||||
|
||||
// Test collision against compound child shapes using its AABB tree
|
||||
CompoundLeafCallback compound_leaf_callback(this, co);
|
||||
tree->collideTV(tree->m_root, local_bounds, compound_leaf_callback);
|
||||
} else {
|
||||
// If there's only a single child shape then there's no need to search any more, we know which child overlaps
|
||||
BroadphaseResult result;
|
||||
result.collision_object = co;
|
||||
result.compound_child_index = 0;
|
||||
results.push_back(result);
|
||||
}
|
||||
} else {
|
||||
BroadphaseResult result;
|
||||
result.collision_object = co;
|
||||
result.compound_child_index = -1;
|
||||
results.push_back(result);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void reset() {
|
||||
result_collision_objects.clear();
|
||||
}
|
||||
};
|
||||
|
||||
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) {
|
||||
|
||||
RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
|
||||
// Calculate the cummulative AABB of all shapes of the kinematic body
|
||||
btVector3 aabb_min, aabb_max;
|
||||
bool shapes_found = false;
|
||||
|
||||
btTransform body_shape_position;
|
||||
btTransform body_shape_position_recovered;
|
||||
|
||||
// Broad phase support
|
||||
btVector3 minAabb, maxAabb;
|
||||
|
||||
bool penetration = false;
|
||||
|
||||
// For each shape
|
||||
for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
|
||||
|
||||
recover_broad_result.reset();
|
||||
|
||||
const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
|
||||
if (!kin_shape.is_active()) {
|
||||
continue;
|
||||
@ -1128,15 +1168,56 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
||||
continue;
|
||||
}
|
||||
|
||||
body_shape_position = p_body_position * kin_shape.transform;
|
||||
body_shape_position_recovered = body_shape_position;
|
||||
body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
|
||||
btTransform shape_transform = p_body_position * kin_shape.transform;
|
||||
shape_transform.getOrigin() += r_delta_recover_movement;
|
||||
|
||||
kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
|
||||
dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
|
||||
btVector3 shape_aabb_min, shape_aabb_max;
|
||||
kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max);
|
||||
|
||||
for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) {
|
||||
btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
|
||||
if (!shapes_found) {
|
||||
aabb_min = shape_aabb_min;
|
||||
aabb_max = shape_aabb_max;
|
||||
shapes_found = true;
|
||||
} else {
|
||||
aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x());
|
||||
aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y());
|
||||
aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z());
|
||||
|
||||
aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x());
|
||||
aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y());
|
||||
aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z());
|
||||
}
|
||||
}
|
||||
|
||||
// If there are no shapes then there is no penetration either
|
||||
if (!shapes_found) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 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);
|
||||
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;
|
||||
}
|
||||
|
||||
if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
|
||||
// Skip rayshape in order to implement custom separation process
|
||||
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;
|
||||
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
|
||||
otherObject->activate(); // Force activation of hitten rigid, soft body
|
||||
continue;
|
||||
@ -1144,30 +1225,28 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
||||
continue;
|
||||
|
||||
if (otherObject->getCollisionShape()->isCompound()) {
|
||||
const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
|
||||
int shape_idx = recover_broad_result.results[i].compound_child_index;
|
||||
ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
|
||||
|
||||
// Each convex shape
|
||||
btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
|
||||
for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
|
||||
if (cs->getChildShape(shape_idx)->isConvex()) {
|
||||
if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(shape_idx)), otherObject, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
|
||||
|
||||
if (cs->getChildShape(x)->isConvex()) {
|
||||
if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(x)), otherObject, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), 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 (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
|
||||
|
||||
penetration = true;
|
||||
}
|
||||
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, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
|
||||
if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, 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, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_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, r_recover_result)) {
|
||||
|
||||
penetration = true;
|
||||
}
|
||||
@ -1183,7 +1262,6 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
|
||||
// Initialize GJK input
|
||||
btGjkPairDetector::ClosestPointInput gjk_input;
|
||||
gjk_input.m_transformA = p_transformA;
|
||||
gjk_input.m_transformA.getOrigin() += r_delta_recover_movement;
|
||||
gjk_input.m_transformB = p_transformB;
|
||||
|
||||
// Perform GJK test
|
||||
@ -1214,7 +1292,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
|
||||
/// Contact test
|
||||
|
||||
btTransform tA(p_transformA);
|
||||
tA.getOrigin() += r_delta_recover_movement;
|
||||
|
||||
btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, tA, -1, p_shapeId_A);
|
||||
btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B);
|
||||
@ -1246,39 +1323,81 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
|
||||
return false;
|
||||
}
|
||||
|
||||
void SpaceBullet::convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const {
|
||||
int SpaceBullet::add_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const {
|
||||
|
||||
const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
|
||||
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
|
||||
// optimize results (ignore non-colliding)
|
||||
if (p_recover_result.penetration_distance < 0.0) {
|
||||
const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
|
||||
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
|
||||
|
||||
r_result->collision_depth = p_recover_result.penetration_distance;
|
||||
B_TO_G(p_recover_result.pointWorld, r_result->collision_point);
|
||||
B_TO_G(p_recover_result.normal, r_result->collision_normal);
|
||||
B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity);
|
||||
r_result->collision_local_shape = p_shape_id;
|
||||
r_result->collider_id = collisionObject->get_instance_id();
|
||||
r_result->collider = collisionObject->get_self();
|
||||
r_result->collider_shape = p_recover_result.other_compound_shape_index;
|
||||
r_result->collision_depth = p_recover_result.penetration_distance;
|
||||
B_TO_G(p_recover_result.pointWorld, r_result->collision_point);
|
||||
B_TO_G(p_recover_result.normal, r_result->collision_normal);
|
||||
B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity);
|
||||
r_result->collision_local_shape = p_shape_id;
|
||||
r_result->collider_id = collisionObject->get_instance_id();
|
||||
r_result->collider = collisionObject->get_self();
|
||||
r_result->collider_shape = p_recover_result.other_compound_shape_index;
|
||||
|
||||
return 1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
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, PhysicsServer::SeparationResult *r_results) {
|
||||
|
||||
RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
|
||||
// Calculate the cummulative AABB of all shapes of the kinematic body
|
||||
btVector3 aabb_min, aabb_max;
|
||||
bool shapes_found = false;
|
||||
|
||||
btTransform body_shape_position;
|
||||
btTransform body_shape_position_recovered;
|
||||
|
||||
// Broad phase support
|
||||
btVector3 minAabb, maxAabb;
|
||||
|
||||
int ray_index = 0;
|
||||
|
||||
// For each shape
|
||||
for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
|
||||
|
||||
recover_broad_result.reset();
|
||||
const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
|
||||
if (!kin_shape.is_active()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (ray_index >= p_result_max) {
|
||||
if (kin_shape.shape->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
btTransform shape_transform = p_body_position * kin_shape.transform;
|
||||
shape_transform.getOrigin() += r_delta_recover_movement;
|
||||
|
||||
btVector3 shape_aabb_min, shape_aabb_max;
|
||||
kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max);
|
||||
|
||||
if (!shapes_found) {
|
||||
aabb_min = shape_aabb_min;
|
||||
aabb_max = shape_aabb_max;
|
||||
shapes_found = true;
|
||||
} else {
|
||||
aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x());
|
||||
aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y());
|
||||
aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z());
|
||||
|
||||
aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x());
|
||||
aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y());
|
||||
aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z());
|
||||
}
|
||||
}
|
||||
|
||||
// If there are no shapes then there is no penetration either
|
||||
if (!shapes_found) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 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);
|
||||
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;
|
||||
}
|
||||
|
||||
@ -1291,15 +1410,11 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
|
||||
continue;
|
||||
}
|
||||
|
||||
body_shape_position = p_body_position * kin_shape.transform;
|
||||
body_shape_position_recovered = body_shape_position;
|
||||
body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
|
||||
btTransform shape_transform = p_body_position * kin_shape.transform;
|
||||
shape_transform.getOrigin() += r_delta_recover_movement;
|
||||
|
||||
kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
|
||||
dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
|
||||
|
||||
for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) {
|
||||
btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
|
||||
for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
|
||||
btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
|
||||
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
|
||||
otherObject->activate(); // Force activation of hitten rigid, soft body
|
||||
continue;
|
||||
@ -1307,29 +1422,25 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
|
||||
continue;
|
||||
|
||||
if (otherObject->getCollisionShape()->isCompound()) {
|
||||
const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
|
||||
int shape_idx = recover_broad_result.results[i].compound_child_index;
|
||||
ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
|
||||
|
||||
// Each convex shape
|
||||
btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
|
||||
for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
|
||||
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)) {
|
||||
|
||||
RecoverResult recover_result;
|
||||
if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
|
||||
|
||||
convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject);
|
||||
}
|
||||
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, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &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)) {
|
||||
|
||||
convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject);
|
||||
ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
++ray_index;
|
||||
}
|
||||
|
||||
return ray_index;
|
||||
return ray_count;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user