Merge pull request #16530 from AndreaCatania/rays

Improved ray shape (2D and 3D) by addiing the possibility to act as r…

15 files changed, 117 insertions, 20 deletions

diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp
index 4164450cd2..8707096038 100644
--- a/modules/bullet/btRayShape.cpp
+++ b/modules/bullet/btRayShape.cpp
@@ -54,6 +54,11 @@ void btRayShape::setLength(btScalar p_length) {
 	reload_cache();
 }
 
+void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {
+
+	slipsOnSlope = p_slipsOnSlope;
+}
+
 btVector3 btRayShape::localGetSupportingVertex(const btVector3 &vec) const {
 	return localGetSupportingVertexWithoutMargin(vec) + (m_shapeAxis * m_collisionMargin);
 }
diff --git a/modules/bullet/btRayShape.h b/modules/bullet/btRayShape.h
index 99a9412dbe..a44c30db4b 100644
--- a/modules/bullet/btRayShape.h
+++ b/modules/bullet/btRayShape.h
@@ -44,6 +44,7 @@ ATTRIBUTE_ALIGNED16(class)
 btRayShape : public btConvexInternalShape {
 
 	btScalar m_length;
+	bool slipsOnSlope;
 	/// The default axis is the z
 	btVector3 m_shapeAxis;
 
@@ -59,6 +60,9 @@ public:
 	void setLength(btScalar p_length);
 	btScalar getLength() const { return m_length; }
 
+	void setSlipsOnSlope(bool p_slipOnSlope);
+	bool getSlipsOnSlope() const { return slipsOnSlope; }
+
 	const btTransform &getSupportPoint() const { return m_cacheSupportPoint; }
 	const btScalar &getScaledLength() const { return m_cacheScaledLength; }
 
diff --git a/modules/bullet/godot_ray_world_algorithm.cpp b/modules/bullet/godot_ray_world_algorithm.cpp
index 4a511b39a7..53d0ab7e3c 100644
--- a/modules/bullet/godot_ray_world_algorithm.cpp
+++ b/modules/bullet/godot_ray_world_algorithm.cpp
@@ -100,14 +100,16 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo
 
 	if (btResult.hasHit()) {
 
-		btVector3 ray_normal(ray_transform.getOrigin() - to.getOrigin());
-		ray_normal.normalize();
 		btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
 
 		if (depth >= -RAY_STABILITY_MARGIN)
 			depth = 0;
 
-		resultOut->addContactPoint(ray_normal, btResult.m_hitPointWorld, depth);
+		if (ray_shape->getSlipsOnSlope())
+			resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
+		else {
+			resultOut->addContactPoint((ray_transform.getOrigin() - to.getOrigin()).normalize(), btResult.m_hitPointWorld, depth);
+		}
 	}
 }
 
diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp
index c6b9695d96..5d8d391bd9 100644
--- a/modules/bullet/shape_bullet.cpp
+++ b/modules/bullet/shape_bullet.cpp
@@ -135,8 +135,10 @@ btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<rea
 	return bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, -fieldHeight, fieldHeight, YAxis, PHY_FLOAT, flipQuadEdges));
 }
 
-btRayShape *ShapeBullet::create_shape_ray(real_t p_length) {
-	return bulletnew(btRayShape(p_length));
+btRayShape *ShapeBullet::create_shape_ray(real_t p_length, bool p_slips_on_slope) {
+	btRayShape *r(bulletnew(btRayShape(p_length)));
+	r->setSlipsOnSlope(p_slips_on_slope);
+	return r;
 }
 
 /* PLANE */
@@ -435,25 +437,33 @@ btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_impli
 /* Ray shape */
 RayShapeBullet::RayShapeBullet() :
 		ShapeBullet(),
-		length(1) {}
+		length(1),
+		slips_on_slope(false) {}
 
 void RayShapeBullet::set_data(const Variant &p_data) {
-	setup(p_data);
+
+	Dictionary d = p_data;
+	setup(d["length"], d["slips_on_slope"]);
 }
 
 Variant RayShapeBullet::get_data() const {
-	return length;
+
+	Dictionary d;
+	d["length"] = length;
+	d["slips_on_slope"] = slips_on_slope;
+	return d;
 }
 
 PhysicsServer::ShapeType RayShapeBullet::get_type() const {
 	return PhysicsServer::SHAPE_RAY;
 }
 
-void RayShapeBullet::setup(real_t p_length) {
+void RayShapeBullet::setup(real_t p_length, bool p_slips_on_slope) {
 	length = p_length;
+	slips_on_slope = p_slips_on_slope;
 	notifyShapeChanged();
 }
 
 btCollisionShape *RayShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
-	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_margin));
+	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_margin, slips_on_slope));
 }
diff --git a/modules/bullet/shape_bullet.h b/modules/bullet/shape_bullet.h
index e04a3c808a..2acba90e36 100644
--- a/modules/bullet/shape_bullet.h
+++ b/modules/bullet/shape_bullet.h
@@ -86,7 +86,7 @@ public:
 	static class btConvexPointCloudShape *create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
 	static class btScaledBvhTriangleMeshShape *create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
 	static class btHeightfieldTerrainShape *create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
-	static class btRayShape *create_shape_ray(real_t p_length);
+	static class btRayShape *create_shape_ray(real_t p_length, bool p_slips_on_slope);
 };
 
 class PlaneShapeBullet : public ShapeBullet {
@@ -216,6 +216,7 @@ class RayShapeBullet : public ShapeBullet {
 
 public:
 	real_t length;
+	bool slips_on_slope;
 
 	RayShapeBullet();
 
@@ -225,6 +226,6 @@ public:
 	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0);
 
 private:
-	void setup(real_t p_length);
+	void setup(real_t p_length, bool p_slips_on_slope);
 };
 #endif
diff --git a/scene/resources/ray_shape.cpp b/scene/resources/ray_shape.cpp
index 78a19050f1..a9dec3e87c 100644
--- a/scene/resources/ray_shape.cpp
+++ b/scene/resources/ray_shape.cpp
@@ -43,7 +43,10 @@ Vector<Vector3> RayShape::_gen_debug_mesh_lines() {
 
 void RayShape::_update_shape() {
 
-	PhysicsServer::get_singleton()->shape_set_data(get_shape(), length);
+	Dictionary d;
+	d["length"] = length;
+	d["slips_on_slope"] = slips_on_slope;
+	PhysicsServer::get_singleton()->shape_set_data(get_shape(), d);
 	emit_changed();
 }
 
@@ -52,6 +55,7 @@ void RayShape::set_length(float p_length) {
 	length = p_length;
 	_update_shape();
 	notify_change_to_owners();
+	_change_notify("length");
 }
 
 float RayShape::get_length() const {
@@ -59,16 +63,33 @@ float RayShape::get_length() const {
 	return length;
 }
 
+void RayShape::set_slips_on_slope(bool p_active) {
+
+	slips_on_slope = p_active;
+	_update_shape();
+	notify_change_to_owners();
+	_change_notify("slips_on_slope");
+}
+
+bool RayShape::get_slips_on_slope() const {
+	return slips_on_slope;
+}
+
 void RayShape::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_length", "length"), &RayShape::set_length);
 	ClassDB::bind_method(D_METHOD("get_length"), &RayShape::get_length);
 
+	ClassDB::bind_method(D_METHOD("set_slips_on_slope", "active"), &RayShape::set_slips_on_slope);
+	ClassDB::bind_method(D_METHOD("get_slips_on_slope"), &RayShape::get_slips_on_slope);
+
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "length", PROPERTY_HINT_RANGE, "0,4096,0.01"), "set_length", "get_length");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "slips_on_slope"), "set_slips_on_slope", "get_slips_on_slope");
 }
 
 RayShape::RayShape() :
 		Shape(PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_RAY)) {
 
 	set_length(1.0);
+	set_slips_on_slope(false);
 }
diff --git a/scene/resources/ray_shape.h b/scene/resources/ray_shape.h
index 4bd96116fe..f11b2e7c3c 100644
--- a/scene/resources/ray_shape.h
+++ b/scene/resources/ray_shape.h
@@ -36,6 +36,7 @@ class RayShape : public Shape {
 
 	GDCLASS(RayShape, Shape);
 	float length;
+	bool slips_on_slope;
 
 protected:
 	static void _bind_methods();
@@ -46,6 +47,9 @@ public:
 	void set_length(float p_length);
 	float get_length() const;
 
+	void set_slips_on_slope(bool p_active);
+	bool get_slips_on_slope() const;
+
 	RayShape();
 };
 #endif // RAY_SHAPE_H
diff --git a/scene/resources/segment_shape_2d.cpp b/scene/resources/segment_shape_2d.cpp
index 81110015b5..58027c127d 100644
--- a/scene/resources/segment_shape_2d.cpp
+++ b/scene/resources/segment_shape_2d.cpp
@@ -106,7 +106,10 @@ SegmentShape2D::SegmentShape2D() :
 
 void RayShape2D::_update_shape() {
 
-	Physics2DServer::get_singleton()->shape_set_data(get_rid(), length);
+	Dictionary d;
+	d["length"] = length;
+	d["slips_on_slope"] = slips_on_slope;
+	Physics2DServer::get_singleton()->shape_set_data(get_rid(), d);
 	emit_changed();
 }
 
@@ -140,7 +143,11 @@ void RayShape2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_length", "length"), &RayShape2D::set_length);
 	ClassDB::bind_method(D_METHOD("get_length"), &RayShape2D::get_length);
 
+	ClassDB::bind_method(D_METHOD("set_slips_on_slope", "active"), &RayShape2D::set_slips_on_slope);
+	ClassDB::bind_method(D_METHOD("get_slips_on_slope"), &RayShape2D::get_slips_on_slope);
+
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "length"), "set_length", "get_length");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "slips_on_slope"), "set_slips_on_slope", "get_slips_on_slope");
 }
 
 void RayShape2D::set_length(real_t p_length) {
@@ -153,9 +160,20 @@ real_t RayShape2D::get_length() const {
 	return length;
 }
 
+void RayShape2D::set_slips_on_slope(bool p_active) {
+
+	slips_on_slope = p_active;
+	_update_shape();
+}
+
+bool RayShape2D::get_slips_on_slope() const {
+	return slips_on_slope;
+}
+
 RayShape2D::RayShape2D() :
 		Shape2D(Physics2DServer::get_singleton()->ray_shape_create()) {
 
 	length = 20;
+	slips_on_slope = false;
 	_update_shape();
 }
diff --git a/scene/resources/segment_shape_2d.h b/scene/resources/segment_shape_2d.h
index 4ed30c0443..700982ac0a 100644
--- a/scene/resources/segment_shape_2d.h
+++ b/scene/resources/segment_shape_2d.h
@@ -63,6 +63,7 @@ class RayShape2D : public Shape2D {
 	GDCLASS(RayShape2D, Shape2D);
 
 	real_t length;
+	bool slips_on_slope;
 
 	void _update_shape();
 
@@ -72,6 +73,10 @@ protected:
 public:
 	void set_length(real_t p_length);
 	real_t get_length() const;
+
+	void set_slips_on_slope(bool p_active);
+	bool get_slips_on_slope() const;
+
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
 
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp
index e26a7a4d89..0037b9a862 100644
--- a/servers/physics/collision_solver_sw.cpp
+++ b/servers/physics/collision_solver_sw.cpp
@@ -90,6 +90,10 @@ bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_t
 		return false;
 
 	Vector3 support_B = p_transform_B.xform(p);
+	if (ray->get_slips_on_slope()) {
+		Vector3 global_n = ai.basis.xform_inv(n).normalized();
+		support_B = support_A + (support_B - support_A).length() * global_n;
+	}
 
 	if (p_result_callback) {
 		if (p_swap_result)
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index b918a42ca5..9d2e5e846d 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -165,6 +165,10 @@ real_t RayShapeSW::get_length() const {
 	return length;
 }
 
+bool RayShapeSW::get_slips_on_slope() const {
+	return slips_on_slope;
+}
+
 void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
 	// don't think this will be even used
@@ -221,25 +225,31 @@ Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const {
 	return Vector3();
 }
 
-void RayShapeSW::_setup(real_t p_length) {
+void RayShapeSW::_setup(real_t p_length, bool p_slips_on_slope) {
 
 	length = p_length;
+	slips_on_slope = p_slips_on_slope;
 	configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
 }
 
 void RayShapeSW::set_data(const Variant &p_data) {
 
-	_setup(p_data);
+	Dictionary d = p_data;
+	_setup(d["length"], d["slips_on_slope"]);
 }
 
 Variant RayShapeSW::get_data() const {
 
-	return length;
+	Dictionary d;
+	d["length"] = length;
+	d["slips_on_slope"] = slips_on_slope;
+	return d;
 }
 
 RayShapeSW::RayShapeSW() {
 
 	length = 1;
+	slips_on_slope = false;
 }
 
 /********** SPHERE *************/
diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h
index 4a9a6289ff..7f7f9f4f98 100644
--- a/servers/physics/shape_sw.h
+++ b/servers/physics/shape_sw.h
@@ -149,11 +149,13 @@ public:
 class RayShapeSW : public ShapeSW {
 
 	real_t length;
+	bool slips_on_slope;
 
-	void _setup(real_t p_length);
+	void _setup(real_t p_length, bool p_slips_on_slope);
 
 public:
 	real_t get_length() const;
+	bool get_slips_on_slope() const;
 
 	virtual real_t get_area() const { return 0.0; }
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index a6ef110149..efee98a35a 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -95,6 +95,10 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Transf
 	}
 
 	Vector2 support_B = p_transform_B.xform(p);
+	if (ray->get_slips_on_slope()) {
+		Vector2 global_n = invb.basis_xform_inv(n).normalized();
+		support_B = support_A + (support_B - support_A).length() * global_n;
+	}
 
 	if (p_result_callback) {
 		if (p_swap_result)
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 5893f19827..433942708e 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -184,13 +184,18 @@ real_t RayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale)
 
 void RayShape2DSW::set_data(const Variant &p_data) {
 
-	length = p_data;
+	Dictionary d = p_data;
+	length = d["length"];
+	slips_on_slope = d["slips_on_slope"];
 	configure(Rect2(0, 0, 0.001, length));
 }
 
 Variant RayShape2DSW::get_data() const {
 
-	return length;
+	Dictionary d;
+	d["length"] = length;
+	d["slips_on_slope"] = slips_on_slope;
+	return d;
 }
 
 /*********************************************************/
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index c4c267b368..d937301f3c 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -197,9 +197,11 @@ public:
 class RayShape2DSW : public Shape2DSW {
 
 	real_t length;
+	bool slips_on_slope;
 
 public:
 	_FORCE_INLINE_ real_t get_length() const { return length; }
+	_FORCE_INLINE_ bool get_slips_on_slope() const { return slips_on_slope; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }