2014-02-10 02:10:30 +01:00
/*************************************************************************/
/* matrix3.cpp */
/*************************************************************************/
/* 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
/*************************************************************************/
2017-01-01 22:01:57 +01:00
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
2017-04-08 00:11:42 +02:00
/* Copyright (c) 2014-2017 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. */
/*************************************************************************/
# include "matrix3.h"
# include "math_funcs.h"
# include "os/copymem.h"
2017-08-07 20:07:43 +02:00
# include "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 ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
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 ( ) {
2014-02-10 02:10:30 +01:00
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 ( ) {
2017-04-06 00:47:13 +02:00
# ifdef MATH_CHECKS
2016-10-18 22:50:21 +02:00
ERR_FAIL_COND ( determinant ( ) = = 0 ) ;
2017-04-06 00:47:13 +02:00
# endif
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 {
2014-02-10 02:10:30 +01:00
2017-01-11 04:52:51 +01:00
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 {
Basis id ;
2017-03-05 16:44:50 +01:00
Basis m = ( * this ) * transposed ( ) ;
2016-10-18 22:50:21 +02:00
2017-04-06 00:47:13 +02:00
return is_equal_approx ( id , m ) ;
2016-10-18 22:50:21 +02:00
}
2017-08-25 21:45:21 +02:00
bool Basis : : is_diagonal ( ) const {
return (
Math : : is_equal_approx ( elements [ 0 ] [ 1 ] , 0 ) & & Math : : is_equal_approx ( elements [ 0 ] [ 2 ] , 0 ) & &
Math : : is_equal_approx ( elements [ 1 ] [ 0 ] , 0 ) & & Math : : is_equal_approx ( elements [ 1 ] [ 2 ] , 0 ) & &
Math : : is_equal_approx ( elements [ 2 ] [ 0 ] , 0 ) & & Math : : is_equal_approx ( elements [ 2 ] [ 1 ] , 0 ) ) ;
}
2017-01-11 04:52:51 +01:00
bool Basis : : is_rotation ( ) const {
2017-04-06 00:47:13 +02:00
return Math : : is_equal_approx ( determinant ( ) , 1 ) & & is_orthogonal ( ) ;
2016-10-18 22:50:21 +02:00
}
2017-01-11 04:52:51 +01:00
bool Basis : : is_symmetric ( ) const {
2016-12-31 15:39:25 +01:00
2017-04-06 00:47:13 +02:00
if ( ! Math : : is_equal_approx ( elements [ 0 ] [ 1 ] , elements [ 1 ] [ 0 ] ) )
2016-12-31 15:39:25 +01:00
return false ;
2017-04-06 00:47:13 +02:00
if ( ! Math : : is_equal_approx ( elements [ 0 ] [ 2 ] , elements [ 2 ] [ 0 ] ) )
2016-12-31 15:39:25 +01:00
return false ;
2017-04-06 00:47:13 +02:00
if ( ! Math : : is_equal_approx ( elements [ 1 ] [ 2 ] , elements [ 2 ] [ 1 ] ) )
2016-12-31 15:39:25 +01:00
return false ;
return true ;
}
2017-01-11 04:52:51 +01:00
Basis Basis : : diagonalize ( ) {
2016-12-31 15:39:25 +01:00
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 {
2014-02-10 02:10:30 +01:00
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 ( ) {
2014-02-10 02:10:30 +01:00
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 {
2014-02-10 02:10:30 +01:00
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 ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
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 {
2014-02-10 02:10:30 +01:00
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 ;
}
2017-01-11 04:52:51 +01:00
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.
2017-01-05 18:31:39 +01:00
real_t det_sign = determinant ( ) > 0 ? 1 : - 1 ;
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.
// This (internal) function is too specı fı c and named too ugly to expose to users, and probably there's no need to do so.
Vector3 Basis : : rotref_posscale_decomposition ( Basis & rotref ) const {
# ifdef MATH_CHECKS
ERR_FAIL_COND_V ( determinant ( ) = = 0 , Vector3 ( ) ) ;
Basis m = transposed ( ) * ( * this ) ;
ERR_FAIL_COND_V ( m . is_diagonal ( ) = = false , Vector3 ( ) ) ;
# 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
ERR_FAIL_COND_V ( rotref . is_orthogonal ( ) = = false , Vector3 ( ) ) ;
# 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-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 ) ;
}
2017-04-06 00:47:13 +02:00
// TODO: rename this to get_rotation_euler
2017-01-11 04:52:51 +01:00
Vector3 Basis : : get_rotation ( ) 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
}
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 ) ;
}
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 {
2014-02-10 02:10:30 +01:00
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-04-06 00:47:13 +02:00
# ifdef MATH_CHECKS
2016-10-18 22:50:21 +02:00
ERR_FAIL_COND_V ( is_rotation ( ) = = false , euler ) ;
2017-04-06 00:47:13 +02:00
# endif
2016-10-18 22:50:21 +02:00
euler . y = Math : : asin ( elements [ 0 ] [ 2 ] ) ;
2017-03-05 16:44:50 +01:00
if ( euler . y < Math_PI * 0.5 ) {
if ( euler . y > - Math_PI * 0.5 ) {
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-08-25 21:45:21 +02:00
// return the simplest form
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 ] ) ;
euler . z = Math : : atan2 ( - elements [ 0 ] [ 1 ] , elements [ 0 ] [ 0 ] ) ;
}
2014-02-10 02:10:30 +01:00
} else {
2017-03-05 16:44:50 +01:00
real_t r = Math : : atan2 ( elements [ 1 ] [ 0 ] , elements [ 1 ] [ 1 ] ) ;
2014-02-10 02:10:30 +01:00
euler . z = 0.0 ;
euler . x = euler . z - r ;
}
} else {
2017-03-05 16:44:50 +01:00
real_t r = Math : : atan2 ( elements [ 0 ] [ 1 ] , elements [ 1 ] [ 1 ] ) ;
2014-02-10 02:10:30 +01:00
euler . z = 0 ;
euler . x = r - euler . z ;
}
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
}
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 ;
# ifdef MATH_CHECKS
ERR_FAIL_COND_V ( is_rotation ( ) = = false , euler ) ;
# endif
real_t m12 = elements [ 1 ] [ 2 ] ;
if ( m12 < 1 ) {
if ( m12 > - 1 ) {
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 ) {
// return the simplest form
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 ;
euler . y = - atan2 ( - elements [ 0 ] [ 1 ] , elements [ 0 ] [ 0 ] ) ;
euler . z = 0 ;
}
} else { // m12 == 1
euler . x = - Math_PI * 0.5 ;
euler . y = - atan2 ( - elements [ 0 ] [ 1 ] , elements [ 0 ] [ 0 ] ) ;
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 ;
}
2017-04-06 00:47:13 +02:00
bool Basis : : is_equal_approx ( const Basis & a , const Basis & b ) const {
2016-10-18 22:50:21 +02:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int j = 0 ; j < 3 ; j + + ) {
2017-04-06 00:47:13 +02:00
if ( Math : : is_equal_approx ( a . elements [ i ] [ j ] , b . elements [ i ] [ j ] ) = = false )
2017-03-05 16:44:50 +01:00
return false ;
}
}
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 {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int j = 0 ; j < 3 ; j + + ) {
2016-10-18 22:50:21 +02:00
if ( elements [ i ] [ j ] ! = p_matrix . elements [ i ] [ j ] )
2014-02-10 02:10:30 +01:00
return false ;
}
}
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 {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
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 + + ) {
2016-03-09 00:00:52 +01:00
2017-03-05 16:44:50 +01:00
for ( int j = 0 ; j < 3 ; j + + ) {
2016-03-09 00:00:52 +01:00
2017-03-05 16:44:50 +01:00
if ( i ! = 0 | | j ! = 0 )
mtx + = " , " ;
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-01-11 04:52:51 +01:00
Basis : : operator Quat ( ) const {
2017-07-05 04:52:23 +02:00
//commenting this check because precision issues cause it to fail when it shouldn't
//#ifdef MATH_CHECKS
//ERR_FAIL_COND_V(is_rotation() == false, Quat());
//#endif
2016-10-18 22:50:21 +02:00
real_t trace = elements [ 0 ] [ 0 ] + elements [ 1 ] [ 1 ] + 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
2017-03-05 16:44:50 +01:00
temp [ 0 ] = ( ( elements [ 2 ] [ 1 ] - elements [ 1 ] [ 2 ] ) * s ) ;
temp [ 1 ] = ( ( elements [ 0 ] [ 2 ] - elements [ 2 ] [ 0 ] ) * s ) ;
temp [ 2 ] = ( ( elements [ 1 ] [ 0 ] - elements [ 0 ] [ 1 ] ) * s ) ;
} else {
2016-10-18 22:50:21 +02:00
int i = elements [ 0 ] [ 0 ] < elements [ 1 ] [ 1 ] ?
2017-03-05 16:44:50 +01:00
( elements [ 1 ] [ 1 ] < elements [ 2 ] [ 2 ] ? 2 : 1 ) :
( elements [ 0 ] [ 0 ] < 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
2016-10-18 22:50:21 +02:00
real_t s = Math : : sqrt ( elements [ i ] [ i ] - elements [ j ] [ j ] - 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
2016-10-18 22:50:21 +02:00
temp [ 3 ] = ( elements [ k ] [ j ] - elements [ j ] [ k ] ) * s ;
temp [ j ] = ( elements [ j ] [ i ] + elements [ i ] [ j ] ) * s ;
temp [ k ] = ( elements [ k ] [ i ] + 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 + + ) {
2014-02-10 02:10:30 +01:00
2017-01-05 18:31:39 +01:00
real_t v = orth [ i ] [ j ] ;
2017-03-05 16:44:50 +01:00
if ( v > 0.5 )
v = 1.0 ;
else if ( v < - 0.5 )
v = - 1.0 ;
2014-02-10 02:10:30 +01:00
else
2017-03-05 16:44:50 +01:00
v = 0 ;
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 + + ) {
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
if ( _ortho_bases [ i ] = = orth )
2014-02-10 02:10:30 +01:00
return i ;
}
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 {
# ifdef MATH_CHECKS
2016-10-18 22:50:21 +02:00
ERR_FAIL_COND ( is_rotation ( ) = = false ) ;
2017-04-06 00:47:13 +02:00
# endif
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 ;
y = 0.7071 ;
z = 0.7071 ;
} 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 ) {
2014-02-10 02:10:30 +01:00
x = 0.7071 ;
y = 0 ;
z = 0.7071 ;
} 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 ) {
2014-02-10 02:10:30 +01:00
x = 0.7071 ;
y = 0.7071 ;
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 ) ;
2016-10-18 22:50:21 +02:00
if ( angle < 0 ) s = - s ;
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-03-05 16:44:50 +01:00
Basis : : Basis ( const Vector3 & p_euler ) {
2016-03-09 00:00:52 +01:00
2017-03-05 16:44:50 +01:00
set_euler ( p_euler ) ;
2014-02-10 02:10:30 +01:00
}
2017-03-05 16:44:50 +01:00
Basis : : Basis ( 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
2017-03-23 18:27:00 +01:00
ERR_FAIL_COND ( p_axis . is_normalized ( ) = = false ) ;
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 ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
real_t cosine = Math : : cos ( p_phi ) ;
real_t sine = Math : : sin ( p_phi ) ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
elements [ 0 ] [ 0 ] = axis_sq . x + cosine * ( 1.0 - axis_sq . x ) ;
elements [ 0 ] [ 1 ] = p_axis . x * p_axis . y * ( 1.0 - cosine ) - p_axis . z * sine ;
elements [ 0 ] [ 2 ] = p_axis . z * p_axis . x * ( 1.0 - cosine ) + p_axis . y * sine ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
elements [ 1 ] [ 0 ] = p_axis . x * p_axis . y * ( 1.0 - cosine ) + p_axis . z * sine ;
elements [ 1 ] [ 1 ] = axis_sq . y + cosine * ( 1.0 - axis_sq . y ) ;
elements [ 1 ] [ 2 ] = p_axis . y * p_axis . z * ( 1.0 - cosine ) - p_axis . x * sine ;
2014-02-10 02:10:30 +01:00
2017-03-05 16:44:50 +01:00
elements [ 2 ] [ 0 ] = p_axis . z * p_axis . x * ( 1.0 - cosine ) - p_axis . y * sine ;
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 ) ;
2014-02-10 02:10:30 +01:00
}
2017-04-06 00:47:13 +02:00
Basis : : Basis ( const Vector3 & p_axis , real_t p_phi ) {
set_axis_angle ( p_axis , p_phi ) ;
}