maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Avoid converting Quat to Euler angles when not necessary.

Browse source 
Also ensure that get_scale doesn't arbitrarlity change the signs of scales, ensuring that the combination of get_rotation and get_scale gives the correct basis.

Added various missing functions and constructors.

Should close #17968.
This commit is contained in:
tagcup 2018-04-14 15:53:25 -04:00
parent e7445c3d82
commit a5e0bb447c
8 changed files with 85 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

63
core/math/matrix3.cpp
View file

@ -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;
}

15
core/math/matrix3.h
View file

@ -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;

4
core/math/transform.cpp
View file

@ -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;

4
modules/bullet/collision_object_bullet.cpp
View file

@ -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);

6
modules/bullet/space_bullet.cpp
View file

@ -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()));

4
scene/3d/spatial.cpp
View file

@ -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;
}

4
scene/animation/animation_player.cpp
View file

@ -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);

3
scene/animation/animation_tree_player.cpp
View file

@ -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)