Inverse trilerp with bilinear coords

/*
* Create rotation matrix
*/
float3x3 rotMat(float angle, float3 axis)
{
    float c, s;
    sincos(angle, s, c);

    float t = 1 - c;
    float x = axis.x;
    float y = axis.y;
    float z = axis.z;

    return float3x3(
        t * x * x + c, t * x * y - s * z, t * x * z + s * y,
        t * x * y + s * z, t * y * y + c, t * y * z - s * x,
        t * x * z - s * y, t * y * z + s * x, t * z * z + c
        );
}

/*
* Calculate barycentric coordinates from the 4 tetrahedron verts.
*/
float4 doBary(float3 p, float3 a, float3 b, float3 c, float3 d)
{
    float3 vap = p - a;
    float3 vbp = p - b;
    float3 vab = b - a;
    float3 vac = c - a;
    float3 vad = d - a;
    float3 vbc = c - b;
    float3 vbd = d - b;
    float v6 = 1.0 / dot(vab, cross(vac, vad));
    float va6 = dot(vbp, cross(vbd, vbc)) * v6;
    float vb6 = dot(vap, cross(vac, vad)) * v6;
    float vc6 = dot(vap, cross(vad, vab)) * v6;
    return float4(va6, vb6, vc6, 1.0 - va6 - vb6 - vc6);
}

/*
* Calculate cartesian from barycentric coordinates using 4 tetrahedron verts.
*/
float3 invBary(float4 bary, float3 a, float3 b, float3 c, float3 d)
{
    float3 ret = float3(0.0, 0.0, 0.0);
    ret.x = bary.x * a.x + bary.y * b.x + bary.z * c.x + bary.w * d.x;
    ret.y = bary.x * a.y + bary.y * b.y + bary.z * c.y + bary.w * d.y;
    ret.z = bary.x * a.z + bary.y * b.z + bary.z * c.z + bary.w * d.z;
    return ret;
}

struct Tetrahedra
{
    float3 p0;
    float3 p1;
    float3 p2;
    float3 p3;
};

Tetrahedra makeTet(float4 p0, float4 p1, float4 p2, float4 p3)
{
    Tetrahedra t;
    t.p0 = p0.xyz;
    t.p1 = p1.xyz;
    t.p2 = p2.xyz;
    t.p3 = p3.xyz;
    return t;
}

void tetrahedralizeCube(float4 t[4], float4 b[4], out Tetrahedra tet[5])
{
    tet[0] = makeTet(t[0], b[2], t[3], b[0]); // centre
    tet[1] = makeTet(t[0], t[1], t[3], b[0]); // t0
    tet[2] = makeTet(t[2], t[3], t[1], b[2]); // t2
    tet[3] = makeTet(b[1], b[0], b[2], t[1]); // b1
    tet[4] = makeTet(b[3], b[2], b[0], t[3]); // b3
}

/*
* Map cartesian coordinate onto unit cube using barycentric coordinates.
*/
float3 mapToUnitCube(float3 pos, float4 t[4], float4 b[4], int rot)
{
    Tetrahedra tet[5], uTet[5];

    float3x3 mRot = rotMat(PI * 0.5 * float(rot), float3(0.0, 1.0, 0.0));
    float4 uTop[4];
    uTop[0] = float4(mul(float3(1.0, 1.0, 1.0), mRot), 0.0);
    uTop[1] = float4(mul(float3(-1.0, 1.0, 1.0), mRot), 0.0);
    uTop[2] = float4(mul(float3(-1.0, 1.0, -1.0), mRot), 0.0);
    uTop[3] = float4(mul(float3(1.0, 1.0, -1.0), mRot), 0.0);
    float4 uBot[4];
    uBot[0] = float4(mul(float3(1.0, -1.0, 1.0), mRot), 0.0);
    uBot[1] = float4(mul(float3(-1.0, -1.0, 1.0), mRot), 0.0);
    uBot[2] = float4(mul(float3(-1.0, -1.0, -1.0), mRot), 0.0);
    uBot[3] = float4(mul(float3(1.0, -1.0, -1.0), mRot), 0.0);

    tetrahedralizeCube(t, b, tet);
    tetrahedralizeCube(uTop, uBot, uTet);

    float highest = -999.9;
    float4 bary = float4(0.0, 0.0, 0.0, 0.0);
    int iT = -1;

    // determine which tetrahedron the point is closest to (if not inside)
    // by finding the barycentric coords of each tetrahedron with the largest
    // minimum.
    for (int i = 0; i < 5; i++)
    {
        float4 bTest = doBary(pos, tet[i].p0, tet[i].p1, tet[i].p2, tet[i].p3);
        float low = min(min(min(bTest.x, bTest.y), bTest.z), bTest.w);
        if (low >= 0.0)
        {
            // if all barycentric coords are positive, point is definitely within
            // that tetrahedra.
            bary = bTest;
            iT = i;
            break;
        }
        else if (low > highest)
        {
            bary = bTest;
            highest = low;
            iT = i;
        }
    }
    // take the barycentric coords of point on the nearest tetrahedron, and
    // convert back into cartesian coords using the corresponding unit tetrahedron.
    return invBary(bary, uTet[iT].p0, uTet[iT].p1, uTet[iT].p2, uTet[iT].p3);
}