2014-02-10 02:10:30 +01:00
/*************************************************************************/
2019-02-09 06:24:02 +01:00
/* basis.cpp */
2014-02-10 02:10:30 +01:00
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
2017-08-27 14:16:55 +02:00
/* https://godotengine.org */
2014-02-10 02:10:30 +01:00
/*************************************************************************/
2021-01-01 20:13:46 +01:00
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
2014-02-10 02:10:30 +01:00
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
2018-01-05 00:50:27 +01:00
2019-02-09 06:24:02 +01:00
# include "basis.h"
2018-09-11 18:13:45 +02:00
# include "core/math/math_funcs.h"
# include "core/print_string.h"
2017-03-05 16:44:50 +01:00
# define cofac(row1, col1, row2, col2) \
2014-02-10 02:10:30 +01:00
( elements [ row1 ] [ col1 ] * elements [ row2 ] [ col2 ] - elements [ row1 ] [ col2 ] * elements [ row2 ] [ col1 ] )
2017-03-05 16:44:50 +01:00
void Basis : : from_z ( const Vector3 & p_z ) {
if ( Math : : abs ( p_z . z ) > Math_SQRT12 ) {
2014-02-10 02:10:30 +01:00
// choose p in y-z plane
2017-03-05 16:44:50 +01:00
real_t a = p_z [ 1 ] * p_z [ 1 ] + p_z [ 2 ] * p_z [ 2 ] ;
real_t k = 1.0 / Math : : sqrt ( a ) ;
elements [ 0 ] = Vector3 ( 0 , - p_z [ 2 ] * k , p_z [ 1 ] * k ) ;
elements [ 1 ] = Vector3 ( a * k , - p_z [ 0 ] * elements [ 0 ] [ 2 ] , p_z [ 0 ] * elements [ 0 ] [ 1 ] ) ;
2014-02-10 02:10:30 +01:00
} else {
// choose p in x-y plane
2017-03-05 16:44:50 +01:00
real_t a = p_z . x * p_z . x + p_z . y * p_z . y ;
real_t k = 1.0 / Math : : sqrt ( a ) ;
elements [ 0 ] = Vector3 ( - p_z . y * k , p_z . x * k , 0 ) ;
elements [ 1 ] = Vector3 ( - p_z . z * elements [ 0 ] . y , p_z . z * elements [ 0 ] . x , a * k ) ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
elements [ 2 ] = p_z ;
2014-02-10 02:10:30 +01:00
}
2017-01-11 04:52:51 +01:00
void Basis : : invert ( ) {
2017-03-05 16:44:50 +01:00
real_t co [ 3 ] = {
2014-02-10 02:10:30 +01:00
cofac ( 1 , 1 , 2 , 2 ) , cofac ( 1 , 2 , 2 , 0 ) , cofac ( 1 , 0 , 2 , 1 )
} ;
2017-03-05 16:44:50 +01:00
real_t det = elements [ 0 ] [ 0 ] * co [ 0 ] +
elements [ 0 ] [ 1 ] * co [ 1 ] +
elements [ 0 ] [ 2 ] * co [ 2 ] ;
2017-04-06 00:47:13 +02:00
# ifdef MATH_CHECKS
2017-03-05 16:44:50 +01:00
ERR_FAIL_COND ( det = = 0 ) ;
2017-04-06 00:47:13 +02:00
# endif
2017-03-05 16:44:50 +01:00
real_t s = 1.0 / det ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
set ( co [ 0 ] * s , cofac ( 0 , 2 , 2 , 1 ) * s , cofac ( 0 , 1 , 1 , 2 ) * s ,
co [ 1 ] * s , cofac ( 0 , 0 , 2 , 2 ) * s , cofac ( 0 , 2 , 1 , 0 ) * s ,
co [ 2 ] * s , cofac ( 0 , 1 , 2 , 0 ) * s , cofac ( 0 , 0 , 1 , 1 ) * s ) ;
2014-02-10 02:10:30 +01:00
}
2017-01-11 04:52:51 +01:00
void Basis : : orthonormalize ( ) {
2014-02-10 02:10:30 +01:00
// Gram-Schmidt Process
2017-03-05 16:44:50 +01:00
Vector3 x = get_axis ( 0 ) ;
Vector3 y = get_axis ( 1 ) ;
Vector3 z = get_axis ( 2 ) ;
2014-02-10 02:10:30 +01:00
x . normalize ( ) ;
2017-03-05 16:44:50 +01:00
y = ( y - x * ( x . dot ( y ) ) ) ;
2014-02-10 02:10:30 +01:00
y . normalize ( ) ;
2017-03-05 16:44:50 +01:00
z = ( z - x * ( x . dot ( z ) ) - y * ( y . dot ( z ) ) ) ;
2014-02-10 02:10:30 +01:00
z . normalize ( ) ;
2017-03-05 16:44:50 +01:00
set_axis ( 0 , x ) ;
set_axis ( 1 , y ) ;
set_axis ( 2 , z ) ;
2014-02-10 02:10:30 +01:00
}
2017-01-11 04:52:51 +01:00
Basis Basis : : orthonormalized ( ) const {
Basis c = * this ;
2014-02-10 02:10:30 +01:00
c . orthonormalize ( ) ;
return c ;
}
2017-01-11 04:52:51 +01:00
bool Basis : : is_orthogonal ( ) const {
2019-10-14 22:33:45 +02:00
Basis identity ;
2017-03-05 16:44:50 +01:00
Basis m = ( * this ) * transposed ( ) ;
2016-10-18 22:50:21 +02:00
2019-10-14 22:33:45 +02:00
return m . is_equal_approx ( identity ) ;
2016-10-18 22:50:21 +02:00
}
2017-08-25 21:45:21 +02:00
bool Basis : : is_diagonal ( ) const {
return (
2019-10-14 22:33:45 +02:00
Math : : is_zero_approx ( elements [ 0 ] [ 1 ] ) & & Math : : is_zero_approx ( elements [ 0 ] [ 2 ] ) & &
Math : : is_zero_approx ( elements [ 1 ] [ 0 ] ) & & Math : : is_zero_approx ( elements [ 1 ] [ 2 ] ) & &
Math : : is_zero_approx ( elements [ 2 ] [ 0 ] ) & & Math : : is_zero_approx ( elements [ 2 ] [ 1 ] ) ) ;
2017-08-25 21:45:21 +02:00
}
2017-01-11 04:52:51 +01:00
bool Basis : : is_rotation ( ) const {
2021-05-20 12:26:45 +02:00
return Math : : is_equal_approx ( determinant ( ) , 1 , ( real_t ) UNIT_EPSILON ) & & is_orthogonal ( ) ;
2016-10-18 22:50:21 +02:00
}
2017-01-11 04:52:51 +01:00
bool Basis : : is_symmetric ( ) const {
2021-05-05 12:44:11 +02:00
if ( ! Math : : is_equal_approx_ratio ( elements [ 0 ] [ 1 ] , elements [ 1 ] [ 0 ] , UNIT_EPSILON ) ) {
2016-12-31 15:39:25 +01:00
return false ;
2021-05-05 12:44:11 +02:00
}
if ( ! Math : : is_equal_approx_ratio ( elements [ 0 ] [ 2 ] , elements [ 2 ] [ 0 ] , UNIT_EPSILON ) ) {
2016-12-31 15:39:25 +01:00
return false ;
2021-05-05 12:44:11 +02:00
}
if ( ! Math : : is_equal_approx_ratio ( elements [ 1 ] [ 2 ] , elements [ 2 ] [ 1 ] , UNIT_EPSILON ) ) {
2016-12-31 15:39:25 +01:00
return false ;
2021-05-05 12:44:11 +02:00
}
2016-12-31 15:39:25 +01:00
return true ;
}
2017-01-11 04:52:51 +01:00
Basis Basis : : diagonalize ( ) {
2017-04-06 00:47:13 +02:00
//NOTE: only implemented for symmetric matrices
//with the Jacobi iterative method method
# ifdef MATH_CHECKS
2017-01-11 04:52:51 +01:00
ERR_FAIL_COND_V ( ! is_symmetric ( ) , Basis ( ) ) ;
2017-04-06 00:47:13 +02:00
# endif
2016-12-31 15:39:25 +01:00
const int ite_max = 1024 ;
2017-03-05 16:44:50 +01:00
real_t off_matrix_norm_2 = elements [ 0 ] [ 1 ] * elements [ 0 ] [ 1 ] + elements [ 0 ] [ 2 ] * elements [ 0 ] [ 2 ] + elements [ 1 ] [ 2 ] * elements [ 1 ] [ 2 ] ;
2016-12-31 15:39:25 +01:00
int ite = 0 ;
2017-01-11 04:52:51 +01:00
Basis acc_rot ;
2017-03-05 16:44:50 +01:00
while ( off_matrix_norm_2 > CMP_EPSILON2 & & ite + + < ite_max ) {
2016-12-31 15:39:25 +01:00
real_t el01_2 = elements [ 0 ] [ 1 ] * elements [ 0 ] [ 1 ] ;
real_t el02_2 = elements [ 0 ] [ 2 ] * elements [ 0 ] [ 2 ] ;
real_t el12_2 = elements [ 1 ] [ 2 ] * elements [ 1 ] [ 2 ] ;
// Find the pivot element
int i , j ;
if ( el01_2 > el02_2 ) {
if ( el12_2 > el01_2 ) {
i = 1 ;
j = 2 ;
} else {
i = 0 ;
j = 1 ;
2017-03-05 16:44:50 +01:00
}
2016-12-31 15:39:25 +01:00
} else {
if ( el12_2 > el02_2 ) {
i = 1 ;
j = 2 ;
} else {
i = 0 ;
j = 2 ;
}
}
// Compute the rotation angle
2017-03-05 16:44:50 +01:00
real_t angle ;
2017-04-06 00:47:13 +02:00
if ( Math : : is_equal_approx ( elements [ j ] [ j ] , elements [ i ] [ i ] ) ) {
2016-12-31 15:39:25 +01:00
angle = Math_PI / 4 ;
} else {
2017-03-05 16:44:50 +01:00
angle = 0.5 * Math : : atan ( 2 * elements [ i ] [ j ] / ( elements [ j ] [ j ] - elements [ i ] [ i ] ) ) ;
2016-12-31 15:39:25 +01:00
}
// Compute the rotation matrix
2017-01-11 04:52:51 +01:00
Basis rot ;
2016-12-31 15:39:25 +01:00
rot . elements [ i ] [ i ] = rot . elements [ j ] [ j ] = Math : : cos ( angle ) ;
2017-03-05 16:44:50 +01:00
rot . elements [ i ] [ j ] = - ( rot . elements [ j ] [ i ] = Math : : sin ( angle ) ) ;
2016-12-31 15:39:25 +01:00
// Update the off matrix norm
off_matrix_norm_2 - = elements [ i ] [ j ] * elements [ i ] [ j ] ;
// Apply the rotation
* this = rot * * this * rot . transposed ( ) ;
acc_rot = rot * acc_rot ;
}
return acc_rot ;
}
2017-01-11 04:52:51 +01:00
Basis Basis : : inverse ( ) const {
2017-03-05 16:44:50 +01:00
Basis inv = * this ;
2014-02-10 02:10:30 +01:00
inv . invert ( ) ;
return inv ;
}
2017-01-11 04:52:51 +01:00
void Basis : : transpose ( ) {
2017-03-05 16:44:50 +01:00
SWAP ( elements [ 0 ] [ 1 ] , elements [ 1 ] [ 0 ] ) ;
SWAP ( elements [ 0 ] [ 2 ] , elements [ 2 ] [ 0 ] ) ;
SWAP ( elements [ 1 ] [ 2 ] , elements [ 2 ] [ 1 ] ) ;
2014-02-10 02:10:30 +01:00
}
2017-01-11 04:52:51 +01:00
Basis Basis : : transposed ( ) const {
2017-03-05 16:44:50 +01:00
Basis tr = * this ;
2014-02-10 02:10:30 +01:00
tr . transpose ( ) ;
return tr ;
}
2017-01-05 18:31:39 +01:00
// Multiplies the matrix from left by the scaling matrix: M -> S.M
2017-01-11 04:52:51 +01:00
// See the comment for Basis::rotated for further explanation.
2017-03-05 16:44:50 +01:00
void Basis : : scale ( const Vector3 & p_scale ) {
elements [ 0 ] [ 0 ] * = p_scale . x ;
elements [ 0 ] [ 1 ] * = p_scale . x ;
elements [ 0 ] [ 2 ] * = p_scale . x ;
elements [ 1 ] [ 0 ] * = p_scale . y ;
elements [ 1 ] [ 1 ] * = p_scale . y ;
elements [ 1 ] [ 2 ] * = p_scale . y ;
elements [ 2 ] [ 0 ] * = p_scale . z ;
elements [ 2 ] [ 1 ] * = p_scale . z ;
elements [ 2 ] [ 2 ] * = p_scale . z ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
Basis Basis : : scaled ( const Vector3 & p_scale ) const {
2017-01-11 04:52:51 +01:00
Basis m = * this ;
2014-02-10 02:10:30 +01:00
m . scale ( p_scale ) ;
return m ;
}
Restore the behavior of Spatial rotations recently changed in c1153f5.
That change was borne out of a confusion regarding the meaning of "local" in #14569.
Affine transformations in Spatial simply correspond to affine operations of its Transform. Such operations take place in a coordinate system that is defined by the parent Spatial. When there is no parent, they correspond to operations in the global coordinate system.
This coordinate system, which is relative to the parent, has been referred to as the local coordinate system in the docs so far, but this sloppy language has apparently confused some users, making them think that the local coordinate system refers to the one whose axes are "painted" on the Spatial node itself.
To avoid such conceptual conflations and misunderstandings in the future, the parent-relative local system is now referred to as "parent-local", and the object-relative local system is called "object-local" in the docs.
This commit adds the functionality "requested" in #14569, not by changing how rotate/scale/translate works, but by adding new rotate_object_local, scale_object_local and translate_object_local functions. Also, for completeness, there is now global_scale.
This commit also updates another part of the docs regarding the rotation property of Spatial, which also leads to confusion among some users.
2017-12-27 01:15:20 +01:00
void Basis : : scale_local ( const Vector3 & p_scale ) {
// performs a scaling in object-local coordinate system:
// M -> (M.S.Minv).M = M.S.
* this = scaled_local ( p_scale ) ;
}
Basis Basis : : scaled_local ( const Vector3 & p_scale ) const {
Basis b ;
2018-05-17 02:32:35 +02:00
b . set_diagonal ( p_scale ) ;
Restore the behavior of Spatial rotations recently changed in c1153f5.
That change was borne out of a confusion regarding the meaning of "local" in #14569.
Affine transformations in Spatial simply correspond to affine operations of its Transform. Such operations take place in a coordinate system that is defined by the parent Spatial. When there is no parent, they correspond to operations in the global coordinate system.
This coordinate system, which is relative to the parent, has been referred to as the local coordinate system in the docs so far, but this sloppy language has apparently confused some users, making them think that the local coordinate system refers to the one whose axes are "painted" on the Spatial node itself.
To avoid such conceptual conflations and misunderstandings in the future, the parent-relative local system is now referred to as "parent-local", and the object-relative local system is called "object-local" in the docs.
This commit adds the functionality "requested" in #14569, not by changing how rotate/scale/translate works, but by adding new rotate_object_local, scale_object_local and translate_object_local functions. Also, for completeness, there is now global_scale.
This commit also updates another part of the docs regarding the rotation property of Spatial, which also leads to confusion among some users.
2017-12-27 01:15:20 +01:00
return ( * this ) * b ;
}
2018-04-14 21:53:25 +02:00
Vector3 Basis : : get_scale_abs ( ) const {
2017-09-04 12:48:14 +02:00
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 ( ) ) ;
}
2018-04-14 21:53:25 +02:00
Vector3 Basis : : get_scale_local ( ) const {
2019-02-12 18:55:46 +01:00
real_t det_sign = SGN ( determinant ( ) ) ;
2018-04-14 21:53:25 +02:00
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 {
2017-04-06 00:47:13 +02:00
// 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).
//
// Despite being different from what we want to achieve, we can nevertheless make use of polar decomposition
// here as follows. We can split O into a rotation and a reflection as O = R.Q, and obtain M = R.S where
// we defined S = Q.P. Now, R is a proper rotation matrix and S is a (signed) scaling matrix,
// which can involve negative scalings. However, there is a catch: unlike the polar decomposition of M = O.P,
// the decomposition of O into a rotation and reflection matrix as O = R.Q is not unique.
// Therefore, we are going to do this decomposition by sticking to a particular convention.
// This may lead to confusion for some users though.
//
// The convention we use here is to absorb the sign flip into the scaling matrix.
2017-08-25 21:45:21 +02:00
// The same convention is also used in other similar functions such as get_rotation_axis_angle, get_rotation, ...
2017-04-06 00:47:13 +02:00
//
// A proper way to get rid of this issue would be to store the scaling values (or at least their signs)
// as a part of Basis. However, if we go that path, we need to disable direct (write) access to the
// matrix elements.
2017-08-25 21:45:21 +02:00
//
// The rotation part of this decomposition is returned by get_rotation* functions.
2019-02-12 18:55:46 +01:00
real_t det_sign = SGN ( determinant ( ) ) ;
2017-03-05 16:44:50 +01:00
return det_sign * 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 ( ) ) ;
2014-02-10 02:10:30 +01:00
}
2017-08-25 21:45:21 +02:00
// Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S.
// Returns the rotation-reflection matrix via reference argument, and scaling information is returned as a Vector3.
2017-09-13 23:21:23 +02:00
// This (internal) function is too specific and named too ugly to expose to users, and probably there's no need to do so.
2017-08-25 21:45:21 +02:00
Vector3 Basis : : rotref_posscale_decomposition ( Basis & rotref ) const {
# ifdef MATH_CHECKS
ERR_FAIL_COND_V ( determinant ( ) = = 0 , Vector3 ( ) ) ;
Basis m = transposed ( ) * ( * this ) ;
2018-10-06 22:20:41 +02:00
ERR_FAIL_COND_V ( ! m . is_diagonal ( ) , Vector3 ( ) ) ;
2017-08-25 21:45:21 +02:00
# endif
Vector3 scale = get_scale ( ) ;
Basis inv_scale = Basis ( ) . scaled ( scale . inverse ( ) ) ; // this will also absorb the sign of scale
rotref = ( * this ) * inv_scale ;
# ifdef MATH_CHECKS
2018-10-06 22:20:41 +02:00
ERR_FAIL_COND_V ( ! rotref . is_orthogonal ( ) , Vector3 ( ) ) ;
2017-08-25 21:45:21 +02:00
# endif
return scale . abs ( ) ;
}
2017-01-05 18:31:39 +01:00
// Multiplies the matrix from left by the rotation matrix: M -> R.M
// Note that this does *not* rotate the matrix itself.
//
2017-01-11 04:52:51 +01:00
// The main use of Basis is as Transform.basis, which is used a the transformation matrix
2017-01-05 18:31:39 +01:00
// of 3D object. Rotate here refers to rotation of the object (which is R * (*this)),
// not the matrix itself (which is R * (*this) * R.transposed()).
2017-03-05 16:44:50 +01:00
Basis Basis : : rotated ( const Vector3 & p_axis , real_t p_phi ) const {
2017-01-11 04:52:51 +01:00
return Basis ( p_axis , p_phi ) * ( * this ) ;
2017-01-05 18:31:39 +01:00
}
2017-03-05 16:44:50 +01:00
void Basis : : rotate ( const Vector3 & p_axis , real_t p_phi ) {
2017-01-05 18:31:39 +01:00
* this = rotated ( p_axis , p_phi ) ;
2014-02-10 02:10:30 +01:00
}
2017-12-26 16:44:58 +01:00
void Basis : : rotate_local ( const Vector3 & p_axis , real_t p_phi ) {
Restore the behavior of Spatial rotations recently changed in c1153f5.
That change was borne out of a confusion regarding the meaning of "local" in #14569.
Affine transformations in Spatial simply correspond to affine operations of its Transform. Such operations take place in a coordinate system that is defined by the parent Spatial. When there is no parent, they correspond to operations in the global coordinate system.
This coordinate system, which is relative to the parent, has been referred to as the local coordinate system in the docs so far, but this sloppy language has apparently confused some users, making them think that the local coordinate system refers to the one whose axes are "painted" on the Spatial node itself.
To avoid such conceptual conflations and misunderstandings in the future, the parent-relative local system is now referred to as "parent-local", and the object-relative local system is called "object-local" in the docs.
This commit adds the functionality "requested" in #14569, not by changing how rotate/scale/translate works, but by adding new rotate_object_local, scale_object_local and translate_object_local functions. Also, for completeness, there is now global_scale.
This commit also updates another part of the docs regarding the rotation property of Spatial, which also leads to confusion among some users.
2017-12-27 01:15:20 +01:00
// performs a rotation in object-local coordinate system:
// M -> (M.R.Minv).M = M.R.
2017-12-26 16:44:58 +01:00
* this = rotated_local ( p_axis , p_phi ) ;
}
Basis Basis : : rotated_local ( const Vector3 & p_axis , real_t p_phi ) const {
return ( * this ) * Basis ( p_axis , p_phi ) ;
}
2017-03-05 16:44:50 +01:00
Basis Basis : : rotated ( const Vector3 & p_euler ) const {
2017-01-11 04:52:51 +01:00
return Basis ( p_euler ) * ( * this ) ;
2017-01-05 18:31:39 +01:00
}
2017-03-05 16:44:50 +01:00
void Basis : : rotate ( const Vector3 & p_euler ) {
2017-01-05 18:31:39 +01:00
* this = rotated ( p_euler ) ;
}
2018-04-14 21:53:25 +02:00
Basis Basis : : rotated ( const Quat & p_quat ) const {
return Basis ( p_quat ) * ( * this ) ;
}
void Basis : : rotate ( const Quat & p_quat ) {
* this = rotated ( p_quat ) ;
}
2018-05-27 05:14:05 +02:00
Vector3 Basis : : get_rotation_euler ( ) const {
2017-01-05 18:31:39 +01:00
// 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.
2017-01-11 04:52:51 +01:00
Basis m = orthonormalized ( ) ;
2017-01-05 18:31:39 +01:00
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.
2017-03-05 16:44:50 +01:00
m . scale ( Vector3 ( - 1 , - 1 , - 1 ) ) ;
2017-01-05 18:31:39 +01:00
}
2014-02-10 02:10:30 +01:00
2017-01-05 18:31:39 +01:00
return m . get_euler ( ) ;
2014-02-10 02:10:30 +01:00
}
2018-05-27 05:14:05 +02:00
Quat Basis : : get_rotation_quat ( ) 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 = orthonormalized ( ) ;
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 ) ) ;
}
return m . get_quat ( ) ;
}
2017-04-06 00:47:13 +02:00
void Basis : : get_rotation_axis_angle ( 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 = orthonormalized ( ) ;
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 ) ;
}
2018-04-14 21:53:25 +02:00
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 ;
}
2017-08-09 04:55:52 +02:00
// get_euler_xyz returns a vector containing the Euler angles in the format
2016-10-18 22:50:21 +02:00
// (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).
//
// The current implementation uses XYZ convention (Z is the first rotation),
// so euler.z is the angle of the (first) rotation around Z axis and so on,
//
// And thus, assuming the matrix is a rotation matrix, this function returns
// the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates
// around the z-axis by a and so on.
2017-08-09 04:55:52 +02:00
Vector3 Basis : : get_euler_xyz ( ) const {
2016-10-18 22:50:21 +02:00
// Euler angles in XYZ convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
2014-02-10 02:10:30 +01:00
// rot = cy*cz -cy*sz sy
2016-10-18 22:50:21 +02:00
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
2014-02-10 02:10:30 +01:00
Vector3 euler ;
2017-09-13 23:21:23 +02:00
real_t sy = elements [ 0 ] [ 2 ] ;
2020-06-12 18:39:59 +02:00
if ( sy < ( 1.0 - CMP_EPSILON ) ) {
if ( sy > - ( 1.0 - CMP_EPSILON ) ) {
2017-08-25 21:45:21 +02:00
// is this a pure Y rotation?
2017-08-18 07:29:30 +02:00
if ( elements [ 1 ] [ 0 ] = = 0.0 & & elements [ 0 ] [ 1 ] = = 0.0 & & elements [ 1 ] [ 2 ] = = 0 & & elements [ 2 ] [ 1 ] = = 0 & & elements [ 1 ] [ 1 ] = = 1 ) {
2017-09-13 23:21:23 +02:00
// return the simplest form (human friendlier in editor and scripts)
2017-08-09 04:55:52 +02:00
euler . x = 0 ;
2017-08-18 07:29:30 +02:00
euler . y = atan2 ( elements [ 0 ] [ 2 ] , elements [ 0 ] [ 0 ] ) ;
2017-08-09 04:55:52 +02:00
euler . z = 0 ;
2017-08-07 20:07:43 +02:00
} else {
euler . x = Math : : atan2 ( - elements [ 1 ] [ 2 ] , elements [ 2 ] [ 2 ] ) ;
2017-09-13 23:21:23 +02:00
euler . y = Math : : asin ( sy ) ;
2017-08-07 20:07:43 +02:00
euler . z = Math : : atan2 ( - elements [ 0 ] [ 1 ] , elements [ 0 ] [ 0 ] ) ;
}
2014-02-10 02:10:30 +01:00
} else {
2020-06-12 18:39:59 +02:00
euler . x = Math : : atan2 ( elements [ 2 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
2017-09-13 23:21:23 +02:00
euler . y = - Math_PI / 2.0 ;
2014-02-10 02:10:30 +01:00
euler . z = 0.0 ;
}
} else {
2020-06-12 18:39:59 +02:00
euler . x = Math : : atan2 ( elements [ 2 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
2017-09-13 23:21:23 +02:00
euler . y = Math_PI / 2.0 ;
euler . z = 0.0 ;
2014-02-10 02:10:30 +01:00
}
return euler ;
}
2017-08-09 04:55:52 +02:00
// set_euler_xyz expects a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
2016-10-18 22:50:21 +02:00
// The current implementation uses XYZ convention (Z is the first rotation).
2017-08-09 04:55:52 +02:00
void Basis : : set_euler_xyz ( const Vector3 & p_euler ) {
2014-02-10 02:10:30 +01:00
real_t c , s ;
c = Math : : cos ( p_euler . x ) ;
s = Math : : sin ( p_euler . x ) ;
2017-03-05 16:44:50 +01:00
Basis xmat ( 1.0 , 0.0 , 0.0 , 0.0 , c , - s , 0.0 , s , c ) ;
2014-02-10 02:10:30 +01:00
c = Math : : cos ( p_euler . y ) ;
s = Math : : sin ( p_euler . y ) ;
2017-03-05 16:44:50 +01:00
Basis ymat ( c , 0.0 , s , 0.0 , 1.0 , 0.0 , - s , 0.0 , c ) ;
2014-02-10 02:10:30 +01:00
c = Math : : cos ( p_euler . z ) ;
s = Math : : sin ( p_euler . z ) ;
2017-03-05 16:44:50 +01:00
Basis zmat ( c , - s , 0.0 , s , c , 0.0 , 0.0 , 0.0 , 1.0 ) ;
2014-02-10 02:10:30 +01:00
//optimizer will optimize away all this anyway
2017-03-05 16:44:50 +01:00
* this = xmat * ( ymat * zmat ) ;
2014-02-10 02:10:30 +01:00
}
2020-06-12 18:39:59 +02:00
Vector3 Basis : : get_euler_xzy ( ) const {
// Euler angles in XZY convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cz*cy -sz cz*sy
// sx*sy+cx*cy*sz cx*cz cx*sz*sy-cy*sx
// cy*sx*sz cz*sx cx*cy+sx*sz*sy
Vector3 euler ;
real_t sz = elements [ 0 ] [ 1 ] ;
if ( sz < ( 1.0 - CMP_EPSILON ) ) {
if ( sz > - ( 1.0 - CMP_EPSILON ) ) {
euler . x = Math : : atan2 ( elements [ 2 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
euler . y = Math : : atan2 ( elements [ 0 ] [ 2 ] , elements [ 0 ] [ 0 ] ) ;
euler . z = Math : : asin ( - sz ) ;
} else {
// It's -1
euler . x = - Math : : atan2 ( elements [ 1 ] [ 2 ] , elements [ 2 ] [ 2 ] ) ;
euler . y = 0.0 ;
euler . z = Math_PI / 2.0 ;
}
} else {
// It's 1
euler . x = - Math : : atan2 ( elements [ 1 ] [ 2 ] , elements [ 2 ] [ 2 ] ) ;
euler . y = 0.0 ;
euler . z = - Math_PI / 2.0 ;
}
return euler ;
}
void Basis : : set_euler_xzy ( const Vector3 & p_euler ) {
real_t c , s ;
c = Math : : cos ( p_euler . x ) ;
s = Math : : sin ( p_euler . x ) ;
Basis xmat ( 1.0 , 0.0 , 0.0 , 0.0 , c , - s , 0.0 , s , c ) ;
c = Math : : cos ( p_euler . y ) ;
s = Math : : sin ( p_euler . y ) ;
Basis ymat ( c , 0.0 , s , 0.0 , 1.0 , 0.0 , - s , 0.0 , c ) ;
c = Math : : cos ( p_euler . z ) ;
s = Math : : sin ( p_euler . z ) ;
Basis zmat ( c , - s , 0.0 , s , c , 0.0 , 0.0 , 0.0 , 1.0 ) ;
* this = xmat * zmat * ymat ;
}
Vector3 Basis : : get_euler_yzx ( ) const {
// Euler angles in YZX convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
Vector3 euler ;
real_t sz = elements [ 1 ] [ 0 ] ;
if ( sz < ( 1.0 - CMP_EPSILON ) ) {
if ( sz > - ( 1.0 - CMP_EPSILON ) ) {
euler . x = Math : : atan2 ( - elements [ 1 ] [ 2 ] , elements [ 1 ] [ 1 ] ) ;
euler . y = Math : : atan2 ( - elements [ 2 ] [ 0 ] , elements [ 0 ] [ 0 ] ) ;
euler . z = Math : : asin ( sz ) ;
} else {
// It's -1
euler . x = Math : : atan2 ( elements [ 2 ] [ 1 ] , elements [ 2 ] [ 2 ] ) ;
euler . y = 0.0 ;
euler . z = - Math_PI / 2.0 ;
}
} else {
// It's 1
euler . x = Math : : atan2 ( elements [ 2 ] [ 1 ] , elements [ 2 ] [ 2 ] ) ;
euler . y = 0.0 ;
euler . z = Math_PI / 2.0 ;
}
return euler ;
}
void Basis : : set_euler_yzx ( const Vector3 & p_euler ) {
real_t c , s ;
c = Math : : cos ( p_euler . x ) ;
s = Math : : sin ( p_euler . x ) ;
Basis xmat ( 1.0 , 0.0 , 0.0 , 0.0 , c , - s , 0.0 , s , c ) ;
c = Math : : cos ( p_euler . y ) ;
s = Math : : sin ( p_euler . y ) ;
Basis ymat ( c , 0.0 , s , 0.0 , 1.0 , 0.0 , - s , 0.0 , c ) ;
c = Math : : cos ( p_euler . z ) ;
s = Math : : sin ( p_euler . z ) ;
Basis zmat ( c , - s , 0.0 , s , c , 0.0 , 0.0 , 0.0 , 1.0 ) ;
* this = ymat * zmat * xmat ;
}
2017-08-09 04:55:52 +02:00
// get_euler_yxz returns a vector containing the Euler angles in the YXZ convention,
// as in first-Z, then-X, last-Y. The angles for X, Y, and Z rotations are returned
// as the x, y, and z components of a Vector3 respectively.
Vector3 Basis : : get_euler_yxz ( ) const {
// Euler angles in YXZ convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
// cx*sz cx*cz -sx
// cy*sx*sz-cz*sy cy*cz*sx+sy*sz cy*cx
Vector3 euler ;
2019-02-26 01:46:24 +01:00
2017-08-09 04:55:52 +02:00
real_t m12 = elements [ 1 ] [ 2 ] ;
2020-06-12 18:39:59 +02:00
if ( m12 < ( 1 - CMP_EPSILON ) ) {
if ( m12 > - ( 1 - CMP_EPSILON ) ) {
2017-08-25 21:45:21 +02:00
// is this a pure X rotation?
if ( elements [ 1 ] [ 0 ] = = 0 & & elements [ 0 ] [ 1 ] = = 0 & & elements [ 0 ] [ 2 ] = = 0 & & elements [ 2 ] [ 0 ] = = 0 & & elements [ 0 ] [ 0 ] = = 1 ) {
2017-09-13 23:21:23 +02:00
// return the simplest form (human friendlier in editor and scripts)
2017-08-18 07:29:30 +02:00
euler . x = atan2 ( - m12 , elements [ 1 ] [ 1 ] ) ;
2017-08-09 04:55:52 +02:00
euler . y = 0 ;
euler . z = 0 ;
} else {
euler . x = asin ( - m12 ) ;
euler . y = atan2 ( elements [ 0 ] [ 2 ] , elements [ 2 ] [ 2 ] ) ;
euler . z = atan2 ( elements [ 1 ] [ 0 ] , elements [ 1 ] [ 1 ] ) ;
}
} else { // m12 == -1
euler . x = Math_PI * 0.5 ;
2020-06-12 18:39:59 +02:00
euler . y = atan2 ( elements [ 0 ] [ 1 ] , elements [ 0 ] [ 0 ] ) ;
2017-08-09 04:55:52 +02:00
euler . z = 0 ;
}
} else { // m12 == 1
euler . x = - Math_PI * 0.5 ;
2020-06-12 18:39:59 +02:00
euler . y = - atan2 ( elements [ 0 ] [ 1 ] , elements [ 0 ] [ 0 ] ) ;
2017-08-09 04:55:52 +02:00
euler . z = 0 ;
}
return euler ;
}
// set_euler_yxz expects a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// The current implementation uses YXZ convention (Z is the first rotation).
void Basis : : set_euler_yxz ( const Vector3 & p_euler ) {
real_t c , s ;
c = Math : : cos ( p_euler . x ) ;
s = Math : : sin ( p_euler . x ) ;
Basis xmat ( 1.0 , 0.0 , 0.0 , 0.0 , c , - s , 0.0 , s , c ) ;
c = Math : : cos ( p_euler . y ) ;
s = Math : : sin ( p_euler . y ) ;
Basis ymat ( c , 0.0 , s , 0.0 , 1.0 , 0.0 , - s , 0.0 , c ) ;
c = Math : : cos ( p_euler . z ) ;
s = Math : : sin ( p_euler . z ) ;
Basis zmat ( c , - s , 0.0 , s , c , 0.0 , 0.0 , 0.0 , 1.0 ) ;
//optimizer will optimize away all this anyway
* this = ymat * xmat * zmat ;
}
2020-06-12 18:39:59 +02:00
Vector3 Basis : : get_euler_zxy ( ) const {
// Euler angles in ZXY convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
// cy*sz+cz*sx*sy cz*cx sz*sy-cz*cy*sx
// -cx*sy sx cx*cy
Vector3 euler ;
real_t sx = elements [ 2 ] [ 1 ] ;
if ( sx < ( 1.0 - CMP_EPSILON ) ) {
if ( sx > - ( 1.0 - CMP_EPSILON ) ) {
euler . x = Math : : asin ( sx ) ;
euler . y = Math : : atan2 ( - elements [ 2 ] [ 0 ] , elements [ 2 ] [ 2 ] ) ;
euler . z = Math : : atan2 ( - elements [ 0 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
} else {
// It's -1
euler . x = - Math_PI / 2.0 ;
euler . y = Math : : atan2 ( elements [ 0 ] [ 2 ] , elements [ 0 ] [ 0 ] ) ;
euler . z = 0 ;
}
} else {
// It's 1
euler . x = Math_PI / 2.0 ;
euler . y = Math : : atan2 ( elements [ 0 ] [ 2 ] , elements [ 0 ] [ 0 ] ) ;
euler . z = 0 ;
}
return euler ;
}
void Basis : : set_euler_zxy ( const Vector3 & p_euler ) {
real_t c , s ;
c = Math : : cos ( p_euler . x ) ;
s = Math : : sin ( p_euler . x ) ;
Basis xmat ( 1.0 , 0.0 , 0.0 , 0.0 , c , - s , 0.0 , s , c ) ;
c = Math : : cos ( p_euler . y ) ;
s = Math : : sin ( p_euler . y ) ;
Basis ymat ( c , 0.0 , s , 0.0 , 1.0 , 0.0 , - s , 0.0 , c ) ;
c = Math : : cos ( p_euler . z ) ;
s = Math : : sin ( p_euler . z ) ;
Basis zmat ( c , - s , 0.0 , s , c , 0.0 , 0.0 , 0.0 , 1.0 ) ;
* this = zmat * xmat * ymat ;
}
Vector3 Basis : : get_euler_zyx ( ) const {
// Euler angles in ZYX convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*cy
// cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
// -sy cy*sx cy*cx
Vector3 euler ;
real_t sy = elements [ 2 ] [ 0 ] ;
if ( sy < ( 1.0 - CMP_EPSILON ) ) {
if ( sy > - ( 1.0 - CMP_EPSILON ) ) {
euler . x = Math : : atan2 ( elements [ 2 ] [ 1 ] , elements [ 2 ] [ 2 ] ) ;
euler . y = Math : : asin ( - sy ) ;
euler . z = Math : : atan2 ( elements [ 1 ] [ 0 ] , elements [ 0 ] [ 0 ] ) ;
} else {
// It's -1
euler . x = 0 ;
euler . y = Math_PI / 2.0 ;
euler . z = - Math : : atan2 ( elements [ 0 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
}
} else {
// It's 1
euler . x = 0 ;
euler . y = - Math_PI / 2.0 ;
euler . z = - Math : : atan2 ( elements [ 0 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
}
return euler ;
}
void Basis : : set_euler_zyx ( const Vector3 & p_euler ) {
real_t c , s ;
c = Math : : cos ( p_euler . x ) ;
s = Math : : sin ( p_euler . x ) ;
Basis xmat ( 1.0 , 0.0 , 0.0 , 0.0 , c , - s , 0.0 , s , c ) ;
c = Math : : cos ( p_euler . y ) ;
s = Math : : sin ( p_euler . y ) ;
Basis ymat ( c , 0.0 , s , 0.0 , 1.0 , 0.0 , - s , 0.0 , c ) ;
c = Math : : cos ( p_euler . z ) ;
s = Math : : sin ( p_euler . z ) ;
Basis zmat ( c , - s , 0.0 , s , c , 0.0 , 0.0 , 0.0 , 1.0 ) ;
* this = zmat * ymat * xmat ;
}
2019-10-14 22:33:45 +02:00
bool Basis : : is_equal_approx ( const Basis & p_basis ) const {
return elements [ 0 ] . is_equal_approx ( p_basis . elements [ 0 ] ) & & elements [ 1 ] . is_equal_approx ( p_basis . elements [ 1 ] ) & & elements [ 2 ] . is_equal_approx ( p_basis . elements [ 2 ] ) ;
2019-04-01 16:11:02 +02:00
}
2019-04-09 17:08:36 +02:00
bool Basis : : is_equal_approx_ratio ( const Basis & a , const Basis & b , real_t p_epsilon ) const {
2019-04-01 16:11:02 +02:00
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int j = 0 ; j < 3 ; j + + ) {
2021-05-05 12:44:11 +02:00
if ( ! Math : : is_equal_approx_ratio ( a . elements [ i ] [ j ] , b . elements [ i ] [ j ] , p_epsilon ) ) {
2017-03-05 16:44:50 +01:00
return false ;
2021-05-05 12:44:11 +02:00
}
2017-03-05 16:44:50 +01:00
}
}
2016-10-18 22:50:21 +02:00
2017-03-05 16:44:50 +01:00
return true ;
2016-10-18 22:50:21 +02:00
}
2017-03-05 16:44:50 +01:00
bool Basis : : operator = = ( const Basis & p_matrix ) const {
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int j = 0 ; j < 3 ; j + + ) {
2021-05-05 12:44:11 +02:00
if ( elements [ i ] [ j ] ! = p_matrix . elements [ i ] [ j ] ) {
2014-02-10 02:10:30 +01:00
return false ;
2021-05-05 12:44:11 +02:00
}
2014-02-10 02:10:30 +01:00
}
}
2016-03-09 00:00:52 +01:00
2014-02-10 02:10:30 +01:00
return true ;
}
2016-10-18 22:50:21 +02:00
2017-03-05 16:44:50 +01:00
bool Basis : : operator ! = ( const Basis & p_matrix ) const {
return ( ! ( * this = = p_matrix ) ) ;
2014-02-10 02:10:30 +01:00
}
2017-01-11 04:52:51 +01:00
Basis : : operator String ( ) const {
2014-02-10 02:10:30 +01:00
String mtx ;
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int j = 0 ; j < 3 ; j + + ) {
2021-05-05 12:44:11 +02:00
if ( i ! = 0 | | j ! = 0 ) {
2017-03-05 16:44:50 +01:00
mtx + = " , " ;
2021-05-05 12:44:11 +02:00
}
2016-03-09 00:00:52 +01:00
2017-03-05 16:44:50 +01:00
mtx + = rtos ( elements [ i ] [ j ] ) ;
2014-02-10 02:10:30 +01:00
}
}
2016-03-09 00:00:52 +01:00
2014-02-10 02:10:30 +01:00
return mtx ;
}
2017-10-10 02:51:45 +02:00
Quat Basis : : get_quat ( ) const {
2019-04-01 16:11:02 +02:00
# ifdef MATH_CHECKS
2021-08-29 18:52:59 +02:00
ERR_FAIL_COND_V_MSG ( ! is_rotation ( ) , Quat ( ) , " Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() if the Basis contains linearly independent vectors. " ) ;
2019-04-01 16:11:02 +02:00
# endif
2019-02-26 01:46:24 +01:00
/* Allow getting a quaternion from an unnormalized transform */
Basis m = * this ;
real_t trace = m . elements [ 0 ] [ 0 ] + m . elements [ 1 ] [ 1 ] + m . elements [ 2 ] [ 2 ] ;
2014-02-10 02:10:30 +01:00
real_t temp [ 4 ] ;
2016-03-09 00:00:52 +01:00
2017-03-05 16:44:50 +01:00
if ( trace > 0.0 ) {
2014-02-10 02:10:30 +01:00
real_t s = Math : : sqrt ( trace + 1.0 ) ;
2017-03-05 16:44:50 +01:00
temp [ 3 ] = ( s * 0.5 ) ;
2014-02-10 02:10:30 +01:00
s = 0.5 / s ;
2016-03-09 00:00:52 +01:00
2019-02-26 01:46:24 +01:00
temp [ 0 ] = ( ( m . elements [ 2 ] [ 1 ] - m . elements [ 1 ] [ 2 ] ) * s ) ;
temp [ 1 ] = ( ( m . elements [ 0 ] [ 2 ] - m . elements [ 2 ] [ 0 ] ) * s ) ;
temp [ 2 ] = ( ( m . elements [ 1 ] [ 0 ] - m . elements [ 0 ] [ 1 ] ) * s ) ;
2017-03-05 16:44:50 +01:00
} else {
2019-02-26 01:46:24 +01:00
int i = m . elements [ 0 ] [ 0 ] < m . elements [ 1 ] [ 1 ] ?
2021-01-12 16:57:55 +01:00
( m . elements [ 1 ] [ 1 ] < m . elements [ 2 ] [ 2 ] ? 2 : 1 ) :
( m . elements [ 0 ] [ 0 ] < m . elements [ 2 ] [ 2 ] ? 2 : 0 ) ;
2016-03-09 00:00:52 +01:00
int j = ( i + 1 ) % 3 ;
2014-02-10 02:10:30 +01:00
int k = ( i + 2 ) % 3 ;
2016-03-09 00:00:52 +01:00
2019-02-26 01:46:24 +01:00
real_t s = Math : : sqrt ( m . elements [ i ] [ i ] - m . elements [ j ] [ j ] - m . elements [ k ] [ k ] + 1.0 ) ;
2014-02-10 02:10:30 +01:00
temp [ i ] = s * 0.5 ;
s = 0.5 / s ;
2016-03-09 00:00:52 +01:00
2019-02-26 01:46:24 +01:00
temp [ 3 ] = ( m . elements [ k ] [ j ] - m . elements [ j ] [ k ] ) * s ;
temp [ j ] = ( m . elements [ j ] [ i ] + m . elements [ i ] [ j ] ) * s ;
temp [ k ] = ( m . elements [ k ] [ i ] + m . elements [ i ] [ k ] ) * s ;
2014-02-10 02:10:30 +01:00
}
2016-03-09 00:00:52 +01:00
2017-03-05 16:44:50 +01:00
return Quat ( temp [ 0 ] , temp [ 1 ] , temp [ 2 ] , temp [ 3 ] ) ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
static const Basis _ortho_bases [ 24 ] = {
2017-01-11 04:52:51 +01:00
Basis ( 1 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 1 ) ,
Basis ( 0 , - 1 , 0 , 1 , 0 , 0 , 0 , 0 , 1 ) ,
Basis ( - 1 , 0 , 0 , 0 , - 1 , 0 , 0 , 0 , 1 ) ,
Basis ( 0 , 1 , 0 , - 1 , 0 , 0 , 0 , 0 , 1 ) ,
Basis ( 1 , 0 , 0 , 0 , 0 , - 1 , 0 , 1 , 0 ) ,
Basis ( 0 , 0 , 1 , 1 , 0 , 0 , 0 , 1 , 0 ) ,
Basis ( - 1 , 0 , 0 , 0 , 0 , 1 , 0 , 1 , 0 ) ,
Basis ( 0 , 0 , - 1 , - 1 , 0 , 0 , 0 , 1 , 0 ) ,
Basis ( 1 , 0 , 0 , 0 , - 1 , 0 , 0 , 0 , - 1 ) ,
Basis ( 0 , 1 , 0 , 1 , 0 , 0 , 0 , 0 , - 1 ) ,
Basis ( - 1 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , - 1 ) ,
Basis ( 0 , - 1 , 0 , - 1 , 0 , 0 , 0 , 0 , - 1 ) ,
Basis ( 1 , 0 , 0 , 0 , 0 , 1 , 0 , - 1 , 0 ) ,
Basis ( 0 , 0 , - 1 , 1 , 0 , 0 , 0 , - 1 , 0 ) ,
Basis ( - 1 , 0 , 0 , 0 , 0 , - 1 , 0 , - 1 , 0 ) ,
Basis ( 0 , 0 , 1 , - 1 , 0 , 0 , 0 , - 1 , 0 ) ,
Basis ( 0 , 0 , 1 , 0 , 1 , 0 , - 1 , 0 , 0 ) ,
Basis ( 0 , - 1 , 0 , 0 , 0 , 1 , - 1 , 0 , 0 ) ,
Basis ( 0 , 0 , - 1 , 0 , - 1 , 0 , - 1 , 0 , 0 ) ,
Basis ( 0 , 1 , 0 , 0 , 0 , - 1 , - 1 , 0 , 0 ) ,
Basis ( 0 , 0 , 1 , 0 , - 1 , 0 , 1 , 0 , 0 ) ,
Basis ( 0 , 1 , 0 , 0 , 0 , 1 , 1 , 0 , 0 ) ,
Basis ( 0 , 0 , - 1 , 0 , 1 , 0 , 1 , 0 , 0 ) ,
Basis ( 0 , - 1 , 0 , 0 , 0 , - 1 , 1 , 0 , 0 )
2014-02-10 02:10:30 +01:00
} ;
2017-01-11 04:52:51 +01:00
int Basis : : get_orthogonal_index ( ) const {
2014-02-10 02:10:30 +01:00
//could be sped up if i come up with a way
2017-03-05 16:44:50 +01:00
Basis orth = * this ;
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int j = 0 ; j < 3 ; j + + ) {
2017-01-05 18:31:39 +01:00
real_t v = orth [ i ] [ j ] ;
2021-05-05 12:44:11 +02:00
if ( v > 0.5 ) {
2017-03-05 16:44:50 +01:00
v = 1.0 ;
2021-05-05 12:44:11 +02:00
} else if ( v < - 0.5 ) {
2017-03-05 16:44:50 +01:00
v = - 1.0 ;
2021-05-05 12:44:11 +02:00
} else {
2017-03-05 16:44:50 +01:00
v = 0 ;
2021-05-05 12:44:11 +02:00
}
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
orth [ i ] [ j ] = v ;
2014-02-10 02:10:30 +01:00
}
}
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < 24 ; i + + ) {
2021-05-05 12:44:11 +02:00
if ( _ortho_bases [ i ] = = orth ) {
2014-02-10 02:10:30 +01:00
return i ;
2021-05-05 12:44:11 +02:00
}
2014-02-10 02:10:30 +01:00
}
return 0 ;
}
2017-03-05 16:44:50 +01:00
void Basis : : set_orthogonal_index ( int p_index ) {
2014-02-10 02:10:30 +01:00
//there only exist 24 orthogonal bases in r3
2017-03-05 16:44:50 +01:00
ERR_FAIL_INDEX ( p_index , 24 ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
* this = _ortho_bases [ p_index ] ;
2014-02-10 02:10:30 +01:00
}
2017-04-06 00:47:13 +02:00
void Basis : : get_axis_angle ( Vector3 & r_axis , real_t & r_angle ) const {
2019-02-26 01:46:24 +01:00
/* checking this is a bad idea, because obtaining from scaled transform is a valid use case
2017-04-06 00:47:13 +02:00
# ifdef MATH_CHECKS
2018-10-06 22:20:41 +02:00
ERR_FAIL_COND ( ! is_rotation ( ) ) ;
2017-04-06 00:47:13 +02:00
# endif
2019-02-26 01:46:24 +01:00
*/
2017-03-05 16:44:50 +01:00
real_t angle , x , y , z ; // variables for result
real_t epsilon = 0.01 ; // margin to allow for rounding errors
real_t epsilon2 = 0.1 ; // margin to distinguish between 0 and 180 degrees
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
if ( ( Math : : abs ( elements [ 1 ] [ 0 ] - elements [ 0 ] [ 1 ] ) < epsilon ) & & ( Math : : abs ( elements [ 2 ] [ 0 ] - elements [ 0 ] [ 2 ] ) < epsilon ) & & ( Math : : abs ( elements [ 2 ] [ 1 ] - elements [ 1 ] [ 2 ] ) < epsilon ) ) {
// singularity found
// first check for identity matrix which must have +1 for all terms
// in leading diagonaland zero in other terms
if ( ( Math : : abs ( elements [ 1 ] [ 0 ] + elements [ 0 ] [ 1 ] ) < epsilon2 ) & & ( Math : : abs ( elements [ 2 ] [ 0 ] + elements [ 0 ] [ 2 ] ) < epsilon2 ) & & ( Math : : abs ( elements [ 2 ] [ 1 ] + elements [ 1 ] [ 2 ] ) < epsilon2 ) & & ( Math : : abs ( elements [ 0 ] [ 0 ] + elements [ 1 ] [ 1 ] + elements [ 2 ] [ 2 ] - 3 ) < epsilon2 ) ) {
2014-02-10 02:10:30 +01:00
// this singularity is identity matrix so angle = 0
2017-03-05 16:44:50 +01:00
r_axis = Vector3 ( 0 , 1 , 0 ) ;
r_angle = 0 ;
2014-02-10 02:10:30 +01:00
return ;
}
// otherwise this singularity is angle = 180
angle = Math_PI ;
2017-03-05 16:44:50 +01:00
real_t xx = ( elements [ 0 ] [ 0 ] + 1 ) / 2 ;
real_t yy = ( elements [ 1 ] [ 1 ] + 1 ) / 2 ;
real_t zz = ( elements [ 2 ] [ 2 ] + 1 ) / 2 ;
real_t xy = ( elements [ 1 ] [ 0 ] + elements [ 0 ] [ 1 ] ) / 4 ;
real_t xz = ( elements [ 2 ] [ 0 ] + elements [ 0 ] [ 2 ] ) / 4 ;
real_t yz = ( elements [ 2 ] [ 1 ] + elements [ 1 ] [ 2 ] ) / 4 ;
2014-02-10 02:10:30 +01:00
if ( ( xx > yy ) & & ( xx > zz ) ) { // elements[0][0] is the largest diagonal term
2017-03-05 16:44:50 +01:00
if ( xx < epsilon ) {
2014-02-10 02:10:30 +01:00
x = 0 ;
2019-12-10 05:13:02 +01:00
y = Math_SQRT12 ;
z = Math_SQRT12 ;
2014-02-10 02:10:30 +01:00
} else {
x = Math : : sqrt ( xx ) ;
2017-03-05 16:44:50 +01:00
y = xy / x ;
z = xz / x ;
2014-02-10 02:10:30 +01:00
}
} else if ( yy > zz ) { // elements[1][1] is the largest diagonal term
2017-03-05 16:44:50 +01:00
if ( yy < epsilon ) {
2019-12-10 05:13:02 +01:00
x = Math_SQRT12 ;
2014-02-10 02:10:30 +01:00
y = 0 ;
2019-12-10 05:13:02 +01:00
z = Math_SQRT12 ;
2014-02-10 02:10:30 +01:00
} else {
y = Math : : sqrt ( yy ) ;
2017-03-05 16:44:50 +01:00
x = xy / y ;
z = yz / y ;
2014-02-10 02:10:30 +01:00
}
} else { // elements[2][2] is the largest diagonal term so base result on this
2017-03-05 16:44:50 +01:00
if ( zz < epsilon ) {
2019-12-10 05:13:02 +01:00
x = Math_SQRT12 ;
y = Math_SQRT12 ;
2014-02-10 02:10:30 +01:00
z = 0 ;
} else {
z = Math : : sqrt ( zz ) ;
2017-03-05 16:44:50 +01:00
x = xz / z ;
y = yz / z ;
2014-02-10 02:10:30 +01:00
}
}
2017-03-05 16:44:50 +01:00
r_axis = Vector3 ( x , y , z ) ;
r_angle = angle ;
2014-02-10 02:10:30 +01:00
return ;
}
// as we have reached here there are no singularities so we can handle normally
2017-03-05 16:44:50 +01:00
real_t s = Math : : sqrt ( ( elements [ 1 ] [ 2 ] - elements [ 2 ] [ 1 ] ) * ( elements [ 1 ] [ 2 ] - elements [ 2 ] [ 1 ] ) + ( elements [ 2 ] [ 0 ] - elements [ 0 ] [ 2 ] ) * ( elements [ 2 ] [ 0 ] - elements [ 0 ] [ 2 ] ) + ( elements [ 0 ] [ 1 ] - elements [ 1 ] [ 0 ] ) * ( elements [ 0 ] [ 1 ] - elements [ 1 ] [ 0 ] ) ) ; // s=|axis||sin(angle)|, used to normalise
2016-10-18 22:50:21 +02:00
2017-03-05 16:44:50 +01:00
angle = Math : : acos ( ( elements [ 0 ] [ 0 ] + elements [ 1 ] [ 1 ] + elements [ 2 ] [ 2 ] - 1 ) / 2 ) ;
2021-05-05 12:44:11 +02:00
if ( angle < 0 ) {
2021-05-04 14:28:27 +02:00
s = - s ;
2021-05-05 12:44:11 +02:00
}
2017-03-05 16:44:50 +01:00
x = ( elements [ 2 ] [ 1 ] - elements [ 1 ] [ 2 ] ) / s ;
y = ( elements [ 0 ] [ 2 ] - elements [ 2 ] [ 0 ] ) / s ;
z = ( elements [ 1 ] [ 0 ] - elements [ 0 ] [ 1 ] ) / s ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
r_axis = Vector3 ( x , y , z ) ;
r_angle = angle ;
2014-02-10 02:10:30 +01:00
}
2017-10-10 02:51:45 +02:00
void Basis : : set_quat ( const Quat & p_quat ) {
2014-02-10 02:10:30 +01:00
real_t d = p_quat . length_squared ( ) ;
real_t s = 2.0 / d ;
2017-03-05 16:44:50 +01:00
real_t xs = p_quat . x * s , ys = p_quat . y * s , zs = p_quat . z * s ;
real_t wx = p_quat . w * xs , wy = p_quat . w * ys , wz = p_quat . w * zs ;
real_t xx = p_quat . x * xs , xy = p_quat . x * ys , xz = p_quat . x * zs ;
real_t yy = p_quat . y * ys , yz = p_quat . y * zs , zz = p_quat . z * zs ;
set ( 1.0 - ( yy + zz ) , xy - wz , xz + wy ,
xy + wz , 1.0 - ( xx + zz ) , yz - wx ,
xz - wy , yz + wx , 1.0 - ( xx + yy ) ) ;
2014-02-10 02:10:30 +01:00
}
2017-04-06 00:47:13 +02:00
void Basis : : set_axis_angle ( const Vector3 & p_axis , real_t p_phi ) {
// Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle
# ifdef MATH_CHECKS
2020-01-24 13:51:25 +01:00
ERR_FAIL_COND_MSG ( ! p_axis . is_normalized ( ) , " The axis Vector3 must be normalized. " ) ;
2017-04-06 00:47:13 +02:00
# endif
2017-03-05 16:44:50 +01:00
Vector3 axis_sq ( p_axis . x * p_axis . x , p_axis . y * p_axis . y , p_axis . z * p_axis . z ) ;
real_t cosine = Math : : cos ( p_phi ) ;
elements [ 0 ] [ 0 ] = axis_sq . x + cosine * ( 1.0 - axis_sq . x ) ;
elements [ 1 ] [ 1 ] = axis_sq . y + cosine * ( 1.0 - axis_sq . y ) ;
elements [ 2 ] [ 2 ] = axis_sq . z + cosine * ( 1.0 - axis_sq . z ) ;
2019-05-17 11:11:12 +02:00
real_t sine = Math : : sin ( p_phi ) ;
real_t t = 1 - cosine ;
real_t xyzt = p_axis . x * p_axis . y * t ;
real_t zyxs = p_axis . z * sine ;
elements [ 0 ] [ 1 ] = xyzt - zyxs ;
elements [ 1 ] [ 0 ] = xyzt + zyxs ;
xyzt = p_axis . x * p_axis . z * t ;
zyxs = p_axis . y * sine ;
elements [ 0 ] [ 2 ] = xyzt + zyxs ;
elements [ 2 ] [ 0 ] = xyzt - zyxs ;
xyzt = p_axis . y * p_axis . z * t ;
zyxs = p_axis . x * sine ;
elements [ 1 ] [ 2 ] = xyzt - zyxs ;
elements [ 2 ] [ 1 ] = xyzt + zyxs ;
2014-02-10 02:10:30 +01:00
}
2018-04-14 21:53:25 +02:00
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 ) ;
}
2019-07-10 11:54:12 +02:00
void Basis : : set_diagonal ( const Vector3 & p_diag ) {
2018-04-14 21:53:25 +02:00
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 ;
}
2018-05-12 02:14:39 +02:00
2021-04-22 19:59:16 +02:00
Basis Basis : : slerp ( const Basis & p_to , const real_t & p_weight ) const {
2019-02-26 01:46:24 +01:00
//consider scale
2018-05-12 02:14:39 +02:00
Quat from ( * this ) ;
2021-04-22 19:59:16 +02:00
Quat to ( p_to ) ;
2018-05-12 02:14:39 +02:00
2021-04-22 19:59:16 +02:00
Basis b ( from . slerp ( to , p_weight ) ) ;
b . elements [ 0 ] * = Math : : lerp ( elements [ 0 ] . length ( ) , p_to . elements [ 0 ] . length ( ) , p_weight ) ;
b . elements [ 1 ] * = Math : : lerp ( elements [ 1 ] . length ( ) , p_to . elements [ 1 ] . length ( ) , p_weight ) ;
b . elements [ 2 ] * = Math : : lerp ( elements [ 2 ] . length ( ) , p_to . elements [ 2 ] . length ( ) , p_weight ) ;
2019-02-26 01:46:24 +01:00
return b ;
2018-05-12 02:14:39 +02:00
}