Skip to content

Commit

Permalink
Improved kinematic body 2D and 3D, Now can move rigid body
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndreaCatania
AndreaCatania committed Feb 20, 2018
1 parent da612c3 commit 6ed392f
Show file tree
Hide file tree
Showing 22 changed files with 96 additions and 88 deletions.
4 changes: 2 additions & 2 deletions modules/bullet/bullet_physics_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -825,12 +825,12 @@ PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) {
return BulletPhysicsDirectBodyState::get_singleton(body);
}

bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result) {
bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result) {
RigidBodyBullet *body = rigid_body_owner.get(p_body);
ERR_FAIL_COND_V(!body, false);
ERR_FAIL_COND_V(!body->get_space(), false);

return body->get_space()->test_body_motion(body, p_from, p_motion, r_result);
return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result);
}

RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) {
Expand Down
2 changes: 1 addition & 1 deletion modules/bullet/bullet_physics_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -253,7 +253,7 @@ class BulletPhysicsServer : public PhysicsServer {
// this function only works on physics process, errors and returns null otherwise
virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);

virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result = NULL);
virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL);

/* SOFT BODY API */

Expand Down
11 changes: 8 additions & 3 deletions modules/bullet/godot_result_callbacks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,11 +98,16 @@ bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *prox
if (gObj == m_self_object) {
return false;
} else {
if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {

// A kinematic body can't be stopped by a rigid body since the mass of kinematic body is infinite
if (m_infinite_inertia && !btObj->isStaticOrKinematicObject())
return false;

if (gObj->getType() == CollisionObjectBullet::TYPE_AREA)
return false;
} else if (m_self_object->has_collision_exception(gObj)) {

if (m_self_object->has_collision_exception(gObj))
return false;
}
}
return true;
} else {
Expand Down
6 changes: 3 additions & 3 deletions modules/bullet/godot_result_callbacks.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,12 +93,12 @@ struct GodotAllConvexResultCallback : public btCollisionWorld::ConvexResultCallb
struct GodotKinClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
public:
const RigidBodyBullet *m_self_object;
const bool m_ignore_areas;
const bool m_infinite_inertia;

GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_ignore_areas) :
GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_infinite_inertia) :
btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld),
m_self_object(p_self_object),
m_ignore_areas(p_ignore_areas) {}
m_infinite_inertia(p_infinite_inertia) {}

virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
};
Expand Down
32 changes: 6 additions & 26 deletions modules/bullet/space_bullet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -804,8 +804,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.
Expand Down Expand Up @@ -839,16 +838,6 @@ 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();
// }
//}

btTransform body_safe_position;
G_TO_B(p_from, body_safe_position);
UNSCALE_BT_BASIS(body_safe_position);
Expand All @@ -857,7 +846,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
btVector3 recover_initial_position(0, 0, 0);
{ /// Phase one - multi shapes depenetration using margin
for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
if (!recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, recover_initial_position)) {
if (!recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, recover_initial_position)) {
break;
}
}
Expand Down Expand Up @@ -900,7 +889,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
btTransform shape_world_to(shape_world_from);
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();

Expand All @@ -926,7 +915,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
real_t l_penetration_distance = 1e20;

for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, delta_recover_movement, &r_recover_result);
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 PERFORM_INITIAL_UNSTACK
Expand Down Expand Up @@ -955,15 +944,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
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);
// 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);
//}

#if debug_test_motion
Vector3 sup_line2;
B_TO_G(motion, sup_line2);
Expand Down Expand Up @@ -1022,7 +1002,7 @@ struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
}
};

bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, 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());

Expand Down Expand Up @@ -1053,7 +1033,7 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran

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_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()) || (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()))
continue;

if (otherObject->getCollisionShape()->isCompound()) {
Expand Down
4 changes: 2 additions & 2 deletions modules/bullet/space_bullet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ class SpaceBullet : public RIDBullet {

void update_gravity();

bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, PhysicsServer::MotionResult *r_result);
bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result);

private:
void create_empty_world(bool p_create_soft_world);
Expand All @@ -199,7 +199,7 @@ class SpaceBullet : public RIDBullet {
local_shape_most_recovered(0) {}
};

bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL);
bool 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 = 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, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL);
Expand Down
27 changes: 14 additions & 13 deletions scene/2d/physics_body_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

#include "physics_body_2d.h"

#include "core/method_bind_ext.gen.inc"
#include "engine.h"
#include "scene/scene_string_names.h"

Expand Down Expand Up @@ -362,12 +363,12 @@ struct _RigidBody2DInOut {
int local_shape;
};

bool RigidBody2D::_test_motion(const Vector2 &p_motion, float p_margin, const Ref<Physics2DTestMotionResult> &p_result) {
bool RigidBody2D::_test_motion(const Vector2 &p_motion, bool p_infinite_inertia, float p_margin, const Ref<Physics2DTestMotionResult> &p_result) {

Physics2DServer::MotionResult *r = NULL;
if (p_result.is_valid())
r = p_result->get_result_ptr();
return Physics2DServer::get_singleton()->body_test_motion(get_rid(), get_global_transform(), p_motion, p_margin, r);
return Physics2DServer::get_singleton()->body_test_motion(get_rid(), get_global_transform(), p_motion, p_infinite_inertia, p_margin, r);
}

void RigidBody2D::_direct_state_changed(Object *p_state) {
Expand Down Expand Up @@ -972,11 +973,11 @@ RigidBody2D::~RigidBody2D() {

//////////////////////////

Ref<KinematicCollision2D> KinematicBody2D::_move(const Vector2 &p_motion) {
Ref<KinematicCollision2D> KinematicBody2D::_move(const Vector2 &p_motion, bool p_infinite_inertia) {

Collision col;

if (move_and_collide(p_motion, col)) {
if (move_and_collide(p_motion, p_infinite_inertia, col)) {
if (motion_cache.is_null()) {
motion_cache.instance();
motion_cache->owner = this;
Expand All @@ -990,11 +991,11 @@ Ref<KinematicCollision2D> KinematicBody2D::_move(const Vector2 &p_motion) {
return Ref<KinematicCollision2D>();
}

bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, Collision &r_collision) {
bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision) {

Transform2D gt = get_global_transform();
Physics2DServer::MotionResult result;
bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, margin, &result);
bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, margin, &result);

if (colliding) {
r_collision.collider_metadata = result.collider_metadata;
Expand All @@ -1014,7 +1015,7 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, Collision &r_col
return colliding;
}

Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle) {
Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, bool p_infinite_inertia, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle) {

Vector2 motion = (floor_velocity + p_linear_velocity) * get_physics_process_delta_time();
Vector2 lv = p_linear_velocity;
Expand All @@ -1029,7 +1030,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const

Collision collision;

bool collided = move_and_collide(motion, collision);
bool collided = move_and_collide(motion, p_infinite_inertia, collision);

if (collided) {

Expand Down Expand Up @@ -1096,11 +1097,11 @@ Vector2 KinematicBody2D::get_floor_velocity() const {
return floor_velocity;
}

bool KinematicBody2D::test_move(const Transform2D &p_from, const Vector2 &p_motion) {
bool KinematicBody2D::test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia) {

ERR_FAIL_COND_V(!is_inside_tree(), false);

return Physics2DServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, margin);
return Physics2DServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia, margin);
}

void KinematicBody2D::set_safe_margin(float p_margin) {
Expand Down Expand Up @@ -1141,10 +1142,10 @@ Ref<KinematicCollision2D> KinematicBody2D::_get_slide_collision(int p_bounce) {

void KinematicBody2D::_bind_methods() {

ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec"), &KinematicBody2D::_move);
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia"), &KinematicBody2D::_move, DEFVAL(true));
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "infinite_inertia", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));

ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec"), &KinematicBody2D::test_move);
ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody2D::test_move);

ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody2D::is_on_floor);
ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody2D::is_on_ceiling);
Expand Down
10 changes: 5 additions & 5 deletions scene/2d/physics_body_2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,7 @@ class RigidBody2D : public PhysicsBody2D {
void _body_inout(int p_status, ObjectID p_instance, int p_body_shape, int p_local_shape);
void _direct_state_changed(Object *p_state);

bool _test_motion(const Vector2 &p_motion, float p_margin = 0.08, const Ref<Physics2DTestMotionResult> &p_result = Ref<Physics2DTestMotionResult>());
bool _test_motion(const Vector2 &p_motion, bool p_infinite_inertia = true, float p_margin = 0.08, const Ref<Physics2DTestMotionResult> &p_result = Ref<Physics2DTestMotionResult>());

protected:
void _notification(int p_what);
Expand Down Expand Up @@ -296,20 +296,20 @@ class KinematicBody2D : public PhysicsBody2D {

_FORCE_INLINE_ bool _ignores_mode(Physics2DServer::BodyMode) const;

Ref<KinematicCollision2D> _move(const Vector2 &p_motion);
Ref<KinematicCollision2D> _move(const Vector2 &p_motion, bool p_infinite_inertia = true);
Ref<KinematicCollision2D> _get_slide_collision(int p_bounce);

protected:
static void _bind_methods();

public:
bool move_and_collide(const Vector2 &p_motion, Collision &r_collision);
bool test_move(const Transform2D &p_from, const Vector2 &p_motion);
bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision);
bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia);

void set_safe_margin(float p_margin);
float get_safe_margin() const;

Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), float p_slope_stop_min_velocity = 5, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45));
Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, float p_slope_stop_min_velocity = 5, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45));
bool is_on_floor() const;
bool is_on_wall() const;
bool is_on_ceiling() const;
Expand Down
Loading

0 comments on commit 6ed392f

Please sign in to comment.