Skip to content

Commit

Permalink
KinematicBody performance and quality improvements
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
aqnuep committed Mar 26, 2019
1 parent 8129266 commit 6dd65c0
Show file tree
Hide file tree
Showing 9 changed files with 235 additions and 120 deletions.
12 changes: 8 additions & 4 deletions modules/bullet/btRayShape.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,11 @@ void btRayShape::setLength(btScalar p_length) {
reload_cache();
}

void btRayShape::setMargin(btScalar margin) {
btConvexInternalShape::setMargin(margin);
reload_cache();
}

void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {

slipsOnSlope = p_slipsOnSlope;
Expand All @@ -77,10 +82,9 @@ void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVecto
}

void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
#define MARGIN_BROADPHASE 0.1
btVector3 localAabbMin(0, 0, 0);
btVector3 localAabbMax(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin));
btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax);
btVector3 localAabbMax(m_shapeAxis * m_cacheScaledLength);
btTransformAabb(localAabbMin, localAabbMax, m_collisionMargin, t, aabbMin, aabbMax);
}

void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
Expand All @@ -100,5 +104,5 @@ void btRayShape::reload_cache() {
m_cacheScaledLength = m_length * m_localScaling[2];

m_cacheSupportPoint.setIdentity();
m_cacheSupportPoint.setOrigin(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin));
m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength);
}
2 changes: 2 additions & 0 deletions modules/bullet/btRayShape.h
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ btRayShape : public btConvexInternalShape {
void setLength(btScalar p_length);
btScalar getLength() const { return m_length; }

virtual void setMargin(btScalar margin);

void setSlipsOnSlope(bool p_slipOnSlope);
bool getSlipsOnSlope() const { return slipsOnSlope; }

Expand Down
5 changes: 3 additions & 2 deletions modules/bullet/collision_object_bullet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,9 @@
@author AndreaCatania
*/

#define enableDynamicAabbTree false
// We enable dynamic AABB tree so that we can actually perform a broadphase on bodies with compound collision shapes.
// This is crucial for the performance of kinematic bodies and for bodies with transforming shapes.
#define enableDynamicAabbTree true

CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}

Expand Down Expand Up @@ -284,7 +286,6 @@ void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transfor
ERR_FAIL_INDEX(p_index, get_shape_count());

shapes.write[p_index].set_transform(p_transform);
// Note, enableDynamicAabbTree is false because on transform change compound is destroyed
reload_shapes();
}

Expand Down
7 changes: 5 additions & 2 deletions modules/bullet/godot_ray_world_algorithm.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,9 @@
@author AndreaCatania
*/

// Epsilon to account for floating point inaccuracies
#define RAY_PENETRATION_DEPTH_EPSILON 0.01

GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world) :
m_world(world) {}

Expand Down Expand Up @@ -100,8 +103,8 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo

btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));

if (depth >= -ray_shape->getMargin() * 0.5)
depth = 0;
if (depth > -RAY_PENETRATION_DEPTH_EPSILON)
depth = 0.0;

if (ray_shape->getSlipsOnSlope())
resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
Expand Down
301 changes: 206 additions & 95 deletions modules/bullet/space_bullet.cpp

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion modules/bullet/space_bullet.h
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,7 @@ class SpaceBullet : public RIDBullet {
/// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm
bool 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 = NULL);

void 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 add_separation_result(PhysicsServer::SeparationResult *r_results, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const;
int 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);
};
#endif
22 changes: 8 additions & 14 deletions scene/3d/physics_body.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1181,19 +1181,16 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
while (p_max_slides) {

Collision collision;

bool found_collision = false;

int test_type = 0;

do {
for (int i = 0; i < 2; ++i) {
bool collided;
if (test_type == 0) { //collide
if (i == 0) { //collide
collided = move_and_collide(motion, p_infinite_inertia, collision);
if (!collided) {
motion = Vector3(); //clear because no collision happened and motion completed
}
} else {
} else { //separate raycasts (if any)
collided = separate_raycast_shapes(p_infinite_inertia, collision);
if (collided) {
collision.remainder = motion; //keep
Expand Down Expand Up @@ -1222,7 +1219,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
floor_velocity = collision.collider_vel;

if (p_stop_on_slope) {
if ((lv_n + p_floor_direction).length() < 0.01) {
if ((lv_n + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
Transform gt = get_global_transform();
gt.origin -= collision.travel;
set_global_transform(gt);
Expand All @@ -1243,21 +1240,18 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
motion = motion.slide(p_floor_direction);
lv = lv.slide(p_floor_direction);
} else {

Vector3 n = collision.normal;
motion = motion.slide(n);
lv = lv.slide(n);
}

for (int i = 0; i < 3; i++) {
if (locked_axis & (1 << i)) {
lv[i] = 0;
for (int j = 0; j < 3; j++) {
if (locked_axis & (1 << j)) {
lv[j] = 0;
}
}
}

++test_type;
} while (!p_stop_on_slope && test_type < 2);
}

if (!found_collision || motion == Vector3())
break;
Expand Down
2 changes: 1 addition & 1 deletion scene/3d/physics_body.h
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,7 @@ class KinematicBody : public PhysicsBody {
static void _bind_methods();

public:
bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia);

bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision);
Expand Down
2 changes: 1 addition & 1 deletion servers/physics/space_sw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -632,7 +632,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo

int ray_index = -1; //reuse shape
for (int k = 0; k < rays_found; k++) {
if (r_results[ray_index].collision_local_shape == j) {
if (r_results[k].collision_local_shape == j) {
ray_index = k;
}
}
Expand Down

0 comments on commit 6dd65c0

Please sign in to comment.