asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Added function to expose floor normal, useful to correctly calculate player velocity.

Browse source 
This work has been kindly sponsored by IMVU.
This commit is contained in:
Andrea Catania 2020-01-10 14:58:19 +01:00
parent abefd42e84
commit 9f1f4620e0
4 changed files with 49 additions and 27 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
scene/2d/physics_body_2d.cpp
View file

@ -1212,7 +1212,7 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_
//so, if you pass 45 as limit, avoid numerical precision errors 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_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
Vector2 body_velocity = p_linear_velocity;
Vector2 body_velocity_normal = body_velocity.normalized();
@ -1234,6 +1234,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
on_ceiling = false;
on_wall = false;
colliders.clear();
floor_normal = p_up_direction;
floor_velocity = Vector2();
while (p_max_slides) {
@ -1262,25 +1263,26 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
colliders.push_back(collision);
motion = collision.remainder;
if (p_floor_direction == Vector2()) {
if (p_up_direction == Vector2()) {
//all is a wall
on_wall = true;
} else {
if (Math::acos(collision.normal.dot(p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
if (Math::acos(collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
on_floor = true;
floor_normal = collision.normal;
on_floor_body = collision.collider_rid;
floor_velocity = collision.collider_vel;
if (p_stop_on_slope) {
if ((body_velocity_normal + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
if ((body_velocity_normal + p_up_direction).length() < 0.01 && collision.travel.length() < 1) {
Transform2D gt = get_global_transform();
gt.elements[2] -= collision.travel.slide(p_floor_direction);
gt.elements[2] -= collision.travel.slide(p_up_direction);
set_global_transform(gt);
return Vector2();
}
}
} else if (Math::acos(collision.normal.dot(-p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
} else if (Math::acos(collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
on_ceiling = true;
} else {
on_wall = true;
@ -1301,11 +1303,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
return body_velocity;
}
Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
bool was_on_floor = on_floor;
Vector2 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
Vector2 ret = move_and_slide(p_linear_velocity, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
if (!was_on_floor || p_snap == Vector2()) {
return ret;
}
@ -1315,15 +1317,16 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci
if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) {
bool apply = true;
if (p_floor_direction != Vector2()) {
if (Math::acos(p_floor_direction.normalized().dot(col.normal)) < p_floor_max_angle) {
if (p_up_direction != Vector2()) {
if (Math::acos(p_up_direction.normalized().dot(col.normal)) < p_floor_max_angle) {
on_floor = true;
floor_normal = col.normal;
on_floor_body = col.collider_rid;
floor_velocity = col.collider_vel;
if (p_stop_on_slope) {
// move and collide may stray the object a bit because of pre un-stucking,
// so only ensure that motion happens on floor direction in this case.
col.travel = p_floor_direction * p_floor_direction.dot(col.travel);
col.travel = p_up_direction * p_up_direction.dot(col.travel);
}
} else {
@ -1353,6 +1356,11 @@ bool KinematicBody2D::is_on_ceiling() const {
return on_ceiling;
}
Vector2 KinematicBody2D::get_floor_normal() const {
return floor_normal;
}
Vector2 KinematicBody2D::get_floor_velocity() const {
return floor_velocity;
@ -1473,6 +1481,7 @@ void KinematicBody2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody2D::is_on_floor);
ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody2D::is_on_ceiling);
ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody2D::is_on_wall);
ClassDB::bind_method(D_METHOD("get_floor_normal"), &KinematicBody2D::get_floor_normal);
ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody2D::get_floor_velocity);
ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &KinematicBody2D::set_safe_margin);

6
scene/2d/physics_body_2d.h
View file

@ -306,6 +306,7 @@ public:
private:
float margin;
Vector2 floor_normal;
Vector2 floor_velocity;
RID on_floor_body;
bool on_floor;
@ -339,11 +340,12 @@ 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_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true);
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_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true);
Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_up_direction = Vector2(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);
Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction = Vector2(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);
bool is_on_floor() const;
bool is_on_wall() const;
bool is_on_ceiling() const;
Vector2 get_floor_normal() const;
Vector2 get_floor_velocity() const;
int get_slide_count() const;

33
scene/3d/physics_body.cpp
View file

@ -1140,7 +1140,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
//so, if you pass 45 as limit, avoid numerical precision errors 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, bool p_stop_on_slope, 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_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
Vector3 body_velocity = p_linear_velocity;
Vector3 body_velocity_normal = body_velocity.normalized();
@ -1159,6 +1159,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
on_ceiling = false;
on_wall = false;
colliders.clear();
floor_normal = p_up_direction;
floor_velocity = Vector3();
while (p_max_slides) {
@ -1187,25 +1188,26 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
colliders.push_back(collision);
motion = collision.remainder;
if (p_floor_direction == Vector3()) {
if (p_up_direction == Vector3()) {
//all is a wall
on_wall = true;
} else {
if (Math::acos(collision.normal.dot(p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
if (Math::acos(collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
on_floor = true;
floor_normal = collision.normal;
on_floor_body = collision.collider_rid;
floor_velocity = collision.collider_vel;
if (p_stop_on_slope) {
if ((body_velocity_normal + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
if ((body_velocity_normal + p_up_direction).length() < 0.01 && collision.travel.length() < 1) {
Transform gt = get_global_transform();
gt.origin -= collision.travel.slide(p_floor_direction);
gt.origin -= collision.travel.slide(p_up_direction);
set_global_transform(gt);
return Vector3();
}
}
} else if (Math::acos(collision.normal.dot(-p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
} else if (Math::acos(collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
on_ceiling = true;
} else {
on_wall = true;
@ -1232,11 +1234,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
return body_velocity;
}
Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
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);
Vector3 ret = move_and_slide(p_linear_velocity, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
if (!was_on_floor || p_snap == Vector3()) {
return ret;
}
@ -1247,15 +1249,16 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity
if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) {
bool apply = true;
if (p_floor_direction != Vector3()) {
if (Math::acos(p_floor_direction.normalized().dot(col.normal)) < p_floor_max_angle) {
if (p_up_direction != Vector3()) {
if (Math::acos(p_up_direction.normalized().dot(col.normal)) < p_floor_max_angle) {
on_floor = true;
floor_normal = col.normal;
on_floor_body = col.collider_rid;
floor_velocity = col.collider_vel;
if (p_stop_on_slope) {
// move and collide may stray the object a bit because of pre un-stucking,
// so only ensure that motion happens on floor direction in this case.
col.travel = col.travel.project(p_floor_direction);
col.travel = col.travel.project(p_up_direction);
}
} else {
apply = false; //snapped with floor direction, but did not snap to a floor, do not snap.
@ -1284,6 +1287,11 @@ bool KinematicBody::is_on_ceiling() const {
return on_ceiling;
}
Vector3 KinematicBody::get_floor_normal() const {
return floor_normal;
}
Vector3 KinematicBody::get_floor_velocity() const {
return floor_velocity;
@ -1392,7 +1400,7 @@ void KinematicBody::_notification(int p_what) {
void KinematicBody::_bind_methods() {
ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody::_move, DEFVAL(true), 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", "linear_velocity", "up_direction", "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", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody::test_move, DEFVAL(true));
@ -1400,6 +1408,7 @@ void KinematicBody::_bind_methods() {
ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody::is_on_floor);
ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody::is_on_ceiling);
ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody::is_on_wall);
ClassDB::bind_method(D_METHOD("get_floor_normal"), &KinematicBody::get_floor_normal);
ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody::get_floor_velocity);
ClassDB::bind_method(D_METHOD("set_axis_lock", "axis", "lock"), &KinematicBody::set_axis_lock);

6
scene/3d/physics_body.h
View file

@ -300,6 +300,7 @@ private:
float margin;
Vector3 floor_normal;
Vector3 floor_velocity;
RID on_floor_body;
bool on_floor;
@ -330,11 +331,12 @@ public:
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), 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_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(const Vector3 &p_linear_velocity, const Vector3 &p_up_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_up_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);
bool is_on_floor() const;
bool is_on_wall() const;
bool is_on_ceiling() const;
Vector3 get_floor_normal() const;
Vector3 get_floor_velocity() const;
int get_slide_count() const;