int
main(int argc, char *argv[])
{
q_type multQuat;
q_type xformedPoint;
q_type point;
double matrix[4][4];
/*
* read in, echo, and normalize 2 quaternions
*/
printf("\nEnter xform quaternion: (vec, s) ");
scanf("%lf %lf %lf %lf",
&multQuat[0], &multQuat[1], &multQuat[2], &multQuat[3]);
printf("\nEnter point: (x y z) ");
scanf("%lf %lf %lf",
&point[Q_X], &point[Q_Y], &point[Q_Z]);
point[Q_W] = 0.0;
/*
* matrix of product quat
*/
q_to_col_matrix(matrix, multQuat);
printf("Transformation (column) matrix:\n");
q_print_matrix(matrix);
/*
* xformedPoint = (multQuat * candQuat) * invertedQuat
*/
q_xform(xformedPoint, multQuat, point);
printf("Xform Result:\n");
q_print(xformedPoint);
return(0);
} /* main */
int main(int argc, char *argv[])
{
/* Set the matrix values directly. Ugly but true. */
q_matrix_type col_matrix = { {-0.999848, 0.002700, -0.017242, 0.001715},
{ 0, -0.987960, -0.154710, -0.207295},
{-0.017452, -0.154687, 0.987809, -0.098239},
{ 0, 0, 0, 1} };
/*
q_matrix_type col_matrix = { { 0.999848, -0.002700, 0.017242, -0.001715},
{ 0, 0.987960, 0.154710, 0.207295},
{-0.017452, -0.154687, 0.987809, -0.098239},
{ 0, 0, 0, 1} };
*/
q_matrix_type row_matrix;
q_xyz_quat_type q;
q_xyz_quat_type q_inverse;
/* Transpose the column matrix into a row matrix */
unsigned i,j;
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
row_matrix[i][j] = col_matrix[j][i];
}
}
printf("Row Matrix directly:\n");
q_print_matrix(row_matrix);
/* Convert to pos/quat and print. */
q_row_matrix_to_xyz_quat(&q, row_matrix);
printf("XYZ Quat:\n");
q_vec_print(q.xyz);
q_print(q.quat);
/* Invert the result and print that. */
printf("XYZ Quat inverse:\n");
q_xyz_quat_invert(&q_inverse, &q);
q_vec_print(q_inverse.xyz);
q_print(q_inverse.quat);
} /* main */
int main(int argc, char *argv[])
{
unsigned i;
q_matrix_type row_matrix;
double sx, sy, sz; /* Scale in X, Y, and Z */
q_xyz_quat_type q;
q_vec_type euler;
const char *infile_name = NULL;
FILE *infile = NULL;
char line[1025];
/* Check the command-line arguments. */
if (argc != 2) {
Usage(argv[0]);
} else {
infile_name = argv[1];
}
/* Open and read the file */
/* Transpose the matrix while it is being read into a row matrix. */
if ( (infile = fopen(infile_name, "r")) == NULL) {
fprintf(stderr,"Cannot open %s for reading\n", infile_name);
return -2;
}
line[1024] = '\0'; /* Ensure null-termination */
/* Print the comment line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read comment line from %s\n", infile_name);
return -3;
}
printf("Comment: %s\n", line);
/* Read and parse the first matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 1 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][0], &row_matrix[1][0], &row_matrix[2][0], &row_matrix[3][0]) != 4) {
fprintf(stderr, "Could not parse matrix line 1 from %s\n", infile_name);
return -3;
}
/* Read and parse the second matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 2 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][1], &row_matrix[1][1], &row_matrix[2][1], &row_matrix[3][1]) != 4) {
fprintf(stderr, "Could not parse matrix line 2 from %s\n", infile_name);
return -3;
}
/* Read and parse the third matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 3 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][2], &row_matrix[1][2], &row_matrix[2][2], &row_matrix[3][2]) != 4) {
fprintf(stderr, "Could not parse matrix line 3 from %s\n", infile_name);
return -3;
}
/* Read and parse the fourth matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 4 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][3], &row_matrix[1][3], &row_matrix[2][3], &row_matrix[3][3]) != 4) {
fprintf(stderr, "Could not parse matrix line 4 from %s\n", infile_name);
return -3;
}
printf("Row Matrix directly:\n");
q_print_matrix(row_matrix);
/* The conversion routines cannot handle non-unit scaling. Compute the
* scale of each row in the matrix, print the scales, then divide each
* row to normalize before we do the rest of the math. */
sx = sqrt(row_matrix[0][0]*row_matrix[0][0] +
row_matrix[0][1]*row_matrix[0][1] +
row_matrix[0][2]*row_matrix[0][2]);
sy = sqrt(row_matrix[1][0]*row_matrix[1][0] +
row_matrix[1][1]*row_matrix[1][1] +
row_matrix[1][2]*row_matrix[1][2]);
sz = sqrt(row_matrix[2][0]*row_matrix[2][0] +
row_matrix[2][1]*row_matrix[2][1] +
row_matrix[2][2]*row_matrix[2][2]);
printf("Scale:\n %lg, %lg, %lg\n", sx, sy, sz);
for (i = 0; i < 3; i++) {
row_matrix[0][i] /= sx;
row_matrix[1][i] /= sy;
row_matrix[2][i] /= sz;
}
/* Convert to pos/quat, then Euler and print. */
q_row_matrix_to_xyz_quat(&q, row_matrix);
q_to_euler(euler, q.quat);
printf("Translation:\n %lf, %lf, %lf\n", q.xyz[0], q.xyz[1], q.xyz[2]);
printf("Euler angles (degrees):\n %lg, %lg, %lg\n",
euler[2] * DEGREES_PER_RADIAN, /* Rotation about X is stored in the third component. */
euler[1] * DEGREES_PER_RADIAN,
euler[0] * DEGREES_PER_RADIAN);
} /* main */