diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index 189b1ef9b3..b0b05d1ec8 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -254,7 +254,7 @@ void Basis::set_scale(const Vector3 &p_scale) { set_axis(2, get_axis(2).normalized() * p_scale.z); } -Vector3 Basis::get_scale() const { +Vector3 Basis::get_scale_abs() const { return Vector3( Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), @@ -262,7 +262,13 @@ Vector3 Basis::get_scale() const { Vector3(elements[0][2], elements[1][2], elements[2][2]).length()); } -Vector3 Basis::get_signed_scale() const { +Vector3 Basis::get_scale_local() const { + real_t det_sign = determinant() > 0 ? 1 : -1; + return det_sign * Vector3(elements[0].length(), elements[1].length(), elements[2].length()); +} + +// get_scale works with get_rotation, use get_scale_abs if you need to enforce positive signature. +Vector3 Basis::get_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). @@ -342,6 +348,14 @@ void Basis::rotate(const Vector3 &p_euler) { *this = rotated(p_euler); } +Basis Basis::rotated(const Quat &p_quat) const { + return Basis(p_quat) * (*this); +} + +void Basis::rotate(const Quat &p_quat) { + *this = rotated(p_quat); +} + // TODO: rename this to get_rotation_euler Vector3 Basis::get_rotation() const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, @@ -371,6 +385,22 @@ void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const { m.get_axis_angle(p_axis, p_angle); } +void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const { + // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, + // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). + // See the comment in get_scale() for further information. + Basis m = transposed(); + m.orthonormalize(); + real_t det = m.determinant(); + if (det < 0) { + // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles. + m.scale(Vector3(-1, -1, -1)); + } + + m.get_axis_angle(p_axis, p_angle); + p_angle = -p_angle; +} + // get_euler_xyz returns a vector containing the Euler angles in the format // (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last // (following the convention they are commonly defined in the literature). @@ -767,3 +797,32 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) { elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine; elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z); } + +void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { + set_diagonal(p_scale); + rotate(p_axis, p_phi); +} + +void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) { + set_diagonal(p_scale); + rotate(p_euler); +} + +void Basis::set_quat_scale(const Quat &p_quat, const Vector3 &p_scale) { + set_diagonal(p_scale); + rotate(p_quat); +} + +void Basis::set_diagonal(const Vector3 p_diag) { + elements[0][0] = p_diag.x; + elements[0][1] = 0; + elements[0][2] = 0; + + elements[1][0] = 0; + elements[1][1] = p_diag.y; + elements[1][2] = 0; + + elements[2][0] = 0; + elements[2][1] = 0; + elements[2][2] = p_diag.z; +} diff --git a/core/math/matrix3.h b/core/math/matrix3.h index c426435729..fd383fc673 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -81,8 +81,12 @@ public: void rotate(const Vector3 &p_euler); Basis rotated(const Vector3 &p_euler) const; + void rotate(const Quat &p_quat); + Basis rotated(const Quat &p_quat) const; + Vector3 get_rotation() const; void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const; + void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const; Vector3 rotref_posscale_decomposition(Basis &rotref) const; @@ -108,7 +112,12 @@ public: void set_scale(const Vector3 &p_scale); Vector3 get_scale() const; - Vector3 get_signed_scale() const; + Vector3 get_scale_abs() const; + Vector3 get_scale_local() const; + + void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale); + void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale); + void set_quat_scale(const Quat &p_quat, const Vector3 &p_scale); // transposed dot products _FORCE_INLINE_ real_t tdotx(const Vector3 &v) const { @@ -140,6 +149,8 @@ public: int get_orthogonal_index() const; void set_orthogonal_index(int p_index); + void set_diagonal(const Vector3 p_diag); + bool is_orthogonal() const; bool is_diagonal() const; bool is_rotation() const; @@ -219,6 +230,8 @@ public: Basis(const Quat &p_quat) { set_quat(p_quat); }; Basis(const Vector3 &p_euler) { set_euler(p_euler); } Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); } + Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); } + Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); } _FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) { elements[0] = row0; diff --git a/core/math/transform.cpp b/core/math/transform.cpp index f727d00e30..7cd186ca60 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -119,11 +119,11 @@ 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_signed_scale(); + Vector3 src_scale = basis.get_scale(); Quat src_rot = basis.orthonormalized(); Vector3 src_loc = origin; - Vector3 dst_scale = p_transform.basis.get_signed_scale(); + Vector3 dst_scale = p_transform.basis.get_scale(); Quat dst_rot = p_transform.basis; Vector3 dst_loc = p_transform.origin; diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp index 05c0e653df..57e4db708e 100644 --- a/modules/bullet/collision_object_bullet.cpp +++ b/modules/bullet/collision_object_bullet.cpp @@ -49,7 +49,7 @@ CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {} void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_transform) { - G_TO_B(p_transform.get_basis().get_scale(), scale); + G_TO_B(p_transform.get_basis().get_scale_abs(), scale); G_TO_B(p_transform, transform); UNSCALE_BT_BASIS(transform); } @@ -158,7 +158,7 @@ int CollisionObjectBullet::get_godot_object_flags() const { void CollisionObjectBullet::set_transform(const Transform &p_global_transform) { - set_body_scale(p_global_transform.basis.get_scale()); + set_body_scale(p_global_transform.basis.get_scale_abs()); btTransform bt_transform; G_TO_B(p_global_transform, bt_transform); diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 8450a66f65..3a1f5d78dd 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -122,7 +122,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape); - btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin); + btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); @@ -202,7 +202,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform & ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape); - btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale(), p_margin); + btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); @@ -234,7 +234,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape); - btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale(), p_margin); + btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { bulletdelete(btShape); ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); diff --git a/scene/3d/spatial.cpp b/scene/3d/spatial.cpp index f8a5c7f400..dab0e573d7 100644 --- a/scene/3d/spatial.cpp +++ b/scene/3d/spatial.cpp @@ -85,9 +85,7 @@ void Spatial::_notify_dirty() { } void Spatial::_update_local_transform() const { - data.local_transform.basis = Basis(); - data.local_transform.basis.scale(data.scale); - data.local_transform.basis.rotate(data.rotation); + data.local_transform.basis.set_euler_scale(data.rotation, data.scale); data.dirty &= ~DIRTY_LOCAL; } diff --git a/scene/animation/animation_player.cpp b/scene/animation/animation_player.cpp index 63580bcae6..eca7caeaf0 100644 --- a/scene/animation/animation_player.cpp +++ b/scene/animation/animation_player.cpp @@ -590,9 +590,7 @@ void AnimationPlayer::_animation_update_transforms() { Transform t; t.origin = nc->loc_accum; - t.basis.scale(nc->scale_accum); - t.basis.rotate(nc->rot_accum.get_euler()); - + t.basis.set_quat_scale(nc->rot_accum, nc->scale_accum); if (nc->skeleton && nc->bone_idx >= 0) { nc->skeleton->set_bone_pose(nc->bone_idx, t); diff --git a/scene/animation/animation_tree_player.cpp b/scene/animation/animation_tree_player.cpp index e811b7a7b3..42fa20068b 100644 --- a/scene/animation/animation_tree_player.cpp +++ b/scene/animation/animation_tree_player.cpp @@ -900,8 +900,7 @@ void AnimationTreePlayer::_process_animation(float p_delta) { t.scale.x += 1.0; t.scale.y += 1.0; t.scale.z += 1.0; - xform.basis.scale(t.scale); - xform.basis.rotate(t.rot.get_euler()); + xform.basis.set_quat_scale(t.rot, t.scale); if (t.bone_idx >= 0) { if (t.skeleton)