setEuler method
Set quaternion with rotation of yaw
, pitch
and roll
.
Implementation
void setEuler(double yaw, double pitch, double roll) {
final double halfYaw = yaw * 0.5;
final double halfPitch = pitch * 0.5;
final double halfRoll = roll * 0.5;
final double cosYaw = math.cos(halfYaw);
final double sinYaw = math.sin(halfYaw);
final double cosPitch = math.cos(halfPitch);
final double sinPitch = math.sin(halfPitch);
final double cosRoll = math.cos(halfRoll);
final double sinRoll = math.sin(halfRoll);
_qStorage[0] = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
_qStorage[1] = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
_qStorage[2] = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
_qStorage[3] = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
}