Merge pull request #6865 from tagcup/godot_issue_6816
Use right handed coordinate system for rotation matrices and quaternions. Also fixed Euler angles (XYZ convention).
This commit is contained in:
Juan Linietsky
GitHub
commit
3a0c19d3f6
16 changed files with 211 additions and 135 deletions
|
|
@ -96,6 +96,15 @@ public:
|
|||
|
||||
static double random(double from, double to);
|
||||
|
||||
static _FORCE_INLINE_ bool isequal_approx(real_t a, real_t b) {
|
||||
// TODO: Comparing floats for approximate-equality is non-trivial.
|
||||
// Using epsilon should cover the typical cases in Godot (where a == b is used to compare two reals), such as matrix and vector comparison operators.
|
||||
// A proper implementation in terms of ULPs should eventually replace the contents of this function.
|
||||
// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ for details.
|
||||
|
||||
return abs(a-b) < CMP_EPSILON;
|
||||
}
|
||||
|
||||
|
||||
static _FORCE_INLINE_ real_t abs(real_t g) {
|
||||
|
||||
|
|
|
|||
|
|
@ -73,6 +73,7 @@ void Matrix3::invert() {
|
|||
}
|
||||
|
||||
void Matrix3::orthonormalize() {
|
||||
ERR_FAIL_COND(determinant() == 0);
|
||||
|
||||
// Gram-Schmidt Process
|
||||
|
||||
|
|
@ -99,6 +100,17 @@ Matrix3 Matrix3::orthonormalized() const {
|
|||
return c;
|
||||
}
|
||||
|
||||
bool Matrix3::is_orthogonal() const {
|
||||
Matrix3 id;
|
||||
Matrix3 m = (*this)*transposed();
|
||||
|
||||
return isequal_approx(id,m);
|
||||
}
|
||||
|
||||
bool Matrix3::is_rotation() const {
|
||||
return Math::isequal_approx(determinant(), 1) && is_orthogonal();
|
||||
}
|
||||
|
||||
|
||||
Matrix3 Matrix3::inverse() const {
|
||||
|
||||
|
|
@ -150,42 +162,58 @@ Vector3 Matrix3::get_scale() const {
|
|||
);
|
||||
|
||||
}
|
||||
void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) {
|
||||
|
||||
// Matrix3::rotate and Matrix3::rotated return M * R(axis,phi), and is a convenience function. They do *not* perform proper matrix rotation.
|
||||
void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) {
|
||||
// TODO: This function should also be renamed as the current name is misleading: rotate does *not* perform matrix rotation.
|
||||
// Same problem affects Matrix3::rotated.
|
||||
// A similar problem exists in 2D math, which will be handled separately.
|
||||
// After Matrix3 is renamed to Basis, this comments needs to be revised.
|
||||
*this = *this * Matrix3(p_axis, p_phi);
|
||||
}
|
||||
|
||||
Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const {
|
||||
|
||||
return *this * Matrix3(p_axis, p_phi);
|
||||
|
||||
}
|
||||
|
||||
// get_euler 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).
|
||||
//
|
||||
// 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.
|
||||
Vector3 Matrix3::get_euler() const {
|
||||
|
||||
// Euler angles in XYZ convention.
|
||||
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
|
||||
//
|
||||
// rot = cy*cz -cy*sz sy
|
||||
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
||||
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
||||
|
||||
Matrix3 m = *this;
|
||||
m.orthonormalize();
|
||||
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
||||
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
||||
|
||||
Vector3 euler;
|
||||
|
||||
euler.y = Math::asin(m[0][2]);
|
||||
ERR_FAIL_COND_V(is_rotation() == false, euler);
|
||||
|
||||
euler.y = Math::asin(elements[0][2]);
|
||||
if ( euler.y < Math_PI*0.5) {
|
||||
if ( euler.y > -Math_PI*0.5) {
|
||||
euler.x = Math::atan2(-m[1][2],m[2][2]);
|
||||
euler.z = Math::atan2(-m[0][1],m[0][0]);
|
||||
euler.x = Math::atan2(-elements[1][2],elements[2][2]);
|
||||
euler.z = Math::atan2(-elements[0][1],elements[0][0]);
|
||||
|
||||
} else {
|
||||
real_t r = Math::atan2(m[1][0],m[1][1]);
|
||||
real_t r = Math::atan2(elements[1][0],elements[1][1]);
|
||||
euler.z = 0.0;
|
||||
euler.x = euler.z - r;
|
||||
|
||||
}
|
||||
} else {
|
||||
real_t r = Math::atan2(m[0][1],m[1][1]);
|
||||
real_t r = Math::atan2(elements[0][1],elements[1][1]);
|
||||
euler.z = 0;
|
||||
euler.x = r - euler.z;
|
||||
}
|
||||
|
|
@ -195,6 +223,9 @@ Vector3 Matrix3::get_euler() const {
|
|||
|
||||
}
|
||||
|
||||
// set_euler expects a vector containing the Euler angles in the format
|
||||
// (c,b,a), where a is the angle of the first rotation, and c is the last.
|
||||
// The current implementation uses XYZ convention (Z is the first rotation).
|
||||
void Matrix3::set_euler(const Vector3& p_euler) {
|
||||
|
||||
real_t c, s;
|
||||
|
|
@ -215,17 +246,30 @@ void Matrix3::set_euler(const Vector3& p_euler) {
|
|||
*this = xmat*(ymat*zmat);
|
||||
}
|
||||
|
||||
bool Matrix3::isequal_approx(const Matrix3& a, const Matrix3& b) const {
|
||||
|
||||
for (int i=0;i<3;i++) {
|
||||
for (int j=0;j<3;j++) {
|
||||
if (Math::isequal_approx(a.elements[i][j],b.elements[i][j]) == false)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Matrix3::operator==(const Matrix3& p_matrix) const {
|
||||
|
||||
for (int i=0;i<3;i++) {
|
||||
for (int j=0;j<3;j++) {
|
||||
if (elements[i][j]!=p_matrix.elements[i][j])
|
||||
if (elements[i][j] != p_matrix.elements[i][j])
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Matrix3::operator!=(const Matrix3& p_matrix) const {
|
||||
|
||||
return (!(*this==p_matrix));
|
||||
|
|
@ -249,11 +293,9 @@ Matrix3::operator String() const {
|
|||
}
|
||||
|
||||
Matrix3::operator Quat() const {
|
||||
ERR_FAIL_COND_V(is_rotation() == false, Quat());
|
||||
|
||||
Matrix3 m=*this;
|
||||
m.orthonormalize();
|
||||
|
||||
real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2];
|
||||
real_t trace = elements[0][0] + elements[1][1] + elements[2][2];
|
||||
real_t temp[4];
|
||||
|
||||
if (trace > 0.0)
|
||||
|
|
@ -262,25 +304,25 @@ Matrix3::operator Quat() const {
|
|||
temp[3]=(s * 0.5);
|
||||
s = 0.5 / s;
|
||||
|
||||
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);
|
||||
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
|
||||
{
|
||||
int i = m.elements[0][0] < m.elements[1][1] ?
|
||||
(m.elements[1][1] < m.elements[2][2] ? 2 : 1) :
|
||||
(m.elements[0][0] < m.elements[2][2] ? 2 : 0);
|
||||
int i = elements[0][0] < elements[1][1] ?
|
||||
(elements[1][1] < elements[2][2] ? 2 : 1) :
|
||||
(elements[0][0] < elements[2][2] ? 2 : 0);
|
||||
int j = (i + 1) % 3;
|
||||
int k = (i + 2) % 3;
|
||||
|
||||
real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0);
|
||||
real_t s = Math::sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0);
|
||||
temp[i] = s * 0.5;
|
||||
s = 0.5 / s;
|
||||
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
return Quat(temp[0],temp[1],temp[2],temp[3]);
|
||||
|
|
@ -356,6 +398,10 @@ void Matrix3::set_orthogonal_index(int p_index){
|
|||
|
||||
|
||||
void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const {
|
||||
// TODO: We can handle improper matrices here too, in which case axis will also correspond to the axis of reflection.
|
||||
// See Eq. (52) in http://scipp.ucsc.edu/~haber/ph251/rotreflect_13.pdf for example
|
||||
// After that change, we should fail on is_orthogonal() == false.
|
||||
ERR_FAIL_COND(is_rotation() == false);
|
||||
|
||||
|
||||
double angle,x,y,z; // variables for result
|
||||
|
|
@ -423,14 +469,13 @@ void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const {
|
|||
// as we have reached here there are no singularities so we can handle normally
|
||||
double 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])); // used to normalise
|
||||
if (Math::abs(s) < 0.001) s=1;
|
||||
// prevent divide by zero, should not happen if matrix is orthogonal and should be
|
||||
// caught by singularity test above, but I've left it in just in case
|
||||
+(elements[0][1] - elements[1][0])*(elements[0][1] - elements[1][0])); // s=|axis||sin(angle)|, used to normalise
|
||||
|
||||
angle = Math::acos(( elements[0][0] + elements[1][1] + elements[2][2] - 1)/2);
|
||||
x = (elements[1][2] - elements[2][1])/s;
|
||||
y = (elements[2][0] - elements[0][2])/s;
|
||||
z = (elements[0][1] - elements[1][0])/s;
|
||||
if (angle < 0) s = -s;
|
||||
x = (elements[2][1] - elements[1][2])/s;
|
||||
y = (elements[0][2] - elements[2][0])/s;
|
||||
z = (elements[1][0] - elements[0][1])/s;
|
||||
|
||||
r_axis=Vector3(x,y,z);
|
||||
r_angle=angle;
|
||||
|
|
@ -457,6 +502,7 @@ Matrix3::Matrix3(const Quat& p_quat) {
|
|||
}
|
||||
|
||||
Matrix3::Matrix3(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_and_angle
|
||||
|
||||
Vector3 axis_sq(p_axis.x*p_axis.x,p_axis.y*p_axis.y,p_axis.z*p_axis.z);
|
||||
|
||||
|
|
@ -464,15 +510,15 @@ Matrix3::Matrix3(const Vector3& p_axis, real_t p_phi) {
|
|||
real_t sine= Math::sin(p_phi);
|
||||
|
||||
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;
|
||||
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;
|
||||
|
||||
elements[1][0] = p_axis.x * p_axis.y * ( 1.0 - cosine ) - p_axis.z * sine;
|
||||
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;
|
||||
elements[1][2] = p_axis.y * p_axis.z * ( 1.0 - cosine ) - p_axis.x * sine;
|
||||
|
||||
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][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 );
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -91,6 +91,8 @@ public:
|
|||
return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
|
||||
}
|
||||
|
||||
bool isequal_approx(const Matrix3& a, const Matrix3& b) const;
|
||||
|
||||
bool operator==(const Matrix3& p_matrix) const;
|
||||
bool operator!=(const Matrix3& p_matrix) const;
|
||||
|
||||
|
|
@ -102,6 +104,9 @@ public:
|
|||
int get_orthogonal_index() const;
|
||||
void set_orthogonal_index(int p_index);
|
||||
|
||||
bool is_orthogonal() const;
|
||||
bool is_rotation() const;
|
||||
|
||||
operator String() const;
|
||||
|
||||
void get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const;
|
||||
|
|
|
|||
|
|
@ -27,22 +27,40 @@
|
|||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
#include "quat.h"
|
||||
#include "matrix3.h"
|
||||
#include "print_string.h"
|
||||
|
||||
// set_euler expects a vector containing the Euler angles in the format
|
||||
// (c,b,a), where a is the angle of the first rotation, and c is the last.
|
||||
// The current implementation uses XYZ convention (Z is the first rotation).
|
||||
void Quat::set_euler(const Vector3& p_euler) {
|
||||
real_t half_yaw = p_euler.x * 0.5;
|
||||
real_t half_pitch = p_euler.y * 0.5;
|
||||
real_t half_roll = p_euler.z * 0.5;
|
||||
real_t cos_yaw = Math::cos(half_yaw);
|
||||
real_t sin_yaw = Math::sin(half_yaw);
|
||||
real_t cos_pitch = Math::cos(half_pitch);
|
||||
real_t sin_pitch = Math::sin(half_pitch);
|
||||
real_t cos_roll = Math::cos(half_roll);
|
||||
real_t sin_roll = Math::sin(half_roll);
|
||||
set(cos_roll * sin_pitch * cos_yaw+sin_roll * cos_pitch * sin_yaw,
|
||||
cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw,
|
||||
sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw,
|
||||
cos_roll * cos_pitch * cos_yaw+sin_roll * sin_pitch * sin_yaw);
|
||||
real_t half_a1 = p_euler.x * 0.5;
|
||||
real_t half_a2 = p_euler.y * 0.5;
|
||||
real_t half_a3 = p_euler.z * 0.5;
|
||||
|
||||
// R = X(a1).Y(a2).Z(a3) convention for Euler angles.
|
||||
// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-2)
|
||||
// a3 is the angle of the first rotation, following the notation in this reference.
|
||||
|
||||
real_t cos_a1 = Math::cos(half_a1);
|
||||
real_t sin_a1 = Math::sin(half_a1);
|
||||
real_t cos_a2 = Math::cos(half_a2);
|
||||
real_t sin_a2 = Math::sin(half_a2);
|
||||
real_t cos_a3 = Math::cos(half_a3);
|
||||
real_t sin_a3 = Math::sin(half_a3);
|
||||
|
||||
set(sin_a1*cos_a2*cos_a3 + sin_a2*sin_a3*cos_a1,
|
||||
-sin_a1*sin_a3*cos_a2 + sin_a2*cos_a1*cos_a3,
|
||||
sin_a1*sin_a2*cos_a3 + sin_a3*cos_a1*cos_a2,
|
||||
-sin_a1*sin_a2*sin_a3 + cos_a1*cos_a2*cos_a3);
|
||||
}
|
||||
|
||||
// get_euler 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.
|
||||
// The current implementation uses XYZ convention (Z is the first rotation).
|
||||
Vector3 Quat::get_euler() const {
|
||||
Matrix3 m(*this);
|
||||
return m.get_euler();
|
||||
}
|
||||
|
||||
void Quat::operator*=(const Quat& q) {
|
||||
|
|
@ -126,26 +144,25 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const {
|
|||
}
|
||||
#else
|
||||
|
||||
real_t to1[4];
|
||||
Quat to1;
|
||||
real_t omega, cosom, sinom, scale0, scale1;
|
||||
|
||||
|
||||
// calc cosine
|
||||
cosom = x * q.x + y * q.y + z * q.z
|
||||
+ w * q.w;
|
||||
|
||||
cosom = dot(q);
|
||||
|
||||
// adjust signs (if necessary)
|
||||
if ( cosom <0.0 ) {
|
||||
cosom = -cosom; to1[0] = - q.x;
|
||||
to1[1] = - q.y;
|
||||
to1[2] = - q.z;
|
||||
to1[3] = - q.w;
|
||||
cosom = -cosom;
|
||||
to1.x = - q.x;
|
||||
to1.y = - q.y;
|
||||
to1.z = - q.z;
|
||||
to1.w = - q.w;
|
||||
} else {
|
||||
to1[0] = q.x;
|
||||
to1[1] = q.y;
|
||||
to1[2] = q.z;
|
||||
to1[3] = q.w;
|
||||
to1.x = q.x;
|
||||
to1.y = q.y;
|
||||
to1.z = q.z;
|
||||
to1.w = q.w;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -165,10 +182,10 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const {
|
|||
}
|
||||
// calculate final values
|
||||
return Quat(
|
||||
scale0 * x + scale1 * to1[0],
|
||||
scale0 * y + scale1 * to1[1],
|
||||
scale0 * z + scale1 * to1[2],
|
||||
scale0 * w + scale1 * to1[3]
|
||||
scale0 * x + scale1 * to1.x,
|
||||
scale0 * y + scale1 * to1.y,
|
||||
scale0 * z + scale1 * to1.z,
|
||||
scale0 * w + scale1 * to1.w
|
||||
);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -186,10 +203,10 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const {
|
|||
newFactor = Math::sin(t * theta) * sinT,
|
||||
invFactor = Math::sin((1.0f - t) * theta) * sinT;
|
||||
|
||||
return Quat( invFactor * from.x + newFactor * q.x,
|
||||
invFactor * from.y + newFactor * q.y,
|
||||
invFactor * from.z + newFactor * q.z,
|
||||
invFactor * from.w + newFactor * q.w );
|
||||
return Quat(invFactor * from.x + newFactor * q.x,
|
||||
invFactor * from.y + newFactor * q.y,
|
||||
invFactor * from.z + newFactor * q.z,
|
||||
invFactor * from.w + newFactor * q.w);
|
||||
|
||||
#if 0
|
||||
real_t to1[4];
|
||||
|
|
@ -203,7 +220,7 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const {
|
|||
|
||||
// adjust signs (if necessary)
|
||||
if ( cosom <0.0 && false) {
|
||||
cosom = -cosom; to1[0] = - q.x;
|
||||
cosom = -cosom;to1[0] = - q.x;
|
||||
to1[1] = - q.y;
|
||||
to1[2] = - q.z;
|
||||
to1[3] = - q.w;
|
||||
|
|
@ -260,8 +277,10 @@ Quat::Quat(const Vector3& axis, const real_t& angle) {
|
|||
if (d==0)
|
||||
set(0,0,0,0);
|
||||
else {
|
||||
real_t s = Math::sin(-angle * 0.5) / d;
|
||||
real_t sin_angle = Math::sin(angle * 0.5);
|
||||
real_t cos_angle = Math::cos(angle * 0.5);
|
||||
real_t s = sin_angle / d;
|
||||
set(axis.x * s, axis.y * s, axis.z * s,
|
||||
Math::cos(-angle * 0.5));
|
||||
cos_angle);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,15 +49,16 @@ public:
|
|||
Quat inverse() const;
|
||||
_FORCE_INLINE_ real_t dot(const Quat& q) const;
|
||||
void set_euler(const Vector3& p_euler);
|
||||
Vector3 get_euler() const;
|
||||
Quat slerp(const Quat& q, const real_t& t) const;
|
||||
Quat slerpni(const Quat& q, const real_t& t) const;
|
||||
Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const;
|
||||
|
||||
_FORCE_INLINE_ void get_axis_and_angle(Vector3& r_axis, real_t &r_angle) const {
|
||||
r_angle = 2 * Math::acos(w);
|
||||
r_axis.x = -x / Math::sqrt(1-w*w);
|
||||
r_axis.y = -y / Math::sqrt(1-w*w);
|
||||
r_axis.z = -z / Math::sqrt(1-w*w);
|
||||
r_axis.x = x / Math::sqrt(1-w*w);
|
||||
r_axis.y = y / Math::sqrt(1-w*w);
|
||||
r_axis.z = z / Math::sqrt(1-w*w);
|
||||
}
|
||||
|
||||
void operator*=(const Quat& q);
|
||||
|
|
@ -183,12 +184,10 @@ Quat Quat::operator/(const real_t& s) const {
|
|||
|
||||
|
||||
bool Quat::operator==(const Quat& p_quat) const {
|
||||
|
||||
return x==p_quat.x && y==p_quat.y && z==p_quat.z && w==p_quat.w;
|
||||
}
|
||||
|
||||
bool Quat::operator!=(const Quat& p_quat) const {
|
||||
|
||||
return x!=p_quat.x || y!=p_quat.y || z!=p_quat.z || w!=p_quat.w;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -293,7 +293,6 @@ bool Vector3::operator==(const Vector3& p_v) const {
|
|||
}
|
||||
|
||||
bool Vector3::operator!=(const Vector3& p_v) const {
|
||||
|
||||
return (x!=p_v.x || y!=p_v.y || z!=p_v.z);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20714,7 +20714,7 @@
|
|||
<argument index="1" name="phi" type="float">
|
||||
</argument>
|
||||
<description>
|
||||
Create a matrix from an axis vector and an angle.
|
||||
Create a matrix which rotates around the given axis by the specified angle.
|
||||
</description>
|
||||
</method>
|
||||
<method name="Matrix3">
|
||||
|
|
@ -20741,7 +20741,7 @@
|
|||
<return type="Vector3">
|
||||
</return>
|
||||
<description>
|
||||
Return euler angles from the matrix.
|
||||
Return euler angles (in the XYZ convention: first Z, then Y, and X last) from the matrix. Returned vector contains the rotation angles in the format (third,second,first).
|
||||
</description>
|
||||
</method>
|
||||
<method name="get_orthogonal_index">
|
||||
|
|
@ -20767,7 +20767,7 @@
|
|||
<return type="Matrix3">
|
||||
</return>
|
||||
<description>
|
||||
Return the orthonormalized version of the matrix (useful to call from time to time to avoid rounding error).
|
||||
Return the orthonormalized version of the matrix (useful to call from time to time to avoid rounding error for orthogonal matrices). This performs a Gram-Schmidt orthonormalization on the basis of the matrix.
|
||||
</description>
|
||||
</method>
|
||||
<method name="rotated">
|
||||
|
|
@ -20777,10 +20777,7 @@
|
|||
</argument>
|
||||
<argument index="1" name="phi" type="float">
|
||||
</argument>
|
||||
<description>
|
||||
Return the rotated version of the matrix, by a given axis and angle.
|
||||
</description>
|
||||
</method>
|
||||
</method>
|
||||
<method name="scaled">
|
||||
<return type="Matrix3">
|
||||
</return>
|
||||
|
|
@ -31485,7 +31482,7 @@
|
|||
Quaternion.
|
||||
</brief_description>
|
||||
<description>
|
||||
Quaternion is a 4 dimensional vector that is used to represent a rotation. It mainly exists to perform SLERP (spherical-linear interpolation) between to rotations obtained by a Matrix3 cheaply. Adding quaternions also cheaply adds the rotations, however quaternions need to be often normalized, or else they suffer from precision issues.
|
||||
Quaternion is a 4 dimensional vector that is used to represent a rotation. It mainly exists to perform SLERP (spherical-linear interpolation) between to rotations obtained by a Matrix3 cheaply. Multiplying quaternions also cheaply reproduces rotation sequences, however quaternions need to be often normalized, or else they suffer from precision issues.
|
||||
</description>
|
||||
<methods>
|
||||
<method name="Quat">
|
||||
|
|
@ -31510,6 +31507,7 @@
|
|||
<argument index="1" name="angle" type="float">
|
||||
</argument>
|
||||
<description>
|
||||
Returns a quaternion that will rotate around the given axis by the specified angle.
|
||||
</description>
|
||||
</method>
|
||||
<method name="Quat">
|
||||
|
|
@ -31518,6 +31516,7 @@
|
|||
<argument index="0" name="from" type="Matrix3">
|
||||
</argument>
|
||||
<description>
|
||||
Returns the rotation matrix corresponding to the given quaternion.
|
||||
</description>
|
||||
</method>
|
||||
<method name="cubic_slerp">
|
||||
|
|
@ -31540,14 +31539,14 @@
|
|||
<argument index="0" name="b" type="Quat">
|
||||
</argument>
|
||||
<description>
|
||||
Returns the dot product between two quaternions.
|
||||
Returns the dot product of two quaternions.
|
||||
</description>
|
||||
</method>
|
||||
<method name="inverse">
|
||||
<return type="Quat">
|
||||
</return>
|
||||
<description>
|
||||
Returns the inverse of the quaternion (applies to the inverse rotation too).
|
||||
Returns the inverse of the quaternion.
|
||||
</description>
|
||||
</method>
|
||||
<method name="length">
|
||||
|
|
@ -43135,7 +43134,7 @@
|
|||
<argument index="1" name="phi" type="float">
|
||||
</argument>
|
||||
<description>
|
||||
Rotate the transform locally.
|
||||
Rotate the transform locally. This introduces an additional pre-rotation to the transform, changing the basis to basis * Matrix3(axis, phi).
|
||||
</description>
|
||||
</method>
|
||||
<method name="scaled">
|
||||
|
|
|
|||
|
|
@ -103,13 +103,13 @@ void GridMapEditor::_menu_option(int p_option) {
|
|||
if (input_action==INPUT_DUPLICATE) {
|
||||
|
||||
r.set_orthogonal_index(selection.duplicate_rot);
|
||||
r.rotate(Vector3(0,1,0),Math_PI/2.0);
|
||||
r.rotate(Vector3(0,1,0),-Math_PI/2.0);
|
||||
selection.duplicate_rot=r.get_orthogonal_index();
|
||||
_update_duplicate_indicator();
|
||||
break;
|
||||
}
|
||||
r.set_orthogonal_index(cursor_rot);
|
||||
r.rotate(Vector3(0,1,0),Math_PI/2.0);
|
||||
r.rotate(Vector3(0,1,0),-Math_PI/2.0);
|
||||
cursor_rot=r.get_orthogonal_index();
|
||||
_update_cursor_transform();
|
||||
} break;
|
||||
|
|
@ -118,14 +118,14 @@ void GridMapEditor::_menu_option(int p_option) {
|
|||
if (input_action==INPUT_DUPLICATE) {
|
||||
|
||||
r.set_orthogonal_index(selection.duplicate_rot);
|
||||
r.rotate(Vector3(1,0,0),Math_PI/2.0);
|
||||
r.rotate(Vector3(1,0,0),-Math_PI/2.0);
|
||||
selection.duplicate_rot=r.get_orthogonal_index();
|
||||
_update_duplicate_indicator();
|
||||
break;
|
||||
}
|
||||
|
||||
r.set_orthogonal_index(cursor_rot);
|
||||
r.rotate(Vector3(1,0,0),Math_PI/2.0);
|
||||
r.rotate(Vector3(1,0,0),-Math_PI/2.0);
|
||||
cursor_rot=r.get_orthogonal_index();
|
||||
_update_cursor_transform();
|
||||
} break;
|
||||
|
|
@ -134,35 +134,35 @@ void GridMapEditor::_menu_option(int p_option) {
|
|||
if (input_action==INPUT_DUPLICATE) {
|
||||
|
||||
r.set_orthogonal_index(selection.duplicate_rot);
|
||||
r.rotate(Vector3(0,0,1),Math_PI/2.0);
|
||||
r.rotate(Vector3(0,0,1),-Math_PI/2.0);
|
||||
selection.duplicate_rot=r.get_orthogonal_index();
|
||||
_update_duplicate_indicator();
|
||||
break;
|
||||
}
|
||||
|
||||
r.set_orthogonal_index(cursor_rot);
|
||||
r.rotate(Vector3(0,0,1),Math_PI/2.0);
|
||||
r.rotate(Vector3(0,0,1),-Math_PI/2.0);
|
||||
cursor_rot=r.get_orthogonal_index();
|
||||
_update_cursor_transform();
|
||||
} break;
|
||||
case MENU_OPTION_CURSOR_BACK_ROTATE_Y: {
|
||||
Matrix3 r;
|
||||
r.set_orthogonal_index(cursor_rot);
|
||||
r.rotate(Vector3(0,1,0),-Math_PI/2.0);
|
||||
r.rotate(Vector3(0,1,0),Math_PI/2.0);
|
||||
cursor_rot=r.get_orthogonal_index();
|
||||
_update_cursor_transform();
|
||||
} break;
|
||||
case MENU_OPTION_CURSOR_BACK_ROTATE_X: {
|
||||
Matrix3 r;
|
||||
r.set_orthogonal_index(cursor_rot);
|
||||
r.rotate(Vector3(1,0,0),-Math_PI/2.0);
|
||||
r.rotate(Vector3(1,0,0),Math_PI/2.0);
|
||||
cursor_rot=r.get_orthogonal_index();
|
||||
_update_cursor_transform();
|
||||
} break;
|
||||
case MENU_OPTION_CURSOR_BACK_ROTATE_Z: {
|
||||
Matrix3 r;
|
||||
r.set_orthogonal_index(cursor_rot);
|
||||
r.rotate(Vector3(0,0,1),-Math_PI/2.0);
|
||||
r.rotate(Vector3(0,0,1),Math_PI/2.0);
|
||||
cursor_rot=r.get_orthogonal_index();
|
||||
_update_cursor_transform();
|
||||
} break;
|
||||
|
|
|
|||
|
|
@ -255,8 +255,8 @@ void CharacterCamera::_compute_camera() {
|
|||
orbit.x=max_orbit_x;
|
||||
|
||||
Matrix3 m;
|
||||
m.rotate(Vector3(0,1,0),Math::deg2rad(orbit.y));
|
||||
m.rotate(Vector3(1,0,0),Math::deg2rad(orbit.x));
|
||||
m.rotate(Vector3(0,1,0),-Math::deg2rad(orbit.y));
|
||||
m.rotate(Vector3(1,0,0),-Math::deg2rad(orbit.x));
|
||||
|
||||
new_pos = (m.get_axis(2) * distance) + character_pos;
|
||||
|
||||
|
|
@ -432,8 +432,8 @@ void CharacterCamera::set_orbit(const Vector2& p_orbit) {
|
|||
float d = char_pos.distance_to(follow_pos);
|
||||
|
||||
Matrix3 m;
|
||||
m.rotate(Vector3(0,1,0),orbit.y);
|
||||
m.rotate(Vector3(1,0,0),orbit.x);
|
||||
m.rotate(Vector3(0,1,0),-orbit.y);
|
||||
m.rotate(Vector3(1,0,0),-orbit.x);
|
||||
|
||||
follow_pos=char_pos + m.get_axis(2) * d;
|
||||
|
||||
|
|
@ -475,8 +475,8 @@ void CharacterCamera::rotate_orbit(const Vector2& p_relative) {
|
|||
if (type == CAMERA_FOLLOW && is_inside_scene()) {
|
||||
|
||||
Matrix3 m;
|
||||
m.rotate(Vector3(0,1,0),Math::deg2rad(p_relative.y));
|
||||
m.rotate(Vector3(1,0,0),Math::deg2rad(p_relative.x));
|
||||
m.rotate(Vector3(0,1,0),-Math::deg2rad(p_relative.y));
|
||||
m.rotate(Vector3(1,0,0),-Math::deg2rad(p_relative.x));
|
||||
|
||||
Vector3 char_pos = get_global_transform().origin;
|
||||
char_pos.y+=height;
|
||||
|
|
|
|||
|
|
@ -144,7 +144,7 @@ Transform Collada::Node::compute_transform(Collada &state) const {
|
|||
case XForm::OP_ROTATE: {
|
||||
if (xf.data.size()>=4) {
|
||||
|
||||
xform_step.rotate(Vector3(xf.data[0],xf.data[1],xf.data[2]),-Math::deg2rad(xf.data[3]));
|
||||
xform_step.rotate(Vector3(xf.data[0],xf.data[1],xf.data[2]),Math::deg2rad(xf.data[3]));
|
||||
}
|
||||
} break;
|
||||
case XForm::OP_SCALE: {
|
||||
|
|
@ -1604,7 +1604,7 @@ Collada::Node* Collada::_parse_visual_instance_camera(XMLParser& parser) {
|
|||
cam->camera= _uri_to_id(parser.get_attribute_value_safe("url"));
|
||||
|
||||
if (state.up_axis==Vector3::AXIS_Z) //collada weirdness
|
||||
cam->post_transform.basis.rotate(Vector3(1,0,0),Math_PI*0.5);
|
||||
cam->post_transform.basis.rotate(Vector3(1,0,0),-Math_PI*0.5);
|
||||
|
||||
if (parser.is_empty()) //nothing else to parse...
|
||||
return cam;
|
||||
|
|
@ -1625,7 +1625,7 @@ Collada::Node* Collada::_parse_visual_instance_light(XMLParser& parser) {
|
|||
cam->light= _uri_to_id(parser.get_attribute_value_safe("url"));
|
||||
|
||||
if (state.up_axis==Vector3::AXIS_Z) //collada weirdness
|
||||
cam->post_transform.basis.rotate(Vector3(1,0,0),Math_PI*0.5);
|
||||
cam->post_transform.basis.rotate(Vector3(1,0,0),-Math_PI*0.5);
|
||||
|
||||
if (parser.is_empty()) //nothing else to parse...
|
||||
return cam;
|
||||
|
|
|
|||
|
|
@ -179,8 +179,8 @@ void MeshLibraryEditor::_import_scene(Node *p_scene, Ref<MeshLibrary> p_library,
|
|||
Vector3 ofs = aabb.pos + aabb.size*0.5;
|
||||
aabb.pos-=ofs;
|
||||
Transform xform;
|
||||
xform.basis=Matrix3().rotated(Vector3(0,1,0),Math_PI*0.25);
|
||||
xform.basis = Matrix3().rotated(Vector3(1,0,0),-Math_PI*0.25)*xform.basis;
|
||||
xform.basis=Matrix3().rotated(Vector3(0,1,0),-Math_PI*0.25);
|
||||
xform.basis = Matrix3().rotated(Vector3(1,0,0),Math_PI*0.25)*xform.basis;
|
||||
AABB rot_aabb = xform.xform(aabb);
|
||||
print_line("rot_aabb: "+rot_aabb);
|
||||
float m = MAX(rot_aabb.size.x,rot_aabb.size.y)*0.5;
|
||||
|
|
|
|||
|
|
@ -807,8 +807,8 @@ Ref<Texture> EditorMeshPreviewPlugin::generate(const RES& p_from) {
|
|||
Vector3 ofs = aabb.pos + aabb.size*0.5;
|
||||
aabb.pos-=ofs;
|
||||
Transform xform;
|
||||
xform.basis=Matrix3().rotated(Vector3(0,1,0),Math_PI*0.125);
|
||||
xform.basis = Matrix3().rotated(Vector3(1,0,0),-Math_PI*0.125)*xform.basis;
|
||||
xform.basis=Matrix3().rotated(Vector3(0,1,0),-Math_PI*0.125);
|
||||
xform.basis = Matrix3().rotated(Vector3(1,0,0),Math_PI*0.125)*xform.basis;
|
||||
AABB rot_aabb = xform.xform(aabb);
|
||||
float m = MAX(rot_aabb.size.x,rot_aabb.size.y)*0.5;
|
||||
if (m==0)
|
||||
|
|
|
|||
|
|
@ -129,8 +129,8 @@ MaterialEditor::MaterialEditor() {
|
|||
viewport->add_child(box_instance);
|
||||
|
||||
Transform box_xform;
|
||||
box_xform.basis.rotate(Vector3(1,0,0),Math::deg2rad(-25));
|
||||
box_xform.basis = box_xform.basis * Matrix3().rotated(Vector3(0,1,0),Math::deg2rad(-25));
|
||||
box_xform.basis.rotate(Vector3(1,0,0),Math::deg2rad(25));
|
||||
box_xform.basis = box_xform.basis * Matrix3().rotated(Vector3(0,1,0),Math::deg2rad(25));
|
||||
box_xform.basis.scale(Vector3(0.8,0.8,0.8));
|
||||
box_instance->set_transform(box_xform);
|
||||
|
||||
|
|
|
|||
|
|
@ -82,8 +82,8 @@ void MeshEditor::_notification(int p_what) {
|
|||
void MeshEditor::_update_rotation() {
|
||||
|
||||
Transform t;
|
||||
t.basis.rotate(Vector3(0, 1, 0), rot_y);
|
||||
t.basis.rotate(Vector3(1, 0, 0), rot_x);
|
||||
t.basis.rotate(Vector3(0, 1, 0), -rot_y);
|
||||
t.basis.rotate(Vector3(1, 0, 0), -rot_x);
|
||||
mesh_instance->set_transform(t);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -207,10 +207,10 @@ void MultiMeshEditor::_populate() {
|
|||
|
||||
Transform axis_xform;
|
||||
if (axis==Vector3::AXIS_Z) {
|
||||
axis_xform.rotate(Vector3(1,0,0),Math_PI*0.5);
|
||||
axis_xform.rotate(Vector3(1,0,0),-Math_PI*0.5);
|
||||
}
|
||||
if (axis==Vector3::AXIS_X) {
|
||||
axis_xform.rotate(Vector3(0,0,1),Math_PI*0.5);
|
||||
axis_xform.rotate(Vector3(0,0,1),-Math_PI*0.5);
|
||||
}
|
||||
|
||||
for(int i=0;i<instance_count;i++) {
|
||||
|
|
@ -238,9 +238,9 @@ void MultiMeshEditor::_populate() {
|
|||
|
||||
Matrix3 post_xform;
|
||||
|
||||
post_xform.rotate(xform.basis.get_axis(0),Math::random(-_tilt_random,_tilt_random)*Math_PI);
|
||||
post_xform.rotate(xform.basis.get_axis(2),Math::random(-_tilt_random,_tilt_random)*Math_PI);
|
||||
post_xform.rotate(xform.basis.get_axis(1),Math::random(-_rotate_random,_rotate_random)*Math_PI);
|
||||
post_xform.rotate(xform.basis.get_axis(0),-Math::random(-_tilt_random,_tilt_random)*Math_PI);
|
||||
post_xform.rotate(xform.basis.get_axis(2),-Math::random(-_tilt_random,_tilt_random)*Math_PI);
|
||||
post_xform.rotate(xform.basis.get_axis(1),-Math::random(-_rotate_random,_rotate_random)*Math_PI);
|
||||
xform.basis = post_xform * xform.basis;
|
||||
//xform.basis.orthonormalize();
|
||||
|
||||
|
|
|
|||
|
|
@ -61,8 +61,8 @@ void SpatialEditorViewport::_update_camera() {
|
|||
|
||||
Transform camera_transform;
|
||||
camera_transform.translate(cursor.pos);
|
||||
camera_transform.basis.rotate(Vector3(0, 1, 0), cursor.y_rot);
|
||||
camera_transform.basis.rotate(Vector3(1, 0, 0), cursor.x_rot);
|
||||
camera_transform.basis.rotate(Vector3(0, 1, 0), -cursor.y_rot);
|
||||
camera_transform.basis.rotate(Vector3(1, 0, 0), -cursor.x_rot);
|
||||
|
||||
if (orthogonal)
|
||||
camera_transform.translate(0, 0, 4096);
|
||||
|
|
@ -474,8 +474,8 @@ Vector3 SpatialEditorViewport::_get_screen_to_space(const Vector3& p_pos) {
|
|||
|
||||
Transform camera_transform;
|
||||
camera_transform.translate( cursor.pos );
|
||||
camera_transform.basis.rotate(Vector3(0,1,0),cursor.y_rot);
|
||||
camera_transform.basis.rotate(Vector3(1,0,0),cursor.x_rot);
|
||||
camera_transform.basis.rotate(Vector3(0,1,0),-cursor.y_rot);
|
||||
camera_transform.basis.rotate(Vector3(1,0,0),-cursor.x_rot);
|
||||
camera_transform.translate(0,0,cursor.distance);
|
||||
|
||||
return camera_transform.xform(Vector3( ((p_pos.x/get_size().width)*2.0-1.0)*screen_w, ((1.0-(p_pos.y/get_size().height))*2.0-1.0)*screen_h,-get_znear()));
|
||||
|
|
@ -1484,7 +1484,7 @@ void SpatialEditorViewport::_sinput(const InputEvent &p_event) {
|
|||
|
||||
|
||||
Transform r;
|
||||
r.basis.rotate(plane.normal,-angle);
|
||||
r.basis.rotate(plane.normal,angle);
|
||||
|
||||
List<Node*> &selection = editor_selection->get_selected_node_list();
|
||||
|
||||
|
|
@ -1591,8 +1591,8 @@ void SpatialEditorViewport::_sinput(const InputEvent &p_event) {
|
|||
Transform camera_transform;
|
||||
|
||||
camera_transform.translate(cursor.pos);
|
||||
camera_transform.basis.rotate(Vector3(0,1,0),cursor.y_rot);
|
||||
camera_transform.basis.rotate(Vector3(1,0,0),cursor.x_rot);
|
||||
camera_transform.basis.rotate(Vector3(0,1,0),-cursor.y_rot);
|
||||
camera_transform.basis.rotate(Vector3(1,0,0),-cursor.x_rot);
|
||||
Vector3 translation(-m.relative_x*pan_speed,m.relative_y*pan_speed,0);
|
||||
translation*=cursor.distance/DISTANCE_DEFAULT;
|
||||
camera_transform.translate(translation);
|
||||
|
|
@ -2810,7 +2810,7 @@ void SpatialEditor::_xform_dialog_action() {
|
|||
continue;
|
||||
Vector3 axis;
|
||||
axis[i]=1.0;
|
||||
t.basis.rotate(axis,rotate[i]);
|
||||
t.basis.rotate(axis,rotate[i]); // BUG(?): Angle not flipped; please check during the review of PR #6865.
|
||||
}
|
||||
|
||||
for(int i=0;i<3;i++) {
|
||||
|
|
@ -3160,7 +3160,7 @@ void SpatialEditor::_init_indicators() {
|
|||
|
||||
|
||||
|
||||
light_transform.rotate(Vector3(1,0,0),Math_PI/5.0);
|
||||
light_transform.rotate(Vector3(1,0,0),-Math_PI/5.0);
|
||||
VisualServer::get_singleton()->instance_set_transform(light_instance,light_transform);
|
||||
|
||||
|
||||
|
|
@ -3773,8 +3773,8 @@ void SpatialEditor::_update_ambient_light_color(const Color& p_color) {
|
|||
void SpatialEditor::_update_default_light_angle() {
|
||||
|
||||
Transform t;
|
||||
t.basis.rotate(Vector3(1,0,0),settings_default_light_rot_x);
|
||||
t.basis.rotate(Vector3(0,1,0),settings_default_light_rot_y);
|
||||
t.basis.rotate(Vector3(1,0,0),-settings_default_light_rot_x);
|
||||
t.basis.rotate(Vector3(0,1,0),-settings_default_light_rot_y);
|
||||
settings_dlight->set_transform(t);
|
||||
if (light_instance.is_valid()) {
|
||||
VS::get_singleton()->instance_set_transform(light_instance,t);
|
||||
|
|
|
|||
Loading…
Reference in a new issue