1 files changed, 139 insertions, 81 deletions

diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index 818ff97730..8e6463f838 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -37,6 +37,8 @@
 #include "servers/physics_server_3d.h"
 #include "skeleton_3d.h"
 
+class KinematicCollision3D;
+
 class PhysicsBody3D : public CollisionObject3D {
 	GDCLASS(PhysicsBody3D, CollisionObject3D);
 
@@ -44,7 +46,19 @@ protected:
 	static void _bind_methods();
 	PhysicsBody3D(PhysicsServer3D::BodyMode p_mode);
 
+	Ref<KinematicCollision3D> motion_cache;
+
+	uint16_t locked_axis = 0;
+
+	Ref<KinematicCollision3D> _move(const Vector3 &p_motion, bool p_test_only = false, real_t p_margin = 0.001);
+
 public:
+	bool move_and_collide(const Vector3 &p_motion, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_test_only = false, bool p_cancel_sliding = true, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>());
+	bool test_move(const Transform3D &p_from, const Vector3 &p_motion, const Ref<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>(), real_t p_margin = 0.001);
+
+	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
+	bool get_axis_lock(PhysicsServer3D::BodyAxis p_axis) const;
+
 	virtual Vector3 get_linear_velocity() const;
 	virtual Vector3 get_angular_velocity() const;
 	virtual real_t get_inverse_mass() const;
@@ -53,12 +67,13 @@ public:
 	void add_collision_exception_with(Node *p_node); //must be physicsbody
 	void remove_collision_exception_with(Node *p_node);
 
-	PhysicsBody3D();
+	virtual ~PhysicsBody3D();
 };
 
 class StaticBody3D : public PhysicsBody3D {
 	GDCLASS(StaticBody3D, PhysicsBody3D);
 
+private:
 	Vector3 constant_linear_velocity;
 	Vector3 constant_angular_velocity;
 
@@ -77,30 +92,70 @@ public:
 	Vector3 get_constant_linear_velocity() const;
 	Vector3 get_constant_angular_velocity() const;
 
-	StaticBody3D();
-	~StaticBody3D();
+	StaticBody3D(PhysicsServer3D::BodyMode p_mode = PhysicsServer3D::BODY_MODE_STATIC);
 
 private:
 	void _reload_physics_characteristics();
 };
 
+class AnimatableBody3D : public StaticBody3D {
+	GDCLASS(AnimatableBody3D, StaticBody3D);
+
+private:
+	Vector3 linear_velocity;
+	Vector3 angular_velocity;
+
+	bool sync_to_physics = false;
+
+	Transform3D last_valid_transform;
+
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	void _body_state_changed(PhysicsDirectBodyState3D *p_state);
+
+protected:
+	void _notification(int p_what);
+	static void _bind_methods();
+
+public:
+	virtual Vector3 get_linear_velocity() const override;
+	virtual Vector3 get_angular_velocity() const override;
+
+	AnimatableBody3D();
+
+private:
+	void _update_kinematic_motion();
+
+	void set_sync_to_physics(bool p_enable);
+	bool is_sync_to_physics_enabled() const;
+};
+
 class RigidBody3D : public PhysicsBody3D {
 	GDCLASS(RigidBody3D, PhysicsBody3D);
 
 public:
 	enum Mode {
-		MODE_RIGID,
+		MODE_DYNAMIC,
 		MODE_STATIC,
-		MODE_CHARACTER,
+		MODE_DYNAMIC_LOCKED,
 		MODE_KINEMATIC,
 	};
 
+	enum CenterOfMassMode {
+		CENTER_OF_MASS_MODE_AUTO,
+		CENTER_OF_MASS_MODE_CUSTOM,
+	};
+
+	GDVIRTUAL1(_integrate_forces, PhysicsDirectBodyState3D *)
+
 protected:
 	bool can_sleep = true;
-	PhysicsDirectBodyState3D *state = nullptr;
-	Mode mode = MODE_RIGID;
+	Mode mode = MODE_DYNAMIC;
 
 	real_t mass = 1.0;
+	Vector3 inertia;
+	CenterOfMassMode center_of_mass_mode = CENTER_OF_MASS_MODE_AUTO;
+	Vector3 center_of_mass;
+
 	Ref<PhysicsMaterial> physics_material_override;
 
 	Vector3 linear_velocity;
@@ -158,11 +213,14 @@ protected:
 	void _body_exit_tree(ObjectID p_id);
 
 	void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape);
-	virtual void _direct_state_changed(Object *p_state);
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state);
 
 	void _notification(int p_what);
 	static void _bind_methods();
 
+	virtual void _validate_property(PropertyInfo &property) const override;
+
 public:
 	void set_mode(Mode p_mode);
 	Mode get_mode() const;
@@ -172,6 +230,15 @@ public:
 
 	virtual real_t get_inverse_mass() const override { return 1.0 / mass; }
 
+	void set_inertia(const Vector3 &p_inertia);
+	const Vector3 &get_inertia() const;
+
+	void set_center_of_mass_mode(CenterOfMassMode p_mode);
+	CenterOfMassMode get_center_of_mass_mode() const;
+
+	void set_center_of_mass(const Vector3 &p_center_of_mass);
+	const Vector3 &get_center_of_mass() const;
+
 	void set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override);
 	Ref<PhysicsMaterial> get_physics_material_override() const;
 
@@ -212,9 +279,6 @@ public:
 	void set_use_continuous_collision_detection(bool p_enable);
 	bool is_using_continuous_collision_detection() const;
 
-	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
-	bool get_axis_lock(PhysicsServer3D::BodyAxis p_axis) const;
-
 	Array get_colliding_bodies() const;
 
 	void add_central_force(const Vector3 &p_force);
@@ -225,7 +289,7 @@ public:
 	void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3());
 	void apply_torque_impulse(const Vector3 &p_impulse);
 
-	TypedArray<String> get_configuration_warnings() const override;
+	virtual TypedArray<String> get_configuration_warnings() const override;
 
 	RigidBody3D();
 	~RigidBody3D();
@@ -235,33 +299,23 @@ private:
 };
 
 VARIANT_ENUM_CAST(RigidBody3D::Mode);
+VARIANT_ENUM_CAST(RigidBody3D::CenterOfMassMode);
 
 class KinematicCollision3D;
 
-class KinematicBody3D : public PhysicsBody3D {
-	GDCLASS(KinematicBody3D, PhysicsBody3D);
-
-public:
-	struct Collision {
-		Vector3 collision;
-		Vector3 normal;
-		Vector3 collider_vel;
-		ObjectID collider;
-		RID collider_rid;
-		int collider_shape = 0;
-		Variant collider_metadata;
-		Vector3 remainder;
-		Vector3 travel;
-		int local_shape = 0;
-	};
+class CharacterBody3D : public PhysicsBody3D {
+	GDCLASS(CharacterBody3D, PhysicsBody3D);
 
 private:
-	Vector3 linear_velocity;
-	Vector3 angular_velocity;
+	real_t margin = 0.001;
 
-	uint16_t locked_axis = 0;
+	bool floor_stop_on_slope = false;
+	int max_slides = 4;
+	real_t floor_max_angle = Math::deg2rad((real_t)45.0);
+	Vector3 snap;
+	Vector3 up_direction = Vector3(0.0, 1.0, 0.0);
 
-	real_t margin;
+	Vector3 linear_velocity;
 
 	Vector3 floor_normal;
 	Vector3 floor_velocity;
@@ -269,57 +323,66 @@ private:
 	bool on_floor = false;
 	bool on_ceiling = false;
 	bool on_wall = false;
-	Vector<Collision> colliders;
+	Vector<PhysicsServer3D::MotionResult> motion_results;
 	Vector<Ref<KinematicCollision3D>> slide_colliders;
-	Ref<KinematicCollision3D> motion_cache;
 
-	_FORCE_INLINE_ bool _ignores_mode(PhysicsServer3D::BodyMode) const;
-
-	Ref<KinematicCollision3D> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
 	Ref<KinematicCollision3D> _get_slide_collision(int p_bounce);
+	Ref<KinematicCollision3D> _get_last_slide_collision();
 
-protected:
-	void _notification(int p_what);
-	static void _bind_methods();
+	void _set_collision_direction(const PhysicsServer3D::MotionResult &p_result);
 
-	virtual void _direct_state_changed(Object *p_state);
+	void set_safe_margin(real_t p_margin);
+	real_t get_safe_margin() const;
 
-public:
-	virtual Vector3 get_linear_velocity() const override;
-	virtual Vector3 get_angular_velocity() const override;
+	bool is_floor_stop_on_slope_enabled() const;
+	void set_floor_stop_on_slope_enabled(bool p_enabled);
 
-	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
-	bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia);
+	int get_max_slides() const;
+	void set_max_slides(int p_max_slides);
 
-	bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision);
+	real_t get_floor_max_angle() const;
+	void set_floor_max_angle(real_t p_radians);
 
-	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
-	bool get_axis_lock(PhysicsServer3D::BodyAxis p_axis) const;
+	const Vector3 &get_snap() const;
+	void set_snap(const Vector3 &p_snap);
 
-	void set_safe_margin(real_t p_margin);
-	real_t get_safe_margin() const;
+	const Vector3 &get_up_direction() const;
+	void set_up_direction(const Vector3 &p_up_direction);
+
+protected:
+	void _notification(int p_what);
+	static void _bind_methods();
+
+public:
+	bool move_and_slide();
+
+	virtual Vector3 get_linear_velocity() const override;
+	void set_linear_velocity(const Vector3 &p_velocity);
 
-	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, real_t p_floor_max_angle = Math::deg2rad((real_t)45.0), 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, real_t p_floor_max_angle = Math::deg2rad((real_t)45.0), bool p_infinite_inertia = true);
 	bool is_on_floor() const;
+	bool is_on_floor_only() const;
 	bool is_on_wall() const;
+	bool is_on_wall_only() const;
 	bool is_on_ceiling() const;
+	bool is_on_ceiling_only() const;
 	Vector3 get_floor_normal() const;
-	Vector3 get_floor_velocity() const;
+	real_t get_floor_angle(const Vector3 &p_up_direction = Vector3(0.0, 1.0, 0.0)) const;
+	Vector3 get_platform_velocity() const;
 
-	int get_slide_count() const;
-	Collision get_slide_collision(int p_bounce) const;
+	int get_slide_collision_count() const;
+	PhysicsServer3D::MotionResult get_slide_collision(int p_bounce) const;
 
-	KinematicBody3D();
-	~KinematicBody3D();
+	CharacterBody3D();
+	~CharacterBody3D();
 };
 
-class KinematicCollision3D : public Reference {
-	GDCLASS(KinematicCollision3D, Reference);
+class KinematicCollision3D : public RefCounted {
+	GDCLASS(KinematicCollision3D, RefCounted);
 
-	KinematicBody3D *owner;
-	friend class KinematicBody3D;
-	KinematicBody3D::Collision collision;
+	PhysicsBody3D *owner = nullptr;
+	friend class PhysicsBody3D;
+	friend class CharacterBody3D;
+	PhysicsServer3D::MotionResult result;
 
 protected:
 	static void _bind_methods();
@@ -329,15 +392,15 @@ public:
 	Vector3 get_normal() const;
 	Vector3 get_travel() const;
 	Vector3 get_remainder() const;
+	real_t get_angle(const Vector3 &p_up_direction = Vector3(0.0, 1.0, 0.0)) const;
 	Object *get_local_shape() const;
 	Object *get_collider() const;
 	ObjectID get_collider_id() const;
+	RID get_collider_rid() const;
 	Object *get_collider_shape() const;
 	int get_collider_shape_index() const;
 	Vector3 get_collider_velocity() const;
 	Variant get_collider_metadata() const;
-
-	KinematicCollision3D();
 };
 
 class PhysicalBone3D : public PhysicsBody3D {
@@ -467,12 +530,12 @@ private:
 #endif
 
 	JointData *joint_data = nullptr;
-	Transform joint_offset;
+	Transform3D joint_offset;
 	RID joint;
 
 	Skeleton3D *parent_skeleton = nullptr;
-	Transform body_offset;
-	Transform body_offset_inverse;
+	Transform3D body_offset;
+	Transform3D body_offset_inverse;
 	bool simulate_physics = false;
 	bool _internal_simulate_physics = false;
 	int bone_id = -1;
@@ -491,7 +554,8 @@ protected:
 	bool _get(const StringName &p_name, Variant &r_ret) const;
 	void _get_property_list(List<PropertyInfo> *p_list) const;
 	void _notification(int p_what);
-	void _direct_state_changed(Object *p_state);
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	void _body_state_changed(PhysicsDirectBodyState3D *p_state);
 
 	static void _bind_methods();
 
@@ -508,8 +572,8 @@ public:
 
 public:
 #ifdef TOOLS_ENABLED
-	virtual Transform get_global_gizmo_transform() const override;
-	virtual Transform get_local_gizmo_transform() const override;
+	virtual Transform3D get_global_gizmo_transform() const override;
+	virtual Transform3D get_local_gizmo_transform() const override;
 #endif
 
 	const JointData *get_joint_data() const;
@@ -520,17 +584,14 @@ public:
 	void set_joint_type(JointType p_joint_type);
 	JointType get_joint_type() const;
 
-	void set_joint_offset(const Transform &p_offset);
-	const Transform &get_joint_offset() const;
+	void set_joint_offset(const Transform3D &p_offset);
+	const Transform3D &get_joint_offset() const;
 
 	void set_joint_rotation(const Vector3 &p_euler_rad);
 	Vector3 get_joint_rotation() const;
 
-	void set_joint_rotation_degrees(const Vector3 &p_euler_deg);
-	Vector3 get_joint_rotation_degrees() const;
-
-	void set_body_offset(const Transform &p_offset);
-	const Transform &get_body_offset() const;
+	void set_body_offset(const Transform3D &p_offset);
+	const Transform3D &get_body_offset() const;
 
 	void set_simulate_physics(bool p_simulate);
 	bool get_simulate_physics();
@@ -560,9 +621,6 @@ public:
 	void set_can_sleep(bool p_active);
 	bool is_able_to_sleep() const;
 
-	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
-	bool get_axis_lock(PhysicsServer3D::BodyAxis p_axis) const;
-
 	void apply_central_impulse(const Vector3 &p_impulse);
 	void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3());