@hughsk/fulltilt
Version:
Standalone device orientation + device motion normalization and conversion library
206 lines (128 loc) • 4.29 kB
JavaScript
////// FULLTILT.Euler //////
FULLTILT.Euler = function ( alpha, beta, gamma ) {
this.set = function ( alpha, beta, gamma ) {
this.alpha = alpha || 0;
this.beta = beta || 0;
this.gamma = gamma || 0;
};
this.copy = function ( inEuler ) {
this.alpha = inEuler.alpha;
this.beta = inEuler.beta;
this.gamma = inEuler.gamma;
};
this.setFromRotationMatrix = (function () {
var R, _alpha, _beta, _gamma;
return function ( matrix ) {
R = matrix.elements;
if (R[8] > 0) { // cos(beta) > 0
_alpha = Math.atan2(-R[1], R[4]);
_beta = Math.asin(R[7]); // beta (-pi/2, pi/2)
_gamma = Math.atan2(-R[6], R[8]); // gamma (-pi/2, pi/2)
} else if (R[8] < 0) { // cos(beta) < 0
_alpha = Math.atan2(R[1], -R[4]);
_beta = -Math.asin(R[7]);
_beta += (_beta >= 0) ? - M_PI : M_PI; // beta [-pi,-pi/2) U (pi/2,pi)
_gamma = Math.atan2(R[6], -R[8]); // gamma (-pi/2, pi/2)
} else { // R[8] == 0
if (R[6] > 0) { // cos(gamma) == 0, cos(beta) > 0
_alpha = Math.atan2(-R[1], R[4]);
_beta = Math.asin(R[7]); // beta [-pi/2, pi/2]
_gamma = - M_PI_2; // gamma = -pi/2
} else if (R[6] < 0) { // cos(gamma) == 0, cos(beta) < 0
_alpha = Math.atan2(R[1], -R[4]);
_beta = -Math.asin(R[7]);
_beta += (_beta >= 0) ? - M_PI : M_PI; // beta [-pi,-pi/2) U (pi/2,pi)
_gamma = - M_PI_2; // gamma = -pi/2
} else { // R[6] == 0, cos(beta) == 0
// gimbal lock discontinuity
_alpha = Math.atan2(R[3], R[0]);
_beta = (R[7] > 0) ? M_PI_2 : - M_PI_2; // beta = +-pi/2
_gamma = 0; // gamma = 0
}
}
// alpha is in [-pi, pi], make sure it is in [0, 2*pi).
if (_alpha < 0) {
_alpha += M_2_PI; // alpha [0, 2*pi)
}
// Convert to degrees
_alpha *= radToDeg;
_beta *= radToDeg;
_gamma *= radToDeg;
// apply derived euler angles to current object
this.set( _alpha, _beta, _gamma );
};
})();
this.setFromQuaternion = (function () {
var _alpha, _beta, _gamma;
return function ( q ) {
var sqw = q.w * q.w;
var sqx = q.x * q.x;
var sqy = q.y * q.y;
var sqz = q.z * q.z;
var unitLength = sqw + sqx + sqy + sqz; // Normalised == 1, otherwise correction divisor.
var wxyz = q.w * q.x + q.y * q.z;
var epsilon = 1e-6; // rounding factor
if (wxyz > (0.5 - epsilon) * unitLength) {
_alpha = 2 * Math.atan2(q.y, q.w);
_beta = M_PI_2;
_gamma = 0;
} else if (wxyz < (-0.5 + epsilon) * unitLength) {
_alpha = -2 * Math.atan2(q.y, q.w);
_beta = -M_PI_2;
_gamma = 0;
} else {
var aX = sqw - sqx + sqy - sqz;
var aY = 2 * (q.w * q.z - q.x * q.y);
var gX = sqw - sqx - sqy + sqz;
var gY = 2 * (q.w * q.y - q.x * q.z);
if (gX > 0) {
_alpha = Math.atan2(aY, aX);
_beta = Math.asin(2 * wxyz / unitLength);
_gamma = Math.atan2(gY, gX);
} else {
_alpha = Math.atan2(-aY, -aX);
_beta = -Math.asin(2 * wxyz / unitLength);
_beta += _beta < 0 ? M_PI : - M_PI;
_gamma = Math.atan2(-gY, -gX);
}
}
// alpha is in [-pi, pi], make sure it is in [0, 2*pi).
if (_alpha < 0) {
_alpha += M_2_PI; // alpha [0, 2*pi)
}
// Convert to degrees
_alpha *= radToDeg;
_beta *= radToDeg;
_gamma *= radToDeg;
// apply derived euler angles to current object
this.set( _alpha, _beta, _gamma );
};
})();
this.rotateX = function ( angle ) {
FULLTILT.Euler.prototype.rotateByAxisAngle( this, [ 1, 0, 0 ], angle );
return this;
};
this.rotateY = function ( angle ) {
FULLTILT.Euler.prototype.rotateByAxisAngle( this, [ 0, 1, 0 ], angle );
return this;
};
this.rotateZ = function ( angle ) {
FULLTILT.Euler.prototype.rotateByAxisAngle( this, [ 0, 0, 1 ], angle );
return this;
};
// Initialize object values
this.set( alpha, beta, gamma );
};
FULLTILT.Euler.prototype = {
constructor: FULLTILT.Euler,
rotateByAxisAngle: function () {
var _matrix = new FULLTILT.RotationMatrix();
var outEuler;
return function ( targetEuler, axis, angle ) {
_matrix.setFromEuler( targetEuler );
_matrix = FULLTILT.RotationMatrix.prototype.rotateByAxisAngle( _matrix, axis, angle );
targetEuler.setFromRotationMatrix( _matrix );
return targetEuler;
};
}()
};