static void polygon_compute_area(polygon_t* poly)
{
// Compute the area using the fan algorithm.
poly->area = 0.0;
vector_t A, B;
int I = poly->ordering[0];
for (int j = 1; j < poly->num_vertices - 1; ++j)
{
// Form a triangle from vertex 0, vertex j, and vertex j+1.
int J = poly->ordering[j];
int K = poly->ordering[j+1];
point_displacement(&poly->vertices[I], &poly->vertices[J], &A);
point_displacement(&poly->vertices[I], &poly->vertices[K], &B);
poly->area += 0.5 * vector_cross_mag(&A, &B);
}
}
void quest_estimator_update(float q[4]) {
// Make best guess for which sequential rotation to use
if (fabsf(last_orientation[0]) > PI * 0.5f) {
// Rotation around x axis
sequential_rotations = 1;
}
else if (fabsf(last_orientation[2]) > PI * 0.5f) {
// Rotation around z axis
sequential_rotations = 3;
}
else {
// No rotation
sequential_rotations = 0;
}
sequential_rotations = 0;
#ifdef PRINT_SEQUENTIAL_ROTATION
PRINT("Rotation %i\n", sequential_rotations);
#endif
for (int attempt = 0; attempt < 4; attempt++) {
rotate_measurement_data();
float B[3][3];
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
B[row][col] = AO_rotated[row] * AR[col] * AccelA + MO_rotated[row] * MR[col] * MagA;
}
}
float minus_s[3][3];
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
minus_s[row][col] = -B[row][col] - B[col][row];
}
}
float sigma = MagA * (MR[0] * MO_rotated[0] + MR[1] * MO_rotated[1] + MR[2] * MO_rotated[2]) + AccelA * (AR[0] * AO_rotated[0] + AR[1] * AO_rotated[1] + AR[2] * AO_rotated[2]);
float Z[3];
Z[0] = B[1][2] - B[2][1];
Z[1] = B[2][0] - B[0][2];
Z[2] = B[0][1] - B[1][0];
float deltaCos = vector_dot(MO_rotated, AO_rotated) * vector_dot(MR, AR) + vector_cross_mag(MO_rotated, AO_rotated) * vector_cross_mag(MR, AR);
float lambda = sqrtf(MagA * MagA + 2 * MagA * AccelA * deltaCos + AccelA * AccelA);
float lamda_plus_sig = lambda + sigma;
minus_s[0][0] += lamda_plus_sig;
minus_s[1][1] += lamda_plus_sig;
minus_s[2][2] += lamda_plus_sig;
float ymat[3][3];
float det = mat3x3_det(minus_s);
if (det < 1e-2f) {
// We made the wrong sequential rotation assumption - try a different one
// This will only occur if spinning really fast or upside down
sequential_rotations++;
if (sequential_rotations > 3)
sequential_rotations = 0;
PRINT("Made incorrect first guess! Changing to %i\n",sequential_rotations);
continue;
}
mat3x3_inv_transpose(minus_s, det, ymat);
float a = B[0][1] - B[1][0];
float b = B[0][2] - B[2][0];
float c = B[1][2] - B[2][1];
for (int i = 0; i < 3; i++) {
q[i] = ymat[2][i] * a - ymat[1][i] * b + ymat[0][i] * c;
}
float qNorm = sqrtf(1.0f + q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
q[3] = 1.0f / qNorm;
for (int i = 0; i < 3; i++)
q[i] /= qNorm;
rotate_quaternion(q);
if (q[3] < 0.0f) {
for (int i = 0; i < 4; i++)
q[i] = -q[i];
}
quat_to_euler(q, last_orientation);
last_w = q[3];
//PRINT("%f %f %f\n", last_orientation[0], last_orientation[1], last_orientation[2]);
break;
}
}
