int main(void)
{
double a[3], b[3], c[3], ri[3], r[3];
FILE * openedFile;
printf("\nEnter the 3 components of vector a, separated by spaces:");
scanf("%lf %lf %lf", a, a+1, a+2);
printf("\nEnter the 3 components of vector b, separated by spaces:");
scanf("%lf %lf %lf", b, b+1, b+2);
printf("\nEnter the 3 components of vector c, separated by spaces:");
scanf("%lf %lf %lf", c, c+1, c+2);
openedFile = fopen("results1.txt", "w");
if (!openedFile)
{
fprintf(stderr, "Could not open results1.txt");
return -1;
}
fprintf(openedFile, "\nThe triple vector cross-product of a x (b x c) is:\n");
v3cross(b,c,ri);
v3cross(a,ri,r);
v3fprint(openedFile,r);
fclose(openedFile);
openedFile = fopen("results2.txt", "w");
if (!openedFile)
{
fprintf(stderr, "Could not open results2.txt");
return -1;
}
fprintf(openedFile, "\nThe triple vector cross-product of (a x b) x c is:\n");
v3cross(a,b,ri);
v3cross(ri,c,r);
v3fprint(openedFile,r);
fclose(openedFile);
return 0;
}
bool IMU_GetErectorQuat( quat *q_erect_out, quat *frame, vec3 *v_measured, vec3 *v_reference, float rads_sec, float dt)
// Calculates the quaternion needed to rotate the measured vector to the reference vector (world space) at a fixed correction rate
// Returns TRUE if correction is necessary, or FALSE if it is below the threshold of caring.
{
// Get the rotation axis we'll use to erect.
// (Normalize returns the length. We do a lower limit. No sense in correcting if we're close)
vec3 c;
v3cross(&c,v_measured, v_reference);
if( v3normalize(&c,&c) > 1.0f * RADIANS_PER_DEGREE)
{
// Get the angle between the two vectors, and clamp to that limit. We don't want to overshoot.
// Angles are always positive since the rotation angle flips appropriately.
float rads = rads_sec * g_rps_scale * dt;
float a = fabs(clamped_acos(v3dot(v_measured, v_reference)));
if((rads>a) && (rads>0))
{
rads=a;
}
else
if((rads<-a) && (rads<0))
{
rads=-a;
}
//ULIMIT(rads,a);
// Get the quat that rotates our sensor toward the world vector by the specified amount
Quat_SetAxisAndAngle( q_erect_out, &c, rads);
return true;
}
return false;
}
void add_row_area(DCELL * top, DCELL * bottom, double sz, struct Cell_head *w,
double *low, double *high)
{
double guess1, guess2, mag, tedge1[3], tedge2[3], crossp[3];
int col;
for (col = 0; col < w->cols - 1; col++) {
/*
For each cell**, we triangulate the four corners in
two different ways, 1) UppperLeft to LowerRight diagonal
and 2) LowerLeft to UpperRight diagonal. Then we add the
smaller of the two areas to "low" and the greater of
the two areas to "high".
** here, the "cell" is actually the quadrangle formed by
the center point of four cells, since these are the
known elevation points.
*/
/* If NAN go to next or we get NAN for everything */
if (Rast_is_d_null_value(&(bottom[col + 1])) ||
Rast_is_d_null_value(&(top[col])) ||
Rast_is_d_null_value(&(top[col + 1])) ||
Rast_is_d_null_value(&(bottom[col]))
)
continue;
/* guess1 --- ul to lr diag */
{
tedge1[X] = w->ew_res;
tedge1[Y] = -w->ns_res;
tedge1[Z] = sz * (bottom[col + 1] - top[col]);
/* upper */
tedge2[X] = 0.0;
tedge2[Y] = w->ns_res;
tedge2[Z] = sz * (top[col + 1] - bottom[col + 1]);
v3cross(tedge1, tedge2, crossp);
v3mag(crossp, &mag);
guess1 = .5 * mag;
/* lower */
tedge2[X] = -w->ew_res;
tedge2[Y] = 0.0;
tedge2[Z] = sz * (bottom[col] - bottom[col + 1]);
v3cross(tedge1, tedge2, crossp);
v3mag(crossp, &mag);
guess1 += .5 * mag;
}
/* guess2 --- ll to ur diag */
{
tedge1[X] = w->ew_res;
tedge1[Y] = w->ns_res;
tedge1[Z] = sz * (top[col + 1] - bottom[col]);
/* upper */
tedge2[X] = -w->ew_res;
tedge2[Y] = 0.0;
tedge2[Z] = sz * (top[col + 1] - top[col + 1]);
v3cross(tedge1, tedge2, crossp);
v3mag(crossp, &mag);
guess2 = .5 * mag;
/* lower */
tedge2[X] = 0.0;
tedge2[Y] = -w->ns_res;
tedge2[Z] = sz * (bottom[col + 1] - top[col + 1]);
v3cross(tedge1, tedge2, crossp);
v3mag(crossp, &mag);
guess2 += .5 * mag;
}
*low += (guess1 < guess2) ? guess1 : guess2;
*high += (guess1 < guess2) ? guess2 : guess1;
} /* ea col */
}
