-Changed KinematicBody API yet again to make it friendlier

-Fixed get_scale functions (and added set_scale) to make it more coherent when decomposing and composing (fixes bugs in transform interpolation)

core/math/matrix3.cpp

@@ -234,7 +234,22 @@ Basis Basis::scaled(const Vector3 &p_scale) const {
 	return m;
 }
 
+void Basis::set_scale(const Vector3 &p_scale) {
+
+	set_axis(0, get_axis(0).normalized() * p_scale.x);
+	set_axis(1, get_axis(1).normalized() * p_scale.y);
+	set_axis(2, get_axis(2).normalized() * p_scale.z);
+}
+
 Vector3 Basis::get_scale() const {
+
+	return Vector3(
+			Vector3(elements[0][0], elements[1][0], elements[2][0]).length(),
+			Vector3(elements[0][1], elements[1][1], elements[2][1]).length(),
+			Vector3(elements[0][2], elements[1][2], elements[2][2]).length());
+}
+
+Vector3 Basis::get_signed_scale() const {
 	// FIXME: We are assuming M = R.S (R is rotation and S is scaling), and use polar decomposition to extract R and S.
 	// A polar decomposition is M = O.P, where O is an orthogonal matrix (meaning rotation and reflection) and
 	// P is a positive semi-definite matrix (meaning it contains absolute values of scaling along its diagonal).

core/math/matrix3.h

@@ -97,7 +97,9 @@ public:
 	void scale(const Vector3 &p_scale);
 	Basis scaled(const Vector3 &p_scale) const;
 
+	void set_scale(const Vector3 &p_scale);
 	Vector3 get_scale() const;
+	Vector3 get_signed_scale() const;
 
 	// transposed dot products
 	_FORCE_INLINE_ real_t tdotx(const Vector3 &v) const {

core/math/transform.cpp

@@ -118,17 +118,17 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c)
 
 	/* not sure if very "efficient" but good enough? */
 
-	Vector3 src_scale = basis.get_scale();
-	Quat src_rot = basis;
+	Vector3 src_scale = basis.get_signed_scale();
+	Quat src_rot = basis.orthonormalized();
 	Vector3 src_loc = origin;
 
-	Vector3 dst_scale = p_transform.basis.get_scale();
+	Vector3 dst_scale = p_transform.basis.get_signed_scale();
 	Quat dst_rot = p_transform.basis;
 	Vector3 dst_loc = p_transform.origin;
 
-	Transform dst;
-	dst.basis = src_rot.slerp(dst_rot, p_c);
-	dst.basis.scale(src_scale.linear_interpolate(dst_scale, p_c));
+	Transform dst; //this could be made faster by using a single function in Basis..
+	dst.basis = src_rot.slerp(dst_rot, p_c).normalized();
+	dst.basis.set_scale(src_scale.linear_interpolate(dst_scale, p_c));
 	dst.origin = src_loc.linear_interpolate(dst_loc, p_c);
 
 	return dst;

editor/plugins/cube_grid_theme_editor_plugin.cpp

@@ -111,7 +111,8 @@ void MeshLibraryEditor::_import_scene(Node *p_scene, Ref<MeshLibrary> p_library,
 			sb->get_shape_owners(&shapes);
 
 			for (List<uint32_t>::Element *E = shapes.front(); E; E = E->next()) {
-				if (sb->is_shape_owner_disabled(E->get())) continue;
+				if (sb->is_shape_owner_disabled(E->get()))
+					continue;
 
 				//Transform shape_transform = sb->shape_owner_get_transform(E->get());
 
@@ -120,7 +121,7 @@ void MeshLibraryEditor::_import_scene(Node *p_scene, Ref<MeshLibrary> p_library,
 				for (int k = 0; k < sb->shape_owner_get_shape_count(E->get()); k++) {
 
 					Ref<Shape> collision = sb->shape_owner_get_shape(E->get(), k);
-					if (collision.is_valid())
+					if (!collision.is_valid())
 						continue;
 					MeshLibrary::ShapeData shape_data;
 					shape_data.shape = collision;

scene/3d/physics_body.cpp

@@ -912,35 +912,24 @@ RigidBody::~RigidBody() {
 //////////////////////////////////////////////////////
 //////////////////////////
 
-Dictionary KinematicBody::_move(const Vector3 &p_motion) {
+Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion) {
 
 	Collision col;
-	if (move(p_motion, col)) {
-		Dictionary d;
-		d["position"] = col.collision;
-		d["normal"] = col.normal;
-		d["local_shape"] = col.local_shape;
-		d["travel"] = col.travel;
-		d["remainder"] = col.remainder;
-		d["collider_id"] = col.collider;
-		if (col.collider) {
-			d["collider"] = ObjectDB::get_instance(col.collider);
-		} else {
-			d["collider"] = Variant();
+	if (move_and_collide(p_motion, col)) {
+		if (motion_cache.is_null()) {
+			motion_cache.instance();
+			motion_cache->owner = this;
 		}
 
-		d["collider_velocity"] = col.collider_vel;
-		d["collider_shape_index"] = col.collider_shape;
-		d["collider_metadata"] = col.collider_metadata;
-
-		return d;
+		motion_cache->collision = col;
 
-	} else {
-		return Dictionary();
+		return motion_cache;
 	}
+
+	return Ref<KinematicCollision>();
 }
 
-bool KinematicBody::move(const Vector3 &p_motion, Collision &r_collision) {
+bool KinematicBody::move_and_collide(const Vector3 &p_motion, Collision &r_collision) {
 
 	Transform gt = get_global_transform();
 	PhysicsServer::MotionResult result;
@@ -964,7 +953,7 @@ bool KinematicBody::move(const Vector3 &p_motion, Collision &r_collision) {
 	return colliding;
 }
 
-Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, float p_slope_stop_min_velocity, int p_max_bounces, float p_floor_max_angle) {
+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) {
 
 	Vector3 motion = (floor_velocity + p_linear_velocity) * get_fixed_process_delta_time();
 	Vector3 lv = p_linear_velocity;
@@ -975,11 +964,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 	colliders.clear();
 	floor_velocity = Vector3();
 
-	while (p_max_bounces) {
+	while (p_max_slides) {
 
 		Collision collision;
 
-		bool collided = move(motion, collision);
+		bool collided = move_and_collide(motion, collision);
 
 		if (collided) {
 
@@ -1017,7 +1006,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 			break;
 		}
 
-		p_max_bounces--;
+		p_max_slides--;
 		if (motion == Vector3())
 			break;
 	}
@@ -1059,79 +1048,36 @@ float KinematicBody::get_safe_margin() const {
 
 	return margin;
 }
-
-int KinematicBody::get_collision_count() const {
+int KinematicBody::get_slide_count() const {
 
 	return colliders.size();
 }
-Vector3 KinematicBody::get_collision_position(int p_collision) const {
-
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
 
-	return colliders[p_collision].collision;
-}
-Vector3 KinematicBody::get_collision_normal(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
-	return colliders[p_collision].normal;
+KinematicBody::Collision KinematicBody::get_slide_collision(int p_bounce) const {
+	ERR_FAIL_INDEX_V(p_bounce, colliders.size(), Collision());
+	return colliders[p_bounce];
 }
 
-Vector3 KinematicBody::get_collision_travel(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
-	return colliders[p_collision].travel;
-}
-Vector3 KinematicBody::get_collision_remainder(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
-	return colliders[p_collision].remainder;
-}
-Object *KinematicBody::get_collision_local_shape(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
-	uint32_t owner = shape_find_owner(colliders[p_collision].local_shape);
-	return shape_owner_get_owner(owner);
-}
-Object *KinematicBody::get_collision_collider(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
+Ref<KinematicCollision> KinematicBody::_get_slide_collision(int p_bounce) {
 
-	if (colliders[p_collision].collider) {
-		return ObjectDB::get_instance(colliders[p_collision].collider);
+	ERR_FAIL_INDEX_V(p_bounce, colliders.size(), Ref<KinematicCollision>());
+	if (p_bounce > slide_colliders.size()) {
+		slide_colliders.resize(p_bounce + 1);
 	}
 
-	return NULL;
-}
-ObjectID KinematicBody::get_collision_collider_id(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), 0);
-
-	return colliders[p_collision].collider;
-}
-Object *KinematicBody::get_collision_collider_shape(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
-	Object *collider = get_collision_collider(p_collision);
-	if (collider) {
-		CollisionObject *obj2d = Object::cast_to<CollisionObject>(collider);
-		if (obj2d) {
-			uint32_t owner = shape_find_owner(colliders[p_collision].collider_shape);
-			return obj2d->shape_owner_get_owner(owner);
-		}
+	if (slide_colliders[p_bounce].is_null()) {
+		slide_colliders[p_bounce].instance();
+		slide_colliders[p_bounce]->owner = this;
 	}
 
-	return NULL;
-}
-int KinematicBody::get_collision_collider_shape_index(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), -1);
-	return colliders[p_collision].collider_shape;
-}
-Vector3 KinematicBody::get_collision_collider_velocity(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
-	return colliders[p_collision].collider_vel;
-}
-Variant KinematicBody::get_collision_collider_metadata(int p_collision) const {
-	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Variant());
-	return colliders[p_collision].collider_metadata;
+	slide_colliders[p_bounce]->collision = colliders[p_bounce];
+	return slide_colliders[p_bounce];
 }
 
 void KinematicBody::_bind_methods() {
 
-	ClassDB::bind_method(D_METHOD("move", "rel_vec"), &KinematicBody::_move);
-	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(0.05), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
+	ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec"), &KinematicBody::_move);
+	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_slides", "floor_max_angle"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(0.05), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
 
 	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec"), &KinematicBody::test_move);
 
@@ -1143,18 +1089,8 @@ void KinematicBody::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &KinematicBody::set_safe_margin);
 	ClassDB::bind_method(D_METHOD("get_safe_margin"), &KinematicBody::get_safe_margin);
 
-	ClassDB::bind_method(D_METHOD("get_collision_count"), &KinematicBody::get_collision_count);
-	ClassDB::bind_method(D_METHOD("get_collision_position", "collision"), &KinematicBody::get_collision_position);
-	ClassDB::bind_method(D_METHOD("get_collision_normal", "collision"), &KinematicBody::get_collision_normal);
-	ClassDB::bind_method(D_METHOD("get_collision_travel", "collision"), &KinematicBody::get_collision_travel);
-	ClassDB::bind_method(D_METHOD("get_collision_remainder", "collision"), &KinematicBody::get_collision_remainder);
-	ClassDB::bind_method(D_METHOD("get_collision_local_shape", "collision"), &KinematicBody::get_collision_local_shape);
-	ClassDB::bind_method(D_METHOD("get_collision_collider", "collision"), &KinematicBody::get_collision_collider);
-	ClassDB::bind_method(D_METHOD("get_collision_collider_id", "collision"), &KinematicBody::get_collision_collider_id);
-	ClassDB::bind_method(D_METHOD("get_collision_collider_shape", "collision"), &KinematicBody::get_collision_collider_shape);
-	ClassDB::bind_method(D_METHOD("get_collision_collider_shape_index", "collision"), &KinematicBody::get_collision_collider_shape_index);
-	ClassDB::bind_method(D_METHOD("get_collision_collider_velocity", "collision"), &KinematicBody::get_collision_collider_velocity);
-	ClassDB::bind_method(D_METHOD("get_collision_collider_metadata", "collision"), &KinematicBody::get_collision_collider_metadata);
+	ClassDB::bind_method(D_METHOD("get_slide_count"), &KinematicBody::get_slide_count);
+	ClassDB::bind_method(D_METHOD("get_slide_collision", "slide_idx"), &KinematicBody::_get_slide_collision);
 
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
 }
@@ -1169,4 +1105,106 @@ KinematicBody::KinematicBody()
 	on_wall = false;
 }
 KinematicBody::~KinematicBody() {
+
+	if (motion_cache.is_valid()) {
+		motion_cache->owner = NULL;
+	}
+
+	for (int i = 0; i < slide_colliders.size(); i++) {
+		if (slide_colliders[i].is_valid()) {
+			slide_colliders[i]->owner = NULL;
+		}
+	}
+}
+///////////////////////////////////////
+
+Vector3 KinematicCollision::get_position() const {
+
+	return collision.collision;
+}
+Vector3 KinematicCollision::get_normal() const {
+	return collision.normal;
+}
+Vector3 KinematicCollision::get_travel() const {
+	return collision.travel;
+}
+Vector3 KinematicCollision::get_remainder() const {
+	return collision.remainder;
+}
+Object *KinematicCollision::get_local_shape() const {
+	ERR_FAIL_COND_V(!owner, NULL);
+	uint32_t ownerid = owner->shape_find_owner(collision.local_shape);
+	return owner->shape_owner_get_owner(ownerid);
+}
+
+Object *KinematicCollision::get_collider() const {
+
+	if (collision.collider) {
+		return ObjectDB::get_instance(collision.collider);
+	}
+
+	return NULL;
+}
+ObjectID KinematicCollision::get_collider_id() const {
+
+	return collision.collider;
+}
+Object *KinematicCollision::get_collider_shape() const {
+
+	Object *collider = get_collider();
+	if (collider) {
+		CollisionObject *obj2d = Object::cast_to<CollisionObject>(collider);
+		if (obj2d) {
+			uint32_t ownerid = obj2d->shape_find_owner(collision.collider_shape);
+			return obj2d->shape_owner_get_owner(ownerid);
+		}
+	}
+
+	return NULL;
+}
+int KinematicCollision::get_collider_shape_index() const {
+
+	return collision.collider_shape;
+}
+Vector3 KinematicCollision::get_collider_velocity() const {
+
+	return collision.collider_vel;
+}
+Variant KinematicCollision::get_collider_metadata() const {
+
+	return Variant();
+}
+
+void KinematicCollision::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("get_position"), &KinematicCollision::get_position);
+	ClassDB::bind_method(D_METHOD("get_normal"), &KinematicCollision::get_normal);
+	ClassDB::bind_method(D_METHOD("get_travel"), &KinematicCollision::get_travel);
+	ClassDB::bind_method(D_METHOD("get_remainder"), &KinematicCollision::get_remainder);
+	ClassDB::bind_method(D_METHOD("get_local_shape"), &KinematicCollision::get_local_shape);
+	ClassDB::bind_method(D_METHOD("get_collider"), &KinematicCollision::get_collider);
+	ClassDB::bind_method(D_METHOD("get_collider_id"), &KinematicCollision::get_collider_id);
+	ClassDB::bind_method(D_METHOD("get_collider_shape"), &KinematicCollision::get_collider_shape);
+	ClassDB::bind_method(D_METHOD("get_collider_shape_index"), &KinematicCollision::get_collider_shape_index);
+	ClassDB::bind_method(D_METHOD("get_collider_velocity"), &KinematicCollision::get_collider_velocity);
+	ClassDB::bind_method(D_METHOD("get_collider_metadata"), &KinematicCollision::get_collider_metadata);
+
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "position"), "", "get_position");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "normal"), "", "get_normal");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "travel"), "", "get_travel");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "remainder"), "", "get_remainder");
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "local_shape"), "", "get_local_shape");
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider"), "", "get_collider");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_id"), "", "get_collider_id");
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider_shape"), "", "get_collider_shape");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_shape_index"), "", "get_collider_shape_index");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "collider_velocity"), "", "get_collider_velocity");
+	ADD_PROPERTY(PropertyInfo(Variant::NIL, "collider_metadata", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NIL_IS_VARIANT), "", "get_collider_metadata");
+}
+
+KinematicCollision::KinematicCollision() {
+	collision.collider = 0;
+	collision.collider_shape = 0;
+	collision.local_shape = 0;
+	owner = NULL;
 }

scene/3d/physics_body.h

@@ -261,6 +261,8 @@ public:
 VARIANT_ENUM_CAST(RigidBody::Mode);
 VARIANT_ENUM_CAST(RigidBody::AxisLock);
 
+class KinematicCollision;
+
 class KinematicBody : public PhysicsBody {
 
 	GDCLASS(KinematicBody, PhysicsBody);
@@ -286,42 +288,62 @@ private:
 	bool on_ceiling;
 	bool on_wall;
 	Vector<Collision> colliders;
+	Vector<Ref<KinematicCollision> > slide_colliders;
+	Ref<KinematicCollision> motion_cache;
 
 	_FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const;
 
-	Dictionary _move(const Vector3 &p_motion);
+	Ref<KinematicCollision> _move(const Vector3 &p_motion);
+	Ref<KinematicCollision> _get_slide_collision(int p_bounce);
 
 protected:
 	static void _bind_methods();
 
 public:
-	bool move(const Vector3 &p_motion, Collision &r_collision);
+	bool move_and_collide(const Vector3 &p_motion, Collision &r_collision);
 	bool test_move(const Transform &p_from, const Vector3 &p_motion);
 
 	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_bounces = 4, float p_floor_max_angle = Math::deg2rad((float)45));
+	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 is_on_floor() const;
 	bool is_on_wall() const;
 	bool is_on_ceiling() const;
 	Vector3 get_floor_velocity() const;
 
-	int get_collision_count() const;
-	Vector3 get_collision_position(int p_collision) const;
-	Vector3 get_collision_normal(int p_collision) const;
-	Vector3 get_collision_travel(int p_collision) const;
-	Vector3 get_collision_remainder(int p_collision) const;
-	Object *get_collision_local_shape(int p_collision) const;
-	Object *get_collision_collider(int p_collision) const;
-	ObjectID get_collision_collider_id(int p_collision) const;
-	Object *get_collision_collider_shape(int p_collision) const;
-	int get_collision_collider_shape_index(int p_collision) const;
-	Vector3 get_collision_collider_velocity(int p_collision) const;
-	Variant get_collision_collider_metadata(int p_collision) const;
+	int get_slide_count() const;
+	Collision get_slide_collision(int p_bounce) const;
 
 	KinematicBody();
 	~KinematicBody();
 };
 
+class KinematicCollision : public Reference {
+
+	GDCLASS(KinematicCollision, Reference);
+
+	KinematicBody *owner;
+	friend class KinematicBody;
+	KinematicBody::Collision collision;
+
+protected:
+	static void _bind_methods();
+
+public:
+	Vector3 get_position() const;
+	Vector3 get_normal() const;
+	Vector3 get_travel() const;
+	Vector3 get_remainder() const;
+	Object *get_local_shape() const;
+	Object *get_collider() const;
+	ObjectID get_collider_id() const;
+	Object *get_collider_shape() const;
+	int get_collider_shape_index() const;
+	Vector3 get_collider_velocity() const;
+	Variant get_collider_metadata() const;
+
+	KinematicCollision();
+};
+
 #endif // PHYSICS_BODY__H

scene/register_scene_types.cpp

@@ -378,6 +378,7 @@ void register_scene_types() {
 	ClassDB::register_virtual_class<CollisionObject>();
 	ClassDB::register_class<StaticBody>();
 	ClassDB::register_class<RigidBody>();
+	ClassDB::register_class<KinematicCollision>();
 	ClassDB::register_class<KinematicBody>();
 
 	ClassDB::register_class<VehicleBody>();