/// Interpolates the values at three 2D points to the
/// location of a third point.
static double interpolate(double p1X, double p1Y, double val1, double p2X,
double p2Y, double val2, double p3X, double p3Y,
double val3, double pointX, double pointY) {
// Fit a plane to three points, using their values as the
// third dimension.
q_vec_type p1, p2, p3;
q_vec_set(p1, p1X, p1Y, val1);
q_vec_set(p2, p2X, p2Y, val2);
q_vec_set(p3, p3X, p3Y, val3);
// The normalized cross product of the vectors from the first
// point to each of the other two is normal to this plane.
q_vec_type v1, v2;
q_vec_subtract(v1, p2, p1);
q_vec_subtract(v2, p3, p1);
q_vec_type ABC;
q_vec_cross_product(ABC, v1, v2);
if (q_vec_magnitude(ABC) == 0) {
// We can't get a normal, degenerate points, just return the first
// value.
return val1;
}
q_vec_normalize(ABC, ABC);
// Solve for the D associated with the plane by filling back
// in one of the points. This is done by taking the dot product
// of the ABC vector with the first point. We then solve for D.
// AX + BY + CZ + D = 0; D = -(AX + BY + CZ)
double D = -q_vec_dot_product(ABC, p1);
// Evaluate the plane equations at our input point, which will interpolate
// or extrapolate our values.
// We're solving for Z in this case, so we get
// CZ = -(AX + BY + D); Z = -(AX + BY + D)/C;
return -(ABC[0] * pointX + ABC[1] * pointY + D) / ABC[2];
}
int
main(int argc, char *argv[])
{
int i, j;
double mag;
q_vec_type vec;
MatrixType A, B, result;
printf("\nEnter 1st pphigs matrix: \n");
/* 3 rows, 4 columns */
for ( i = 0; i < 3; i++ )
{
for ( j = 0; j < 4; j++ )
scanf("%f", &A[i][j]);
}
printf("\nEnter 2nd pphigs matrix: \n");
/* 3 rows, 4 columns */
for ( i = 0; i < 3; i++ )
{
for ( j = 0; j < 4; j++ )
scanf("%f", &B[i][j]);
}
qp_matrix_mult(result, A, B);
/* mag of one axis = mag of all */
mag = qp_matrix_3x3_determinant(result);
printf("result determinant = %lf\n", mag);
printf("result:\n");
qp_print_matrix(result);
/* put each row into a qvec */
for ( j = 0; j < 3; j++ )
vec[j] = result[0][j];
/* mag of one axis = mag of all */
mag = q_vec_magnitude(vec);
printf("result scale = %lf\n", mag);
/* normalize matrix */
for ( i = 0; i < 3; i++ )
for ( j = 0; j < 3; j++ )
result[i][j] /= mag;
printf("normalized result:\n");
qp_print_matrix(result);
/* put each row into a qvec */
for ( j = 0; j < 3; j++ )
vec[j] = result[0][j];
/* mag of one axis = mag of all */
mag = q_vec_magnitude(vec);
printf("normalized mag = %lf\n", mag);
} /* main */
