Merge pull request #20908 from AndreaCatania/kiSlope
Improved move_and_slide function stay on slope
This commit is contained in:
Juan Linietsky
GitHub
commit
1b66b08fdb
11 changed files with 345 additions and 73 deletions
|
|
@ -869,6 +869,14 @@ bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from,
|
|||
return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result);
|
||||
}
|
||||
|
||||
int BulletPhysicsServer::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) {
|
||||
RigidBodyBullet *body = rigid_body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body, 0);
|
||||
ERR_FAIL_COND_V(!body->get_space(), 0);
|
||||
|
||||
return body->get_space()->test_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
|
||||
}
|
||||
|
||||
RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) {
|
||||
SoftBodyBullet *body = bulletnew(SoftBodyBullet);
|
||||
body->set_collision_layer(1);
|
||||
|
|
|
|||
|
|
@ -259,6 +259,7 @@ public:
|
|||
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, bool p_infinite_inertia, MotionResult *r_result = NULL);
|
||||
virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001);
|
||||
|
||||
/* SOFT BODY API */
|
||||
|
||||
|
|
|
|||
|
|
@ -260,10 +260,19 @@ void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3
|
|||
|
||||
if (m_penetration_distance > depth) { // Has penetration?
|
||||
|
||||
bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
|
||||
const bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
|
||||
m_penetration_distance = depth;
|
||||
m_other_compound_shape_index = isSwapped ? m_index0 : m_index1;
|
||||
m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
|
||||
m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB;
|
||||
|
||||
const btCollisionObjectWrapper *bw0 = m_body0Wrap;
|
||||
if (isSwapped)
|
||||
bw0 = m_body1Wrap;
|
||||
|
||||
if (bw0->getCollisionShape()->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
|
||||
m_pointNormalWorld = bw0->m_worldTransform.getBasis().transpose() * btVector3(0, 0, 1);
|
||||
} else {
|
||||
m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -894,6 +894,12 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
|
|||
// Skip no convex shape
|
||||
continue;
|
||||
}
|
||||
|
||||
if (p_body->get_bt_shape(shIndex)->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
|
||||
// Skip rayshape in order to implement custom separation process
|
||||
continue;
|
||||
}
|
||||
|
||||
btConvexShape *convex_shape_test(static_cast<btConvexShape *>(p_body->get_bt_shape(shIndex)));
|
||||
|
||||
btTransform shape_world_from = body_transform * p_body->get_kinematic_utilities()->shapes[shIndex].transform;
|
||||
|
|
@ -924,11 +930,11 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
|
|||
btVector3 __rec(0, 0, 0);
|
||||
RecoverResult r_recover_result;
|
||||
|
||||
has_penetration = recover_from_penetration(p_body, body_transform, 0, p_infinite_inertia, __rec, &r_recover_result);
|
||||
has_penetration = recover_from_penetration(p_body, body_transform, 1, p_infinite_inertia, __rec, &r_recover_result);
|
||||
|
||||
// Parse results
|
||||
if (r_result) {
|
||||
B_TO_G(motion + initial_recover_motion, r_result->motion);
|
||||
B_TO_G(motion + initial_recover_motion + __rec, r_result->motion);
|
||||
|
||||
if (has_penetration) {
|
||||
|
||||
|
|
@ -964,6 +970,39 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
|
|||
return has_penetration;
|
||||
}
|
||||
|
||||
int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, float p_margin) {
|
||||
|
||||
btTransform body_transform;
|
||||
G_TO_B(p_transform, body_transform);
|
||||
UNSCALE_BT_BASIS(body_transform);
|
||||
|
||||
btVector3 recover_motion(0, 0, 0);
|
||||
|
||||
int rays_found;
|
||||
|
||||
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);
|
||||
|
||||
rays_found = MAX(last_ray_index, rays_found);
|
||||
if (!rays_found) {
|
||||
break;
|
||||
} else {
|
||||
body_transform.getOrigin() += recover_motion;
|
||||
}
|
||||
}
|
||||
|
||||
//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]);
|
||||
}
|
||||
}
|
||||
|
||||
B_TO_G(recover_motion, r_recover_motion);
|
||||
return rays_found;
|
||||
}
|
||||
|
||||
struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
|
||||
private:
|
||||
const btCollisionObject *self_collision_object;
|
||||
|
|
@ -1020,6 +1059,11 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
|
|||
continue;
|
||||
}
|
||||
|
||||
if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
|
||||
// Skip rayshape in order to implement custom separation process
|
||||
continue;
|
||||
}
|
||||
|
||||
body_shape_position = p_body_position * kin_shape.transform;
|
||||
body_shape_position_recovered = body_shape_position;
|
||||
body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
|
||||
|
|
@ -1122,7 +1166,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
|
|||
|
||||
if (contactPointResult.hasHit()) {
|
||||
r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale);
|
||||
|
||||
if (r_recover_result) {
|
||||
if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) {
|
||||
r_recover_result->hasPenetration = true;
|
||||
|
|
@ -1138,3 +1181,79 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
|
|||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
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());
|
||||
|
||||
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) {
|
||||
|
||||
recover_broad_result.reset();
|
||||
|
||||
if (ray_index >= p_result_max) {
|
||||
break;
|
||||
}
|
||||
|
||||
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) {
|
||||
continue;
|
||||
}
|
||||
|
||||
body_shape_position = p_body_position * kin_shape.transform;
|
||||
body_shape_position_recovered = body_shape_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_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()) {
|
||||
|
||||
// Each convex shape
|
||||
btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
|
||||
for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
|
||||
|
||||
RecoverResult 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)) {
|
||||
|
||||
const btRigidBody *btRigid = static_cast<const btRigidBody *>(otherObject);
|
||||
CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(otherObject->getUserPointer());
|
||||
|
||||
r_results[ray_index].collision_depth = r_recover_result.penetration_distance;
|
||||
B_TO_G(r_recover_result.pointWorld, r_results[ray_index].collision_point);
|
||||
B_TO_G(r_recover_result.normal, r_results[ray_index].collision_normal);
|
||||
B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_results[ray_index].collider_velocity);
|
||||
r_results[ray_index].collision_local_shape = kinIndex;
|
||||
r_results[ray_index].collider_id = collisionObject->get_instance_id();
|
||||
r_results[ray_index].collider = collisionObject->get_self();
|
||||
r_results[ray_index].collider_shape = r_recover_result.other_compound_shape_index;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
++ray_index;
|
||||
}
|
||||
|
||||
return ray_index;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -175,6 +175,7 @@ public:
|
|||
void update_gravity();
|
||||
|
||||
bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result);
|
||||
int test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, float p_margin);
|
||||
|
||||
private:
|
||||
void create_empty_world(bool p_create_soft_world);
|
||||
|
|
@ -208,5 +209,7 @@ private:
|
|||
/// 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, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL);
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -1207,7 +1207,7 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_
|
|||
//so, if you pass 45 as limit, avoid numerical precision erros when angle is 45.
|
||||
#define FLOOR_ANGLE_THRESHOLD 0.01
|
||||
|
||||
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 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, bool p_infinite_inertia, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle) {
|
||||
|
||||
Vector2 floor_motion = floor_velocity;
|
||||
if (on_floor && on_floor_body.is_valid()) {
|
||||
|
|
@ -1228,6 +1228,8 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
|
|||
colliders.clear();
|
||||
floor_velocity = Vector2();
|
||||
|
||||
Vector2 lv_n = p_linear_velocity.normalized();
|
||||
|
||||
while (p_max_slides) {
|
||||
|
||||
Collision collision;
|
||||
|
|
@ -1254,8 +1256,10 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
|
|||
|
||||
if (collided) {
|
||||
|
||||
colliders.push_back(collision);
|
||||
motion = collision.remainder;
|
||||
|
||||
bool is_on_slope = false;
|
||||
if (p_floor_direction == Vector2()) {
|
||||
//all is a wall
|
||||
on_wall = true;
|
||||
|
|
@ -1266,15 +1270,17 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
|
|||
on_floor_body = collision.collider_rid;
|
||||
floor_velocity = collision.collider_vel;
|
||||
|
||||
Vector2 rel_v = lv - floor_velocity;
|
||||
Vector2 hv = rel_v - p_floor_direction * p_floor_direction.dot(rel_v);
|
||||
|
||||
if (collision.travel.length() < 1 && hv.length() < p_slope_stop_min_velocity) {
|
||||
Transform2D gt = get_global_transform();
|
||||
gt.elements[2] -= collision.travel;
|
||||
set_global_transform(gt);
|
||||
return Vector2();
|
||||
if (p_stop_on_slope) {
|
||||
if (Vector2() == lv_n + p_floor_direction) {
|
||||
Transform2D gt = get_global_transform();
|
||||
gt.elements[2] -= collision.travel;
|
||||
set_global_transform(gt);
|
||||
return Vector2();
|
||||
}
|
||||
}
|
||||
|
||||
is_on_slope = true;
|
||||
|
||||
} else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling
|
||||
on_ceiling = true;
|
||||
} else {
|
||||
|
|
@ -1282,12 +1288,18 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
|
|||
}
|
||||
}
|
||||
|
||||
Vector2 n = collision.normal;
|
||||
motion = motion.slide(n);
|
||||
lv = lv.slide(n);
|
||||
|
||||
colliders.push_back(collision);
|
||||
if (p_stop_on_slope && is_on_slope) {
|
||||
motion = motion.slide(p_floor_direction);
|
||||
lv = lv.slide(p_floor_direction);
|
||||
} else {
|
||||
Vector2 n = collision.normal;
|
||||
motion = motion.slide(n);
|
||||
lv = lv.slide(n);
|
||||
}
|
||||
}
|
||||
|
||||
if (p_stop_on_slope)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!found_collision) {
|
||||
|
|
@ -1301,11 +1313,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
|
|||
return lv;
|
||||
}
|
||||
|
||||
Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, 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 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction, bool p_infinite_inertia, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle) {
|
||||
|
||||
bool was_on_floor = on_floor;
|
||||
|
||||
Vector2 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_infinite_inertia, p_slope_stop_min_velocity, p_max_slides, p_floor_max_angle);
|
||||
Vector2 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_infinite_inertia, p_stop_on_slope, p_max_slides, p_floor_max_angle);
|
||||
if (!was_on_floor || p_snap == Vector2()) {
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -1439,8 +1451,8 @@ void KinematicBody2D::_notification(int p_what) {
|
|||
void KinematicBody2D::_bind_methods() {
|
||||
|
||||
ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody2D::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false));
|
||||
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("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "infinite_inertia", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide_with_snap, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
|
||||
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "infinite_inertia", "stop_on_slope", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
|
||||
ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "infinite_inertia", "stop_on_slope", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide_with_snap, DEFVAL(Vector2(0, 0)), DEFVAL(true), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
|
||||
|
||||
ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody2D::test_move);
|
||||
|
||||
|
|
|
|||
|
|
@ -338,8 +338,8 @@ public:
|
|||
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), 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));
|
||||
Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, 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));
|
||||
Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45));
|
||||
Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_infinite_inertia = true, bool p_stop_on_slope = false, 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;
|
||||
|
|
|
|||
|
|
@ -1078,10 +1078,10 @@ void RigidBody::_reload_physics_characteristics() {
|
|||
//////////////////////////////////////////////////////
|
||||
//////////////////////////
|
||||
|
||||
Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_infinite_inertia) {
|
||||
Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_infinite_inertia, bool p_test_only) {
|
||||
|
||||
Collision col;
|
||||
if (move_and_collide(p_motion, p_infinite_inertia, col)) {
|
||||
if (move_and_collide(p_motion, p_infinite_inertia, col, p_test_only)) {
|
||||
if (motion_cache.is_null()) {
|
||||
motion_cache.instance();
|
||||
motion_cache->owner = this;
|
||||
|
|
@ -1095,7 +1095,7 @@ Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_inf
|
|||
return Ref<KinematicCollision>();
|
||||
}
|
||||
|
||||
bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision) {
|
||||
bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_test_only) {
|
||||
|
||||
Transform gt = get_global_transform();
|
||||
PhysicsServer::MotionResult result;
|
||||
|
|
@ -1108,6 +1108,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
|
|||
r_collision.collision = result.collision_point;
|
||||
r_collision.normal = result.collision_normal;
|
||||
r_collision.collider = result.collider_id;
|
||||
r_collision.collider_rid = result.collider;
|
||||
r_collision.travel = result.motion;
|
||||
r_collision.remainder = result.remainder;
|
||||
r_collision.local_shape = result.collision_local_shape;
|
||||
|
|
@ -1119,8 +1120,10 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
|
|||
}
|
||||
}
|
||||
|
||||
gt.origin += result.motion;
|
||||
set_global_transform(gt);
|
||||
if (!p_test_only) {
|
||||
gt.origin += result.motion;
|
||||
set_global_transform(gt);
|
||||
}
|
||||
|
||||
return colliding;
|
||||
}
|
||||
|
|
@ -1128,7 +1131,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
|
|||
//so, if you pass 45 as limit, avoid numerical precision erros when angle is 45.
|
||||
#define FLOOR_ANGLE_THRESHOLD 0.01
|
||||
|
||||
Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, float p_slope_stop_min_velocity, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
|
||||
Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
|
||||
|
||||
Vector3 lv = p_linear_velocity;
|
||||
|
||||
|
|
@ -1146,69 +1149,127 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
|
|||
colliders.clear();
|
||||
floor_velocity = Vector3();
|
||||
|
||||
Vector3 lv_n = p_linear_velocity.normalized();
|
||||
|
||||
while (p_max_slides) {
|
||||
|
||||
Collision collision;
|
||||
|
||||
bool collided = move_and_collide(motion, p_infinite_inertia, collision);
|
||||
bool found_collision = false;
|
||||
|
||||
if (collided) {
|
||||
int test_type = 0;
|
||||
|
||||
motion = collision.remainder;
|
||||
|
||||
if (p_floor_direction == Vector3()) {
|
||||
//all is a wall
|
||||
on_wall = true;
|
||||
do {
|
||||
bool collided;
|
||||
if (test_type == 0) { //collide
|
||||
collided = move_and_collide(motion, p_infinite_inertia, collision);
|
||||
if (!collided) {
|
||||
motion = Vector3(); //clear because no collision happened and motion completed
|
||||
}
|
||||
} else {
|
||||
if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //floor
|
||||
collided = separate_raycast_shapes(p_infinite_inertia, collision);
|
||||
if (collided) {
|
||||
collision.remainder = motion; //keep
|
||||
collision.travel = Vector3();
|
||||
}
|
||||
}
|
||||
|
||||
on_floor = true;
|
||||
floor_velocity = collision.collider_vel;
|
||||
if (collided) {
|
||||
found_collision = true;
|
||||
}
|
||||
|
||||
Vector3 rel_v = lv - floor_velocity;
|
||||
Vector3 hv = rel_v - p_floor_direction * p_floor_direction.dot(rel_v);
|
||||
if (collided) {
|
||||
|
||||
if (collision.travel.length() < 0.05 && hv.length() < p_slope_stop_min_velocity) {
|
||||
Transform gt = get_global_transform();
|
||||
gt.origin -= collision.travel;
|
||||
set_global_transform(gt);
|
||||
return floor_velocity - p_floor_direction * p_floor_direction.dot(floor_velocity);
|
||||
}
|
||||
} else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling
|
||||
on_ceiling = true;
|
||||
} else {
|
||||
colliders.push_back(collision);
|
||||
motion = collision.remainder;
|
||||
|
||||
bool is_on_slope = false;
|
||||
if (p_floor_direction == Vector3()) {
|
||||
//all is a wall
|
||||
on_wall = true;
|
||||
} else {
|
||||
if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //floor
|
||||
|
||||
on_floor = true;
|
||||
on_floor_body = collision.collider_rid;
|
||||
floor_velocity = collision.collider_vel;
|
||||
|
||||
if (p_stop_on_slope) {
|
||||
if (Vector3() == lv_n + p_floor_direction) {
|
||||
Transform gt = get_global_transform();
|
||||
gt.origin -= collision.travel;
|
||||
set_global_transform(gt);
|
||||
return Vector3();
|
||||
}
|
||||
}
|
||||
|
||||
is_on_slope = true;
|
||||
|
||||
} else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle + FLOOR_ANGLE_THRESHOLD)) { //ceiling
|
||||
on_ceiling = true;
|
||||
} else {
|
||||
on_wall = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (p_stop_on_slope && is_on_slope) {
|
||||
motion = motion.slide(p_floor_direction);
|
||||
lv = lv.slide(p_floor_direction);
|
||||
} else {
|
||||
Vector3 n = collision.normal;
|
||||
motion = motion.slide(n);
|
||||
lv = lv.slide(n);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (locked_axis & (1 << i)) {
|
||||
lv[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 n = collision.normal;
|
||||
motion = motion.slide(n);
|
||||
lv = lv.slide(n);
|
||||
++test_type;
|
||||
} while (!p_stop_on_slope && test_type < 2);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (locked_axis & (1 << i)) {
|
||||
lv[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
colliders.push_back(collision);
|
||||
|
||||
} else {
|
||||
if (!found_collision || motion == Vector3())
|
||||
break;
|
||||
}
|
||||
|
||||
p_max_slides--;
|
||||
if (motion == Vector3())
|
||||
break;
|
||||
--p_max_slides;
|
||||
}
|
||||
|
||||
return lv;
|
||||
}
|
||||
|
||||
Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction, bool p_infinite_inertia, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle) {
|
||||
|
||||
bool was_on_floor = on_floor;
|
||||
|
||||
Vector3 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
|
||||
if (!was_on_floor || p_snap == Vector3()) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
Collision col;
|
||||
Transform gt = get_global_transform();
|
||||
|
||||
if (move_and_collide(p_snap, p_infinite_inertia, col, true)) {
|
||||
gt.origin += col.travel;
|
||||
if (p_floor_direction != Vector3() && Math::acos(p_floor_direction.normalized().dot(col.normal)) < p_floor_max_angle) {
|
||||
on_floor = true;
|
||||
on_floor_body = col.collider_rid;
|
||||
floor_velocity = col.collider_vel;
|
||||
}
|
||||
set_global_transform(gt);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool KinematicBody::is_on_floor() const {
|
||||
|
||||
return on_floor;
|
||||
}
|
||||
|
||||
bool KinematicBody::is_on_wall() const {
|
||||
|
||||
return on_wall;
|
||||
|
|
@ -1230,6 +1291,43 @@ bool KinematicBody::test_move(const Transform &p_from, const Vector3 &p_motion,
|
|||
return PhysicsServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, p_infinite_inertia);
|
||||
}
|
||||
|
||||
bool KinematicBody::separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision) {
|
||||
|
||||
PhysicsServer::SeparationResult sep_res[8]; //max 8 rays
|
||||
|
||||
Transform gt = get_global_transform();
|
||||
|
||||
Vector3 recover;
|
||||
int hits = PhysicsServer::get_singleton()->body_test_ray_separation(get_rid(), gt, p_infinite_inertia, recover, sep_res, 8, margin);
|
||||
int deepest = -1;
|
||||
float deepest_depth;
|
||||
for (int i = 0; i < hits; i++) {
|
||||
if (deepest == -1 || sep_res[i].collision_depth > deepest_depth) {
|
||||
deepest = i;
|
||||
deepest_depth = sep_res[i].collision_depth;
|
||||
}
|
||||
}
|
||||
|
||||
gt.origin += recover;
|
||||
set_global_transform(gt);
|
||||
|
||||
if (deepest != -1) {
|
||||
r_collision.collider = sep_res[deepest].collider_id;
|
||||
r_collision.collider_metadata = sep_res[deepest].collider_metadata;
|
||||
r_collision.collider_shape = sep_res[deepest].collider_shape;
|
||||
r_collision.collider_vel = sep_res[deepest].collider_velocity;
|
||||
r_collision.collision = sep_res[deepest].collision_point;
|
||||
r_collision.normal = sep_res[deepest].collision_normal;
|
||||
r_collision.local_shape = sep_res[deepest].collision_local_shape;
|
||||
r_collision.travel = recover;
|
||||
r_collision.remainder = Vector3();
|
||||
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void KinematicBody::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock) {
|
||||
PhysicsServer::get_singleton()->body_set_axis_lock(get_rid(), p_axis, p_lock);
|
||||
}
|
||||
|
|
@ -1276,8 +1374,9 @@ Ref<KinematicCollision> KinematicBody::_get_slide_collision(int p_bounce) {
|
|||
|
||||
void KinematicBody::_bind_methods() {
|
||||
|
||||
ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia"), &KinematicBody::_move, DEFVAL(true));
|
||||
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(0.05), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
|
||||
ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "test_only"), &KinematicBody::_move, DEFVAL(true), DEFVAL(false));
|
||||
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
|
||||
ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "infinite_inertia", "stop_on_slope", "max_bounces", "floor_max_angle"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(true), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
|
||||
|
||||
ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody::test_move);
|
||||
|
||||
|
|
|
|||
|
|
@ -285,6 +285,7 @@ public:
|
|||
Vector3 normal;
|
||||
Vector3 collider_vel;
|
||||
ObjectID collider;
|
||||
RID collider_rid;
|
||||
int collider_shape;
|
||||
Variant collider_metadata;
|
||||
Vector3 remainder;
|
||||
|
|
@ -298,6 +299,7 @@ private:
|
|||
float margin;
|
||||
|
||||
Vector3 floor_velocity;
|
||||
RID on_floor_body;
|
||||
bool on_floor;
|
||||
bool on_ceiling;
|
||||
bool on_wall;
|
||||
|
|
@ -307,23 +309,26 @@ private:
|
|||
|
||||
_FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const;
|
||||
|
||||
Ref<KinematicCollision> _move(const Vector3 &p_motion, bool p_infinite_inertia = true);
|
||||
Ref<KinematicCollision> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_test_only = false);
|
||||
Ref<KinematicCollision> _get_slide_collision(int p_bounce);
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
public:
|
||||
bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz);
|
||||
bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz, bool p_test_only = false);
|
||||
bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia);
|
||||
|
||||
bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision);
|
||||
|
||||
void set_axis_lock(PhysicsServer::BodyAxis p_axis, bool p_lock);
|
||||
bool get_axis_lock(PhysicsServer::BodyAxis p_axis) const;
|
||||
|
||||
void set_safe_margin(float p_margin);
|
||||
float get_safe_margin() const;
|
||||
|
||||
Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), float p_slope_stop_min_velocity = 0.05, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true);
|
||||
Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true);
|
||||
Vector3 move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_infinite_inertia = true, bool p_stop_on_slope = false, 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;
|
||||
|
|
|
|||
|
|
@ -231,6 +231,7 @@ public:
|
|||
virtual bool body_is_ray_pickable(RID p_body) const;
|
||||
|
||||
virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL);
|
||||
virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) { return 0; }
|
||||
|
||||
// this function only works on physics process, errors and returns null otherwise
|
||||
virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
|
||||
|
|
|
|||
|
|
@ -484,6 +484,21 @@ public:
|
|||
|
||||
virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL) = 0;
|
||||
|
||||
struct SeparationResult {
|
||||
|
||||
float collision_depth;
|
||||
Vector3 collision_point;
|
||||
Vector3 collision_normal;
|
||||
Vector3 collider_velocity;
|
||||
int collision_local_shape;
|
||||
ObjectID collider_id;
|
||||
RID collider;
|
||||
int collider_shape;
|
||||
Variant collider_metadata;
|
||||
};
|
||||
|
||||
virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) = 0;
|
||||
|
||||
/* SOFT BODY */
|
||||
|
||||
virtual RID soft_body_create(bool p_init_sleeping = false) = 0;
|
||||
|
|
|
|||
Loading…
Reference in a new issue