Fixed kinematic movement on concave shape
This commit is contained in:
AndreaCatania
parent
6277e6d40a
commit
e6ba163031
|
|
@ -77,7 +77,7 @@ btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalCo
|
|||
|
||||
PhysicsDirectSpaceState::ShapeResult &result = m_results[count];
|
||||
|
||||
result.shape = convexResult.m_localShapeInfo->m_shapePart;
|
||||
result.shape = convexResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is a odd name but contains the compound shape ID
|
||||
result.rid = gObj->get_self();
|
||||
result.collider_id = gObj->get_instance_id();
|
||||
result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
|
||||
|
|
@ -122,7 +122,7 @@ bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0)
|
|||
|
||||
btScalar GodotClosestConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
|
||||
btScalar res = btCollisionWorld::ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
|
||||
m_shapePart = convexResult.m_localShapeInfo->m_shapePart;
|
||||
m_shapeId = convexResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is a odd name but contains the compound shape ID
|
||||
return res;
|
||||
}
|
||||
|
||||
|
|
@ -242,3 +242,21 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp
|
|||
|
||||
return cp.getDistance();
|
||||
}
|
||||
|
||||
void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth) {
|
||||
|
||||
if (depth < 0) {
|
||||
// Has penetration
|
||||
if (m_most_penetrated_distance > depth) {
|
||||
|
||||
bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
|
||||
|
||||
m_most_penetrated_distance = depth;
|
||||
m_pointCollisionObject = (isSwapped ? m_body0Wrap : m_body1Wrap)->getCollisionObject();
|
||||
m_other_compound_shape_index = isSwapped ? m_index1 : m_index0;
|
||||
m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
|
||||
m_pointWorld = isSwapped ? (pointInWorldOnB + normalOnBInWorld * depth) : pointInWorldOnB;
|
||||
m_penetration_distance = depth;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -88,7 +88,7 @@ public:
|
|||
struct GodotClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
|
||||
public:
|
||||
const Set<RID> *m_exclude;
|
||||
int m_shapePart;
|
||||
int m_shapeId;
|
||||
|
||||
GodotClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const Set<RID> *p_exclude)
|
||||
: btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld), m_exclude(p_exclude) {}
|
||||
|
|
@ -149,4 +149,31 @@ public:
|
|||
virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
|
||||
};
|
||||
|
||||
struct GodotDeepPenetrationContactResultCallback : public btManifoldResult {
|
||||
btVector3 m_pointNormalWorld;
|
||||
btVector3 m_pointWorld;
|
||||
btScalar m_penetration_distance;
|
||||
int m_other_compound_shape_index;
|
||||
const btCollisionObject *m_pointCollisionObject;
|
||||
|
||||
btScalar m_most_penetrated_distance;
|
||||
|
||||
GodotDeepPenetrationContactResultCallback(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap)
|
||||
: btManifoldResult(body0Wrap, body1Wrap),
|
||||
m_pointCollisionObject(NULL),
|
||||
m_penetration_distance(0),
|
||||
m_other_compound_shape_index(0),
|
||||
m_most_penetrated_distance(1e20) {}
|
||||
|
||||
void reset() {
|
||||
m_pointCollisionObject = NULL;
|
||||
m_most_penetrated_distance = 1e20;
|
||||
}
|
||||
|
||||
bool hasHit() {
|
||||
return m_pointCollisionObject;
|
||||
}
|
||||
|
||||
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth);
|
||||
};
|
||||
#endif // GODOT_RESULT_CALLBACKS_H
|
||||
|
|
|
|||
|
|
@ -183,7 +183,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
|
|||
B_TO_G(btResult.m_hitNormalWorld, r_info->normal);
|
||||
r_info->rid = collision_object->get_self();
|
||||
r_info->collider_id = collision_object->get_instance_id();
|
||||
r_info->shape = btResult.m_shapePart;
|
||||
r_info->shape = btResult.m_shapeId;
|
||||
}
|
||||
|
||||
bulletdelete(bt_convex_shape);
|
||||
|
|
@ -910,26 +910,26 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
|
|||
|
||||
{ /// Phase three - Recover + contact test with margin
|
||||
|
||||
RecoverResult recover_result;
|
||||
RecoverResult r_recover_result;
|
||||
|
||||
hasPenetration = recover_from_penetration(p_body, body_safe_position, recovered_motion, &recover_result);
|
||||
hasPenetration = recover_from_penetration(p_body, body_safe_position, recovered_motion, &r_recover_result);
|
||||
|
||||
if (r_result) {
|
||||
|
||||
B_TO_G(recovered_motion + recover_initial_position, r_result->motion);
|
||||
|
||||
if (hasPenetration) {
|
||||
const btRigidBody *btRigid = static_cast<const btRigidBody *>(recover_result.other_collision_object);
|
||||
const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
|
||||
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
|
||||
|
||||
r_result->remainder = p_motion - r_result->motion; // is the remaining movements
|
||||
B_TO_G(recover_result.pointWorld, r_result->collision_point);
|
||||
B_TO_G(recover_result.pointNormalWorld, r_result->collision_normal);
|
||||
B_TO_G(btRigid->getVelocityInLocalPoint(recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
|
||||
B_TO_G(r_recover_result.pointWorld, r_result->collision_point);
|
||||
B_TO_G(r_recover_result.pointNormalWorld, r_result->collision_normal);
|
||||
B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
|
||||
r_result->collider = collisionObject->get_self();
|
||||
r_result->collider_id = collisionObject->get_instance_id();
|
||||
r_result->collider_shape = recover_result.other_compound_shape_index;
|
||||
r_result->collision_local_shape = recover_result.local_shape_most_recovered;
|
||||
r_result->collider_shape = r_recover_result.other_compound_shape_index;
|
||||
r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
|
||||
|
||||
//{ /// Add manifold point to manage collisions
|
||||
// btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid);
|
||||
|
|
@ -995,7 +995,7 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btVector3 &out_recover_position, RecoverResult *recover_result) {
|
||||
bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
|
||||
|
||||
RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
|
||||
|
||||
|
|
@ -1005,9 +1005,6 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
|||
// Broad phase support
|
||||
btVector3 minAabb, maxAabb;
|
||||
|
||||
// GJK support
|
||||
btGjkPairDetector::ClosestPointInput gjk_input;
|
||||
|
||||
bool penetration = false;
|
||||
|
||||
// For each shape
|
||||
|
|
@ -1022,7 +1019,7 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
|||
|
||||
body_shape_position = p_body_position * kin_shape.transform;
|
||||
body_shape_position_recovered = body_shape_position;
|
||||
body_shape_position_recovered.getOrigin() += out_recover_position;
|
||||
body_shape_position_recovered.getOrigin() += r_recover_position;
|
||||
|
||||
kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
|
||||
dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
|
||||
|
|
@ -1032,66 +1029,33 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
|||
if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()))
|
||||
continue;
|
||||
|
||||
if (otherObject->getCollisionShape()->isCompound()) { /// Execute GJK test against all shapes
|
||||
if (otherObject->getCollisionShape()->isCompound()) {
|
||||
|
||||
// Each convex shape
|
||||
btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
|
||||
for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
|
||||
|
||||
if (!cs->getChildShape(x)->isConvex())
|
||||
continue;
|
||||
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), r_recover_position, r_recover_result)) {
|
||||
|
||||
// Initialize GJK input
|
||||
gjk_input.m_transformA = body_shape_position;
|
||||
gjk_input.m_transformA.getOrigin() += out_recover_position;
|
||||
gjk_input.m_transformB = otherObject->getWorldTransform() * cs->getChildTransform(x);
|
||||
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), r_recover_position, r_recover_result)) {
|
||||
|
||||
// Perform GJK test
|
||||
btPointCollector result;
|
||||
btGjkPairDetector gjk_pair_detector(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(x)), gjk_simplex_solver, gjk_epa_pen_solver);
|
||||
gjk_pair_detector.getClosestPoints(gjk_input, result, 0);
|
||||
if (0 > result.m_distance) {
|
||||
// Has penetration
|
||||
out_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1);
|
||||
penetration = true;
|
||||
|
||||
if (recover_result) {
|
||||
|
||||
recover_result->hasPenetration = true;
|
||||
recover_result->other_collision_object = otherObject;
|
||||
recover_result->other_compound_shape_index = x;
|
||||
recover_result->penetration_distance = result.m_distance;
|
||||
recover_result->pointNormalWorld = result.m_normalOnBInWorld;
|
||||
recover_result->pointWorld = result.m_pointInWorld;
|
||||
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(), r_recover_position, r_recover_result)) {
|
||||
|
||||
// Initialize GJK input
|
||||
gjk_input.m_transformA = body_shape_position;
|
||||
gjk_input.m_transformA.getOrigin() += out_recover_position;
|
||||
gjk_input.m_transformB = otherObject->getWorldTransform();
|
||||
|
||||
// Perform GJK test
|
||||
btPointCollector result;
|
||||
btGjkPairDetector gjk_pair_detector(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), gjk_simplex_solver, gjk_epa_pen_solver);
|
||||
gjk_pair_detector.getClosestPoints(gjk_input, result, 0);
|
||||
if (0 > result.m_distance) {
|
||||
// Has penetration
|
||||
out_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1);
|
||||
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(), r_recover_position, r_recover_result)) {
|
||||
|
||||
if (recover_result) {
|
||||
|
||||
recover_result->hasPenetration = true;
|
||||
recover_result->other_collision_object = otherObject;
|
||||
recover_result->other_compound_shape_index = 0;
|
||||
recover_result->penetration_distance = result.m_distance;
|
||||
recover_result->pointNormalWorld = result.m_normalOnBInWorld;
|
||||
recover_result->pointWorld = result.m_pointInWorld;
|
||||
}
|
||||
penetration = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1099,3 +1063,70 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
|||
|
||||
return penetration;
|
||||
}
|
||||
|
||||
bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
|
||||
|
||||
// Initialize GJK input
|
||||
btGjkPairDetector::ClosestPointInput gjk_input;
|
||||
gjk_input.m_transformA = p_transformA;
|
||||
gjk_input.m_transformA.getOrigin() += r_recover_position;
|
||||
gjk_input.m_transformB = p_transformB;
|
||||
|
||||
// Perform GJK test
|
||||
btPointCollector result;
|
||||
btGjkPairDetector gjk_pair_detector(p_shapeA, p_shapeB, gjk_simplex_solver, gjk_epa_pen_solver);
|
||||
gjk_pair_detector.getClosestPoints(gjk_input, result, 0);
|
||||
if (0 > result.m_distance) {
|
||||
// Has penetration
|
||||
r_recover_position += result.m_normalOnBInWorld * (result.m_distance * -1);
|
||||
|
||||
if (r_recover_result) {
|
||||
|
||||
r_recover_result->hasPenetration = true;
|
||||
r_recover_result->other_collision_object = p_objectB;
|
||||
r_recover_result->other_compound_shape_index = p_shapeId_B;
|
||||
r_recover_result->penetration_distance = result.m_distance;
|
||||
r_recover_result->pointNormalWorld = result.m_normalOnBInWorld;
|
||||
r_recover_result->pointWorld = result.m_pointInWorld;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
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, btVector3 &r_recover_position, RecoverResult *r_recover_result) {
|
||||
|
||||
/// Contact test
|
||||
|
||||
btTransform p_recovered_transformA(p_transformA);
|
||||
p_recovered_transformA.getOrigin() += r_recover_position;
|
||||
|
||||
btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, p_recovered_transformA, -1, p_shapeId_A);
|
||||
btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B);
|
||||
|
||||
btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CLOSEST_POINT_ALGORITHMS);
|
||||
if (algorithm) {
|
||||
GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB);
|
||||
//discrete collision detection query
|
||||
algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult);
|
||||
|
||||
algorithm->~btCollisionAlgorithm();
|
||||
dispatcher->freeCollisionAlgorithm(algorithm);
|
||||
|
||||
if (contactPointResult.hasHit()) {
|
||||
r_recover_position += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1);
|
||||
|
||||
if (r_recover_result) {
|
||||
|
||||
r_recover_result->hasPenetration = true;
|
||||
r_recover_result->other_collision_object = p_objectB;
|
||||
r_recover_result->other_compound_shape_index = p_shapeId_B;
|
||||
r_recover_result->penetration_distance = contactPointResult.m_penetration_distance;
|
||||
r_recover_result->pointNormalWorld = contactPointResult.m_pointNormalWorld;
|
||||
r_recover_result->pointWorld = contactPointResult.m_pointWorld;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -189,6 +189,12 @@ private:
|
|||
: hasPenetration(false) {}
|
||||
};
|
||||
|
||||
bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btVector3 &out_recover_position, RecoverResult *recover_result = NULL);
|
||||
bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btVector3 &r_recover_position, RecoverResult *r_recover_result = NULL);
|
||||
/// This is an API that recover a kinematic object from penetration
|
||||
/// This allow only Convex Convex test and it always use GJK algorithm, With this API we don't benefit of Bullet special accelerated functions
|
||||
bool RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btVector3 &r_recover_position, RecoverResult *r_recover_result);
|
||||
/// This is an API that recover a kinematic object from penetration
|
||||
/// 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, btVector3 &r_recover_position, RecoverResult *r_recover_result);
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in a new issue