static void rotateM(float* m,
float a, float x, float y, float z) {
float temp[16];
float temp2[16];
setRotateM(temp, 0, a, x, y, z);
mulMM(temp2, m, temp);
memcpy(m, temp2, 16 * sizeof(float));
}
/* Compute the inverse deformation field within a single cube */
static void invert_it(int x0, int x1, int x2, float *y0, float *y1, float *y2,
int dim_f[3], float *iy0, float *iy1, float *iy2, REAL U[4][3], REAL V[4][3])
{
int i, j, k, vox[MAXV][3], nvox;
REAL Y0[4][3], Y[4][3], M[4][3], IM[4][3];
/* Determine tetrahedral arrangement */
k = (x0%2)==(x1%2)==(x2%2);
for(i=0; i<5; i++) /* Five tetrahedra within a cube */
{
/* Find the vertices (in mm space) */
Y0[0][0] = y0[off[k][0][i]]; Y0[0][1] = y1[off[k][0][i]]; Y0[0][2] = y2[off[k][0][i]];
Y0[1][0] = y0[off[k][1][i]]; Y0[1][1] = y1[off[k][1][i]]; Y0[1][2] = y2[off[k][1][i]];
Y0[2][0] = y0[off[k][2][i]]; Y0[2][1] = y1[off[k][2][i]]; Y0[2][2] = y2[off[k][2][i]];
Y0[3][0] = y0[off[k][3][i]]; Y0[3][1] = y1[off[k][3][i]]; Y0[3][2] = y2[off[k][3][i]];
/* Convert vertex co-ordinates to voxels */
mulMX(Y, U, Y0);
/* Compute affine transform mapping vertices together */
getM(Y, ix[k][i], M, x0, x1, x2);
if (mxIsFinite(M[0][0])) /* Prevent from bombing out when NaNs are encountered */
{
/* Find integer co-ordinates within tetrahedron */
scan_tetrahedron(Y, &nvox, vox, MAXV);
if (nvox>0)
{
/* Invert the affine mapping */
invertM(M, IM);
/* Convert the mapping from voxels to mm */
mulMM(M, V, IM);
/* Insert the new mappings into each voxel within the tetrahedron */
for(j=0; j<nvox; j++)
{
if ((vox[j][0]>=1) && (vox[j][0]<=dim_f[0]) &&
(vox[j][1]>=1) && (vox[j][1]<=dim_f[1]) &&
(vox[j][2]>=1) && (vox[j][2]<=dim_f[2]))
{
int o = vox[j][0]+dim_f[0]*(vox[j][1]+dim_f[1]*vox[j][2]);
iy0[o] = M[0][0]*vox[j][0] + M[1][0]*vox[j][1] + M[2][0]*vox[j][2] + M[3][0];
iy1[o] = M[0][1]*vox[j][0] + M[1][1]*vox[j][1] + M[2][1]*vox[j][2] + M[3][1];
iy2[o] = M[0][2]*vox[j][0] + M[1][2]*vox[j][1] + M[2][2]*vox[j][2] + M[3][2];
}
}
}
}
}
}
