//note: euler conversion math based on notes from
// http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
// rotation matrix is constructed according to:
// R = ([Ry(heading)][Rz(attitude)])[Rx(bank)]
void matrix3::from_euler(const float heading, const float attitude, const float bank) {
float sin_heading, cos_heading, sin_attitude, cos_attitude, sin_bank, cos_bank;
fsincos(heading, sin_heading, cos_heading);
fsincos(attitude, sin_attitude, cos_attitude);
fsincos(bank, sin_bank, cos_bank);
m[0][0] = cos_heading * cos_attitude; m[0][1] = sin_heading * sin_bank - cos_heading * sin_attitude * cos_bank; m[0][2] = cos_heading * sin_attitude * sin_bank + sin_heading * cos_bank;
m[1][0] = sin_attitude; m[1][1] = cos_attitude * cos_bank; m[1][2] =-cos_attitude * sin_bank;
m[2][0] = -sin_heading * cos_attitude; m[2][1] = sin_heading * sin_attitude * cos_bank + cos_heading * sin_bank; m[2][2] =-sin_heading * sin_attitude * sin_bank + cos_heading * cos_bank;
}
void matrix3::from_rotation(const float angle, const vector3& axis) {
float sin_angle, cos_angle;
fsincos(angle, sin_angle, cos_angle);
const vector3 axis2 = axis * axis;
const float l2 = (axis2.x * axis2.x) + (axis2.y * axis2.y) + (axis2.z * axis2.z);
const float l2_sqrt = sqrtf(l2);
if (approx_ne(l2, 0.f)) {
m[0][0] = (axis2.x + (axis2.y + axis2.z) * cos_angle) / l2;
m[0][1] = (axis.x * axis.y * (1 - cos_angle) - axis.z * l2_sqrt * sin_angle) / l2;
m[0][2] = (axis.x * axis.z * (1 - cos_angle) + axis.y * l2_sqrt * sin_angle) / l2;
m[1][0] = (axis.x * axis.y * (1 - cos_angle) + axis.z * l2_sqrt * sin_angle) / l2;
m[1][1] = (axis2.y + (axis2.x + axis2.z) * cos_angle) / l2;
m[1][2] = (axis.y * axis.z * (1 - cos_angle) - axis.x * l2_sqrt * sin_angle) / l2;
m[2][0] = (axis.x * axis.z * (1 - cos_angle) - axis.y * l2_sqrt * sin_angle) / l2;
m[2][1] = (axis.y * axis.z * (1 - cos_angle) + axis.x * l2_sqrt * sin_angle) / l2;
m[2][2] = (axis2.z + (axis2.x + axis2.y) * cos_angle) / l2;
}
else {
(*this) = matrix3(mth::IDENTITY_INITIALIZATION);
}
}
static void filter_shelf_coefs(unsigned long cutoff, float A, bool low, int32_t *c){
int32_t sin, cos;
int32_t b0, b1, a0, a1; /* s3.28 */
const long g = integerGain(TXSqrtApprox(A)) << 4; /* 10^(db/40), s3.28 */
sin = fsincos(cutoff/2, &cos);
if (low) {
const int32_t sin_div_g = DIV64(sin, g, 25);
cos >>= 3;
b0 = FRACMUL(sin, g) + cos; /* 0.25 .. 4.10 */
b1 = FRACMUL(sin, g) - cos; /* -1 .. 3.98 */
a0 = sin_div_g + cos; /* 0.25 .. 4.10 */
a1 = sin_div_g - cos; /* -1 .. 3.98 */
} else {
int fcos(floatx80 &a, float_status_t &status)
{
return fsincos(a, 0, &a, status);
}
