summaryrefslogtreecommitdiff
path: root/modules/bullet/space_bullet.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'modules/bullet/space_bullet.cpp')
-rw-r--r--modules/bullet/space_bullet.cpp273
1 files changed, 137 insertions, 136 deletions
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index dc16d8402d..3a1f5d78dd 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -1,10 +1,9 @@
/*************************************************************************/
/* space_bullet.cpp */
-/* Author: AndreaCatania */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
-/* http://www.godotengine.org */
+/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */
@@ -30,14 +29,7 @@
/*************************************************************************/
#include "space_bullet.h"
-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-#include "BulletCollision/CollisionDispatch/btGhostObject.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
-#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
-#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
-#include "btBulletDynamicsCommon.h"
+
#include "bullet_physics_server.h"
#include "bullet_types_converter.h"
#include "bullet_utilities.h"
@@ -48,8 +40,22 @@
#include "servers/physics_server.h"
#include "soft_body_bullet.h"
#include "ustring.h"
+
+#include <BulletCollision/CollisionDispatch/btCollisionObject.h>
+#include <BulletCollision/CollisionDispatch/btGhostObject.h>
+#include <BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h>
+#include <BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h>
+#include <BulletCollision/NarrowPhaseCollision/btPointCollector.h>
+#include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
+#include <BulletSoftBody/btSoftRigidDynamicsWorld.h>
+#include <btBulletDynamicsCommon.h>
+
#include <assert.h>
+/**
+ @author AndreaCatania
+*/
+
BulletPhysicsDirectSpaceState::BulletPhysicsDirectSpaceState(SpaceBullet *p_space) :
PhysicsDirectSpaceState(),
space(p_space) {}
@@ -110,13 +116,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 *p_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) {
+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) {
if (p_result_max <= 0)
return 0;
ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
- btCollisionShape *btShape = shape->create_bt_shape();
+ btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin);
if (!btShape->isConvex()) {
bulletdelete(btShape);
ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -124,21 +130,18 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
}
btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
- btVector3 scale_with_margin;
- G_TO_B(p_xform.basis.get_scale(), scale_with_margin);
- btConvex->setLocalScaling(scale_with_margin);
-
btTransform bt_xform;
G_TO_B(p_xform, bt_xform);
+ UNSCALE_BT_BASIS(bt_xform);
btCollisionObject collision_object;
collision_object.setCollisionShape(btConvex);
collision_object.setWorldTransform(bt_xform);
- GodotAllContactResultCallback btQuery(&collision_object, p_results, p_result_max, &p_exclude);
+ GodotAllContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude);
btQuery.m_collisionFilterGroup = 0;
btQuery.m_collisionFilterMask = p_collision_mask;
- btQuery.m_closestDistanceThreshold = p_margin;
+ btQuery.m_closestDistanceThreshold = 0;
space->dynamicsWorld->contactTest(&collision_object, btQuery);
bulletdelete(btConvex);
@@ -149,7 +152,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, ShapeRestInfo *r_info) {
ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
- btCollisionShape *btShape = shape->create_bt_shape();
+ btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
if (!btShape->isConvex()) {
bulletdelete(btShape);
ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -160,12 +163,9 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
btVector3 bt_motion;
G_TO_B(p_motion, bt_motion);
- btVector3 scale_with_margin;
- G_TO_B(p_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
- bt_convex_shape->setLocalScaling(scale_with_margin);
-
btTransform bt_xform_from;
G_TO_B(p_xform, bt_xform_from);
+ UNSCALE_BT_BASIS(bt_xform_from);
btTransform bt_xform_to(bt_xform_from);
bt_xform_to.getOrigin() += bt_motion;
@@ -202,7 +202,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
- btCollisionShape *btShape = shape->create_bt_shape();
+ btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
if (!btShape->isConvex()) {
bulletdelete(btShape);
ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -210,12 +210,9 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
}
btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
- btVector3 scale_with_margin;
- G_TO_B(p_shape_xform.basis.get_scale(), scale_with_margin);
- btConvex->setLocalScaling(scale_with_margin);
-
btTransform bt_xform;
G_TO_B(p_shape_xform, bt_xform);
+ UNSCALE_BT_BASIS(bt_xform);
btCollisionObject collision_object;
collision_object.setCollisionShape(btConvex);
@@ -224,7 +221,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
GodotContactPairContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude);
btQuery.m_collisionFilterGroup = 0;
btQuery.m_collisionFilterMask = p_collision_mask;
- btQuery.m_closestDistanceThreshold = p_margin;
+ btQuery.m_closestDistanceThreshold = 0;
space->dynamicsWorld->contactTest(&collision_object, btQuery);
r_result_count = btQuery.m_count;
@@ -237,7 +234,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
- btCollisionShape *btShape = shape->create_bt_shape();
+ btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
if (!btShape->isConvex()) {
bulletdelete(btShape);
ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
@@ -245,12 +242,9 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
}
btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
- btVector3 scale_with_margin;
- G_TO_B(p_shape_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
- btConvex->setLocalScaling(scale_with_margin);
-
btTransform bt_xform;
G_TO_B(p_shape_xform, bt_xform);
+ UNSCALE_BT_BASIS(bt_xform);
btCollisionObject collision_object;
collision_object.setCollisionShape(btConvex);
@@ -259,7 +253,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
GodotRestInfoContactResultCallback btQuery(&collision_object, r_info, &p_exclude);
btQuery.m_collisionFilterGroup = 0;
btQuery.m_collisionFilterMask = p_collision_mask;
- btQuery.m_closestDistanceThreshold = p_margin;
+ btQuery.m_closestDistanceThreshold = 0;
space->dynamicsWorld->contactTest(&collision_object, btQuery);
bulletdelete(btConvex);
@@ -634,7 +628,7 @@ void SpaceBullet::destroy_world() {
void SpaceBullet::check_ghost_overlaps() {
- /// Algorith support variables
+ /// Algorithm support variables
btConvexShape *other_body_shape;
btConvexShape *area_shape;
btGjkPairDetector::ClosestPointInput gjk_input;
@@ -666,7 +660,10 @@ void SpaceBullet::check_ghost_overlaps() {
// For each overlapping
for (i = ghostOverlaps.size() - 1; 0 <= i; --i) {
- if (!(ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_RIGID_BODY || ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_AREA))
+ if (ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_AREA) {
+ if (!static_cast<AreaBullet *>(ghostOverlaps[i]->getUserPointer())->is_monitorable())
+ continue;
+ } else if (ghostOverlaps[i]->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY)
continue;
otherObject = static_cast<RigidCollisionObjectBullet *>(ghostOverlaps[i]->getUserPointer());
@@ -796,7 +793,9 @@ void SpaceBullet::update_gravity() {
/// I'm leaving this here just for future tests.
/// Debug motion and normal vector drawing
#define debug_test_motion 0
-#define PERFORM_INITIAL_UNSTACK 1
+
+#define RECOVERING_MOVEMENT_SCALE 0.4
+#define RECOVERING_MOVEMENT_CYCLES 4
#if debug_test_motion
@@ -808,8 +807,7 @@ static Ref<SpatialMaterial> red_mat;
static Ref<SpatialMaterial> blue_mat;
#endif
-#define IGNORE_AREAS_TRUE true
-bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, PhysicsServer::MotionResult *r_result) {
+bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result) {
#if debug_test_motion
/// Yes I know this is not good, but I've used it as fast debugging hack.
@@ -820,6 +818,9 @@ 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);
+
red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
red_mat->set_line_width(20.0);
@@ -840,37 +841,29 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
}
#endif
- ///// Release all generated manifolds
- //{
- // if(p_body->get_kinematic_utilities()){
- // for(int i= p_body->get_kinematic_utilities()->m_generatedManifold.size()-1; 0<=i; --i){
- // dispatcher->releaseManifold( p_body->get_kinematic_utilities()->m_generatedManifold[i] );
- // }
- // p_body->get_kinematic_utilities()->m_generatedManifold.clear();
- // }
- //}
-
- btVector3 recover_initial_position(0, 0, 0);
-
btTransform body_safe_position;
G_TO_B(p_from, body_safe_position);
+ UNSCALE_BT_BASIS(body_safe_position);
+ btVector3 recover_initial_position(0, 0, 0);
{ /// Phase one - multi shapes depenetration using margin
-#if PERFORM_INITIAL_UNSTACK
- if (recover_from_penetration(p_body, body_safe_position, recover_initial_position)) {
-
- // Add recover position to "From" and "To" transforms
- body_safe_position.getOrigin() += recover_initial_position;
+ for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
+ if (!recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, recover_initial_position)) {
+ break;
+ }
}
-#endif
+
+ // Add recover movement in order to make it safe
+ body_safe_position.getOrigin() += recover_initial_position;
}
- btVector3 recovered_motion;
- G_TO_B(p_motion, recovered_motion);
- const int shape_count(p_body->get_shape_count());
+ btVector3 motion;
+ G_TO_B(p_motion, motion);
{ /// phase two - sweep test, from a secure position without margin
+ const int shape_count(p_body->get_shape_count());
+
#if debug_test_motion
Vector3 sup_line;
B_TO_G(body_safe_position.getOrigin(), sup_line);
@@ -892,82 +885,85 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
}
btConvexShape *convex_shape_test(static_cast<btConvexShape *>(p_body->get_bt_shape(shIndex)));
- btTransform shape_world_from;
- G_TO_B(p_body->get_shape_transform(shIndex), shape_world_from);
-
- // Add local shape transform
- shape_world_from = body_safe_position * shape_world_from;
+ btTransform shape_world_from = body_safe_position * p_body->get_kinematic_utilities()->shapes[shIndex].transform;
btTransform shape_world_to(shape_world_from);
- shape_world_to.getOrigin() += recovered_motion;
+ shape_world_to.getOrigin() += motion;
- GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, IGNORE_AREAS_TRUE);
+ GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia);
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, 0.002);
+ dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
if (btResult.hasHit()) {
/// 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
- recovered_motion *= btResult.m_closestHitFraction;
+ motion *= btResult.m_closestHitFraction;
}
}
+
+ body_safe_position.getOrigin() += motion;
}
- bool hasPenetration = false;
+ bool has_penetration = false;
{ /// Phase three - Recover + contact test with margin
+ btVector3 delta_recover_movement(0, 0, 0);
RecoverResult r_recover_result;
+ bool l_has_penetration;
+ real_t l_penetration_distance = 1e20;
- hasPenetration = recover_from_penetration(p_body, body_safe_position, recovered_motion, &r_recover_result);
+ for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
+ l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, delta_recover_movement, &r_recover_result);
- if (r_result) {
+ if (r_result) {
+ B_TO_G(motion + delta_recover_movement + recover_initial_position, r_result->motion);
- B_TO_G(recovered_motion + recover_initial_position, r_result->motion);
+ if (l_has_penetration) {
+ has_penetration = true;
+ if (l_penetration_distance <= r_recover_result.penetration_distance) {
+ continue;
+ }
- if (hasPenetration) {
- const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
- CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
+ l_penetration_distance = r_recover_result.penetration_distance;
- r_result->remainder = p_motion - r_result->motion; // is the remaining movements
- 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 = r_recover_result.other_compound_shape_index;
- r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
+ const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
+ CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
- //{ /// Add manifold point to manage collisions
- // btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid);
- // btManifoldPoint manifoldPoint(result_callabck.m_pointWorld, result_callabck.m_pointWorld, result_callabck.m_pointNormalWorld, result_callabck.m_penetration_distance);
- // manifoldPoint.m_index0 = r_result->collision_local_shape;
- // manifoldPoint.m_index1 = r_result->collider_shape;
- // manifold->addManifoldPoint(manifoldPoint);
- // p_body->get_kinematic_utilities()->m_generatedManifold.push_back(manifold);
- //}
+ B_TO_G(motion, r_result->remainder); // is the remaining movements
+ r_result->remainder = p_motion - r_result->remainder;
+ B_TO_G(r_recover_result.pointWorld, r_result->collision_point);
+ B_TO_G(r_recover_result.normal, 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 = r_recover_result.other_compound_shape_index;
+ r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
#if debug_test_motion
- Vector3 sup_line2;
- B_TO_G(recovered_motion, sup_line2);
- //Vector3 sup_pos;
- //B_TO_G( pt.getPositionWorldOnB(), sup_pos);
- normalLine->clear();
- normalLine->begin(Mesh::PRIMITIVE_LINES, NULL);
- normalLine->add_vertex(r_result->collision_point);
- normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
- normalLine->end();
+ Vector3 sup_line2;
+ B_TO_G(motion, sup_line2);
+ normalLine->clear();
+ normalLine->begin(Mesh::PRIMITIVE_LINES, NULL);
+ normalLine->add_vertex(r_result->collision_point);
+ normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
+ normalLine->end();
#endif
-
+ } else {
+ r_result->remainder = Vector3();
+ }
} else {
- r_result->remainder = Vector3();
+ if (!l_has_penetration)
+ break;
+ else
+ has_penetration = true;
}
}
}
- return hasPenetration;
+ return has_penetration;
}
struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
@@ -1000,11 +996,11 @@ public:
}
void reset() {
- result_collision_objects.empty();
+ result_collision_objects.clear();
}
};
-bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btVector3 &r_recover_position, 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());
@@ -1028,14 +1024,17 @@ 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() += r_recover_position;
+ body_shape_position_recovered.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];
- if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()))
+ 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()))
continue;
if (otherObject->getCollisionShape()->isCompound()) {
@@ -1045,24 +1044,24 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
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), r_recover_position, r_recover_result)) {
+ 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(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), r_recover_position, r_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, 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, 0, body_shape_position, otherObject->getWorldTransform(), r_recover_position, r_recover_result)) {
+ 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)) {
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 (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)) {
penetration = true;
}
@@ -1073,12 +1072,12 @@ 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) {
+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, 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;
- gjk_input.m_transformA.getOrigin() += r_recover_position;
+ gjk_input.m_transformA.getOrigin() += r_delta_recover_movement;
gjk_input.m_transformB = p_transformB;
// Perform GJK test
@@ -1087,33 +1086,34 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
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);
+ r_delta_recover_movement += result.m_normalOnBInWorld * (result.m_distance * -1 * p_recover_movement_scale);
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;
+ if (result.m_distance < r_recover_result->penetration_distance) {
+ 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->pointWorld = result.m_pointInWorld;
+ r_recover_result->normal = result.m_normalOnBInWorld;
+ }
}
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) {
+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 p_recovered_transformA(p_transformA);
- p_recovered_transformA.getOrigin() += r_recover_position;
+ btTransform tA(p_transformA);
+ tA.getOrigin() += r_delta_recover_movement;
- btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, p_recovered_transformA, -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);
- btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CLOSEST_POINT_ALGORITHMS);
+ btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CONTACT_POINT_ALGORITHMS);
if (algorithm) {
GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB);
//discrete collision detection query
@@ -1123,16 +1123,17 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
dispatcher->freeCollisionAlgorithm(algorithm);
if (contactPointResult.hasHit()) {
- r_recover_position += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1);
+ r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale);
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;
+ if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) {
+ 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->pointWorld = contactPointResult.m_pointWorld;
+ r_recover_result->normal = contactPointResult.m_pointNormalWorld;
+ }
}
return true;
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-29 01:45:06 +0000