Merge pull request #27415 from aqnuep/kinematicbody_fixes
KinematicBody performance and quality improvements
This commit is contained in:
commit
262924296b
9 changed files with 241 additions and 126 deletions
|
@ -54,6 +54,11 @@ void btRayShape::setLength(btScalar p_length) {
|
||||||
reload_cache();
|
reload_cache();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void btRayShape::setMargin(btScalar margin) {
|
||||||
|
btConvexInternalShape::setMargin(margin);
|
||||||
|
reload_cache();
|
||||||
|
}
|
||||||
|
|
||||||
void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {
|
void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {
|
||||||
|
|
||||||
slipsOnSlope = p_slipsOnSlope;
|
slipsOnSlope = p_slipsOnSlope;
|
||||||
|
@ -77,10 +82,9 @@ void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVecto
|
||||||
}
|
}
|
||||||
|
|
||||||
void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
|
void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
|
||||||
#define MARGIN_BROADPHASE 0.1
|
|
||||||
btVector3 localAabbMin(0, 0, 0);
|
btVector3 localAabbMin(0, 0, 0);
|
||||||
btVector3 localAabbMax(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin));
|
btVector3 localAabbMax(m_shapeAxis * m_cacheScaledLength);
|
||||||
btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax);
|
btTransformAabb(localAabbMin, localAabbMax, m_collisionMargin, t, aabbMin, aabbMax);
|
||||||
}
|
}
|
||||||
|
|
||||||
void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
|
void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
|
||||||
|
@ -100,5 +104,5 @@ void btRayShape::reload_cache() {
|
||||||
m_cacheScaledLength = m_length * m_localScaling[2];
|
m_cacheScaledLength = m_length * m_localScaling[2];
|
||||||
|
|
||||||
m_cacheSupportPoint.setIdentity();
|
m_cacheSupportPoint.setIdentity();
|
||||||
m_cacheSupportPoint.setOrigin(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin));
|
m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength);
|
||||||
}
|
}
|
||||||
|
|
|
@ -60,6 +60,8 @@ public:
|
||||||
void setLength(btScalar p_length);
|
void setLength(btScalar p_length);
|
||||||
btScalar getLength() const { return m_length; }
|
btScalar getLength() const { return m_length; }
|
||||||
|
|
||||||
|
virtual void setMargin(btScalar margin);
|
||||||
|
|
||||||
void setSlipsOnSlope(bool p_slipOnSlope);
|
void setSlipsOnSlope(bool p_slipOnSlope);
|
||||||
bool getSlipsOnSlope() const { return slipsOnSlope; }
|
bool getSlipsOnSlope() const { return slipsOnSlope; }
|
||||||
|
|
||||||
|
|
|
@ -43,7 +43,9 @@
|
||||||
@author AndreaCatania
|
@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() {}
|
CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
|
||||||
|
|
||||||
|
@ -284,7 +286,6 @@ void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transfor
|
||||||
ERR_FAIL_INDEX(p_index, get_shape_count());
|
ERR_FAIL_INDEX(p_index, get_shape_count());
|
||||||
|
|
||||||
shapes.write[p_index].set_transform(p_transform);
|
shapes.write[p_index].set_transform(p_transform);
|
||||||
// Note, enableDynamicAabbTree is false because on transform change compound is destroyed
|
|
||||||
reload_shapes();
|
reload_shapes();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -39,6 +39,9 @@
|
||||||
@author AndreaCatania
|
@author AndreaCatania
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
// Epsilon to account for floating point inaccuracies
|
||||||
|
#define RAY_PENETRATION_DEPTH_EPSILON 0.01
|
||||||
|
|
||||||
GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world) :
|
GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world) :
|
||||||
m_world(world) {}
|
m_world(world) {}
|
||||||
|
|
||||||
|
@ -100,8 +103,8 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo
|
||||||
|
|
||||||
btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
|
btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
|
||||||
|
|
||||||
if (depth >= -ray_shape->getMargin() * 0.5)
|
if (depth > -RAY_PENETRATION_DEPTH_EPSILON)
|
||||||
depth = 0;
|
depth = 0.0;
|
||||||
|
|
||||||
if (ray_shape->getSlipsOnSlope())
|
if (ray_shape->getSlipsOnSlope())
|
||||||
resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
|
resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
|
||||||
|
|
|
@ -1043,23 +1043,16 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p
|
||||||
btVector3 recover_motion(0, 0, 0);
|
btVector3 recover_motion(0, 0, 0);
|
||||||
|
|
||||||
int rays_found = 0;
|
int rays_found = 0;
|
||||||
|
int rays_found_this_round = 0;
|
||||||
|
|
||||||
for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
|
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);
|
rays_found += rays_found_this_round;
|
||||||
if (!rays_found) {
|
if (rays_found_this_round == 0) {
|
||||||
break;
|
|
||||||
} else {
|
|
||||||
body_transform.getOrigin() += recover_motion;
|
body_transform.getOrigin() += recover_motion;
|
||||||
}
|
break;
|
||||||
}
|
|
||||||
|
|
||||||
//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]);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1069,18 +1062,47 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p
|
||||||
|
|
||||||
struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
|
struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
|
||||||
private:
|
private:
|
||||||
|
btDbvtVolume bounds;
|
||||||
|
|
||||||
const btCollisionObject *self_collision_object;
|
const btCollisionObject *self_collision_object;
|
||||||
uint32_t collision_layer;
|
uint32_t collision_layer;
|
||||||
uint32_t collision_mask;
|
uint32_t collision_mask;
|
||||||
|
|
||||||
public:
|
struct CompoundLeafCallback : btDbvt::ICollide {
|
||||||
Vector<btCollisionObject *> result_collision_objects;
|
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:
|
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),
|
self_collision_object(p_self_collision_object),
|
||||||
collision_layer(p_collision_layer),
|
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() {}
|
virtual ~RecoverPenetrationBroadPhaseCallback() {}
|
||||||
|
|
||||||
|
@ -1089,35 +1111,53 @@ public:
|
||||||
btCollisionObject *co = static_cast<btCollisionObject *>(proxy->m_clientObject);
|
btCollisionObject *co = static_cast<btCollisionObject *>(proxy->m_clientObject);
|
||||||
if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) {
|
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 && 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 true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return false;
|
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) {
|
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) {
|
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]);
|
const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
|
||||||
if (!kin_shape.is_active()) {
|
if (!kin_shape.is_active()) {
|
||||||
continue;
|
continue;
|
||||||
|
@ -1128,15 +1168,56 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
body_shape_position = p_body_position * kin_shape.transform;
|
btTransform shape_transform = p_body_position * kin_shape.transform;
|
||||||
body_shape_position_recovered = body_shape_position;
|
shape_transform.getOrigin() += r_delta_recover_movement;
|
||||||
body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
|
|
||||||
|
|
||||||
kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
|
btVector3 shape_aabb_min, shape_aabb_max;
|
||||||
dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
|
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) {
|
if (!shapes_found) {
|
||||||
btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
|
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()) {
|
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
|
||||||
otherObject->activate(); // Force activation of hitten rigid, soft body
|
otherObject->activate(); // Force activation of hitten rigid, soft body
|
||||||
continue;
|
continue;
|
||||||
|
@ -1144,30 +1225,28 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
if (otherObject->getCollisionShape()->isCompound()) {
|
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
|
if (cs->getChildShape(shape_idx)->isConvex()) {
|
||||||
btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
|
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)) {
|
||||||
for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
|
|
||||||
|
|
||||||
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;
|
penetration = true;
|
||||||
}
|
}
|
||||||
} else {
|
} 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)) {
|
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;
|
penetration = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
} else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape
|
} 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;
|
penetration = true;
|
||||||
}
|
}
|
||||||
} else {
|
} 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;
|
penetration = true;
|
||||||
}
|
}
|
||||||
|
@ -1183,7 +1262,6 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
|
||||||
// Initialize GJK input
|
// Initialize GJK input
|
||||||
btGjkPairDetector::ClosestPointInput gjk_input;
|
btGjkPairDetector::ClosestPointInput gjk_input;
|
||||||
gjk_input.m_transformA = p_transformA;
|
gjk_input.m_transformA = p_transformA;
|
||||||
gjk_input.m_transformA.getOrigin() += r_delta_recover_movement;
|
|
||||||
gjk_input.m_transformB = p_transformB;
|
gjk_input.m_transformB = p_transformB;
|
||||||
|
|
||||||
// Perform GJK test
|
// Perform GJK test
|
||||||
|
@ -1214,7 +1292,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
|
||||||
/// Contact test
|
/// Contact test
|
||||||
|
|
||||||
btTransform tA(p_transformA);
|
btTransform tA(p_transformA);
|
||||||
tA.getOrigin() += r_delta_recover_movement;
|
|
||||||
|
|
||||||
btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, tA, -1, p_shapeId_A);
|
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);
|
btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B);
|
||||||
|
@ -1246,8 +1323,10 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
|
||||||
return false;
|
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 {
|
||||||
|
|
||||||
|
// optimize results (ignore non-colliding)
|
||||||
|
if (p_recover_result.penetration_distance < 0.0) {
|
||||||
const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
|
const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
|
||||||
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
|
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
|
||||||
|
|
||||||
|
@ -1259,26 +1338,66 @@ void SpaceBullet::convert_to_separation_result(PhysicsServer::SeparationResult *
|
||||||
r_result->collider_id = collisionObject->get_instance_id();
|
r_result->collider_id = collisionObject->get_instance_id();
|
||||||
r_result->collider = collisionObject->get_self();
|
r_result->collider = collisionObject->get_self();
|
||||||
r_result->collider_shape = p_recover_result.other_compound_shape_index;
|
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) {
|
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) {
|
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;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1291,15 +1410,11 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
body_shape_position = p_body_position * kin_shape.transform;
|
btTransform shape_transform = p_body_position * kin_shape.transform;
|
||||||
body_shape_position_recovered = body_shape_position;
|
shape_transform.getOrigin() += r_delta_recover_movement;
|
||||||
body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
|
|
||||||
|
|
||||||
kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
|
for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
|
||||||
dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
|
btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
|
||||||
|
|
||||||
for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) {
|
|
||||||
btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
|
|
||||||
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
|
if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
|
||||||
otherObject->activate(); // Force activation of hitten rigid, soft body
|
otherObject->activate(); // Force activation of hitten rigid, soft body
|
||||||
continue;
|
continue;
|
||||||
|
@ -1307,29 +1422,25 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
if (otherObject->getCollisionShape()->isCompound()) {
|
if (otherObject->getCollisionShape()->isCompound()) {
|
||||||
|
const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
|
||||||
// Each convex shape
|
int shape_idx = recover_broad_result.results[i].compound_child_index;
|
||||||
btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
|
ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
|
||||||
for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
|
|
||||||
|
|
||||||
RecoverResult 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)) {
|
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)) {
|
||||||
|
|
||||||
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 {
|
} else {
|
||||||
|
|
||||||
RecoverResult recover_result;
|
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_count;
|
||||||
}
|
|
||||||
|
|
||||||
return ray_index;
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -212,7 +212,7 @@ private:
|
||||||
/// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm
|
/// 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);
|
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);
|
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
|
#endif
|
||||||
|
|
|
@ -1181,19 +1181,16 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
|
||||||
while (p_max_slides) {
|
while (p_max_slides) {
|
||||||
|
|
||||||
Collision collision;
|
Collision collision;
|
||||||
|
|
||||||
bool found_collision = false;
|
bool found_collision = false;
|
||||||
|
|
||||||
int test_type = 0;
|
for (int i = 0; i < 2; ++i) {
|
||||||
|
|
||||||
do {
|
|
||||||
bool collided;
|
bool collided;
|
||||||
if (test_type == 0) { //collide
|
if (i == 0) { //collide
|
||||||
collided = move_and_collide(motion, p_infinite_inertia, collision);
|
collided = move_and_collide(motion, p_infinite_inertia, collision);
|
||||||
if (!collided) {
|
if (!collided) {
|
||||||
motion = Vector3(); //clear because no collision happened and motion completed
|
motion = Vector3(); //clear because no collision happened and motion completed
|
||||||
}
|
}
|
||||||
} else {
|
} else { //separate raycasts (if any)
|
||||||
collided = separate_raycast_shapes(p_infinite_inertia, collision);
|
collided = separate_raycast_shapes(p_infinite_inertia, collision);
|
||||||
if (collided) {
|
if (collided) {
|
||||||
collision.remainder = motion; //keep
|
collision.remainder = motion; //keep
|
||||||
|
@ -1219,7 +1216,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
|
||||||
floor_velocity = collision.collider_vel;
|
floor_velocity = collision.collider_vel;
|
||||||
|
|
||||||
if (p_stop_on_slope) {
|
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();
|
Transform gt = get_global_transform();
|
||||||
gt.origin -= collision.travel;
|
gt.origin -= collision.travel;
|
||||||
set_global_transform(gt);
|
set_global_transform(gt);
|
||||||
|
@ -1240,21 +1237,18 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
|
||||||
motion = motion.slide(p_floor_direction);
|
motion = motion.slide(p_floor_direction);
|
||||||
lv = lv.slide(p_floor_direction);
|
lv = lv.slide(p_floor_direction);
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
Vector3 n = collision.normal;
|
Vector3 n = collision.normal;
|
||||||
motion = motion.slide(n);
|
motion = motion.slide(n);
|
||||||
lv = lv.slide(n);
|
lv = lv.slide(n);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int j = 0; j < 3; j++) {
|
||||||
if (locked_axis & (1 << i)) {
|
if (locked_axis & (1 << j)) {
|
||||||
lv[i] = 0;
|
lv[j] = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
++test_type;
|
|
||||||
} while (!p_stop_on_slope && test_type < 2);
|
|
||||||
|
|
||||||
if (!found_collision || motion == Vector3())
|
if (!found_collision || motion == Vector3())
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -317,7 +317,7 @@ protected:
|
||||||
static void _bind_methods();
|
static void _bind_methods();
|
||||||
|
|
||||||
public:
|
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 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);
|
bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision);
|
||||||
|
|
|
@ -630,7 +630,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
|
||||||
|
|
||||||
int ray_index = -1; //reuse shape
|
int ray_index = -1; //reuse shape
|
||||||
for (int k = 0; k < rays_found; k++) {
|
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;
|
ray_index = k;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue