surface.c

Go to the documentation of this file.

/*
 * This work is licensed under a Creative Commons 
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * 
 *    http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 * 
 * You are free:
 * 
 *    to Share - to copy, distribute, display, and perform the work
 *    to Remix - to make derivative works
 * 
 * Under the following conditions:
 * 
 *    Attribution. You must attribute the work in the manner specified by the
 *    author or licensor (but not in any way that suggests that they endorse you
 *    or your use of the work).
 * 
 *    Noncommercial. You may not use this work for commercial purposes.
 * 
 *    Share Alike. If you alter, transform, or build upon this work, you may
 *    distribute the resulting work only under the same or similar license to
 *    this one.
 * 
 * For any reuse or distribution, you must make clear to others the license
 * terms of this work. The best way to do this is by including this header.
 * 
 * Any of the above conditions can be waived if you get permission from the
 * copyright holder.
 * 
 * Apart from the remix rights granted under this license, nothing in this
 * license impairs or restricts the author's moral rights.
 */


#include <jwsc/config.h>

#include <stdlib.h>
#include <stdio.h>
#include <inttypes.h>
#include <assert.h>
#include <math.h>
#include <string.h>
#include <errno.h>

#include "jwsc/base/error.h"
#include "jwsc/vector/vector.h"
#include "jwsc/vector/vector_math.h"
#include "jwsc/matblock/matblock.h"
#include "jwsc/matblock/matblock_fft.h"
#include "jwsc/matblock/matblock_conv.h"
#include "jwsc/filter/3d.h"
#include "jwsc/prob/pmf.h"
#include "jwsc/imgblock/surface.h"

#ifdef JWSC_HAVE_DMALLOC
#include <dmalloc.h>
#endif


#define INV_SQRT_2 0.70710678


#define INV_SQRT_3 0.57735027


typedef struct
{
    float dcol;

    float drow;

    float dmat;

    float mag;

    uint8_t marked;
}
Gradient_f;


static Error* create_gradient_map_f
(
    Gradient_f****    map_out, 
    const Matblock_f* m, 
    float             sigma,
    float             grad_mat_weight,
    float             grad_row_weight,
    float             grad_col_weight
)
{
    uint32_t num_mats, num_rows, num_cols;
    uint32_t mat, row, col;
    float    dmat, dcol, drow;

    Matblock_f*   gauss  = NULL;
    Matblock_f*   m_dmat = NULL;
    Matblock_f*   m_drow = NULL;
    Matblock_f*   m_dcol = NULL;
    Gradient_f*** map;
    Gradient_f**  map_rows;
    Gradient_f*   map_elts;
    Error*        e;

    num_mats = m->num_mats;
    num_rows = m->num_rows;
    num_cols = m->num_cols;

    assert(*map_out == NULL);

#ifdef JWSC_HAVE_FFTW3F
    if ((e = create_3d_gaussian_dx_filter_f(&gauss, sigma, sigma, sigma,
                    num_mats, num_rows, num_cols)) != NULL)
    {
        return e;
    }
    shift_matblock_f(&gauss, gauss);
    assert(convolve_matblock_fft_f(&m_dcol, m, gauss) == NULL);

    if ((e = create_3d_gaussian_dy_filter_f(&gauss, sigma, sigma, sigma,
                    num_mats, num_rows, num_cols)) != NULL)
    {
        return e;
    }
    shift_matblock_f(&gauss, gauss);
    assert(convolve_matblock_fft_f(&m_drow, m, gauss) == NULL);

    if ((e = create_3d_gaussian_dz_filter_f(&gauss, sigma, sigma, sigma,
                    num_mats, num_rows, num_cols)) != NULL)
    {
        return e;
    }
    shift_matblock_f(&gauss, gauss);
    assert(convolve_matblock_fft_f(&m_dmat, m, gauss) == NULL);
#else
    if ((e = create_auto_3d_gaussian_dx_filter_f(&gauss, sigma, sigma, 
                    sigma)) != NULL)
    {
        return e;
    }
    assert(convolve_matblock_f(&m_dcol, m, gauss) == NULL);

    if ((e = create_auto_3d_gaussian_dy_filter_f(&gauss, sigma, sigma, 
                    sigma)) != NULL)
    {
        return e;
    }
    assert(convolve_matblock_f(&m_drow, m, gauss) == NULL);

    if ((e = create_auto_3d_gaussian_dz_filter_f(&gauss, sigma, sigma, 
                    sigma)) != NULL)
    {
        return e;
    }
    assert(convolve_matblock_f(&m_dmat, m, gauss) == NULL);
#endif

    free_matblock_f(gauss);

    assert(map_elts = malloc(num_mats*num_rows*num_cols * sizeof(Gradient_f)));
    assert(map_rows = malloc(num_mats*num_rows*sizeof(Gradient_f*)));
    assert(*map_out = malloc(num_mats*sizeof(Gradient_f**)));
    map = *map_out;

    for (mat = 0; mat < num_mats; mat++)
    {
        map[ mat ] = &(map_rows[ mat*num_rows ]);

        for (row = 0; row < num_rows; row++)
        {
            map[ mat ][ row ] = &(map_elts[ (row+mat*num_rows)*num_cols ]);
        }
    }

    for (mat = 0; mat < num_mats; mat++)
    {
        for (row = 0; row < num_rows; row++)
        {
            for (col = 0; col < num_cols; col++)
            {
                dcol = m_dcol->elts[ mat ][ row ][ col ];
                drow = m_drow->elts[ mat ][ row ][ col ];
                dmat = m_dmat->elts[ mat ][ row ][ col ];

                map[ mat ][ row ][ col ].dcol = dcol;
                map[ mat ][ row ][ col ].drow = drow;
                map[ mat ][ row ][ col ].dmat = dmat;
                map[ mat ][ row ][ col ].mag  = sqrt(grad_col_weight*dcol*dcol +
                                                     grad_row_weight*drow*drow +
                                                     grad_mat_weight*dmat*dmat);
                map[ mat ][ row ][ col ].marked = 0;

                if (map[ mat ][ row ][ col ].mag >= 1.0e-8)
                {
                    map[ mat ][ row ][ col ].dcol /= 
                        map[ mat ][ row ][ col ].mag;
                    map[ mat ][ row ][ col ].drow /= 
                        map[ mat ][ row ][ col ].mag;
                    map[ mat ][ row ][ col ].dmat /= 
                        map[ mat ][ row ][ col ].mag;
                }
            }
        }
    }

    free_matblock_f(m_dcol);
    free_matblock_f(m_drow);
    free_matblock_f(m_dmat);

    return NULL;
}


static Gradient_f* follow_gradient_f
(
    int32_t*      grad_mat_in_out, 
    int32_t*      grad_row_in_out, 
    int32_t*      grad_col_in_out,
    float         dir_x,
    float         dir_y,
    float         dir_z,
    Gradient_f*** map,
    uint32_t      num_mats,
    uint32_t      num_rows,
    uint32_t      num_cols
)
{
    static const int neighbor_positions[ 26 ][ 3 ] = {
        {1,0,1},{1,1,1},{0,1,1},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},
        {0,0,1},{1,0,0},{1,1,0},{0,1,0},{-1,1,0},{-1,0,0},{-1,-1,0},{0,-1,0},
        {1,-1,0},{1,0,-1},{1,1,-1},{0,1,-1},{-1,1,-1},{-1,0,-1},{-1,-1,-1},
        {0,-1,-1},{1,-1,-1},{0,0,-1} };
    static const float neighbor_vectors[ 26 ][ 3 ] = {
        {INV_SQRT_2,0,INV_SQRT_2}, {INV_SQRT_3,INV_SQRT_3,INV_SQRT_3}, 
        {0,INV_SQRT_2,INV_SQRT_2}, {-INV_SQRT_3,INV_SQRT_3,INV_SQRT_3},
        {-INV_SQRT_2,0,INV_SQRT_2}, {-INV_SQRT_3,-INV_SQRT_3,INV_SQRT_3},
        {0,-INV_SQRT_2,INV_SQRT_2}, {INV_SQRT_3,-INV_SQRT_3,INV_SQRT_3},
        {0,0,1}, {1,0,0}, {INV_SQRT_2,INV_SQRT_2,0}, {0,1,0},
        {-INV_SQRT_2,INV_SQRT_2,0}, {-1,0,0}, {-INV_SQRT_2,-INV_SQRT_2,0},
        {0,-1,0}, {INV_SQRT_2,-INV_SQRT_2,0}, {INV_SQRT_2,0,-INV_SQRT_2},
        {INV_SQRT_3,INV_SQRT_3,-INV_SQRT_3}, {0,INV_SQRT_2,-INV_SQRT_2},
        {-INV_SQRT_3,INV_SQRT_3,-INV_SQRT_3}, {-INV_SQRT_2,0,-INV_SQRT_2}, 
        {-INV_SQRT_3,-INV_SQRT_3,-INV_SQRT_3}, {0,-INV_SQRT_2,-INV_SQRT_2},
        {INV_SQRT_3,-INV_SQRT_3,-INV_SQRT_3},{0,0,-1} };

    uint16_t i;
    uint16_t max_neighbor;
    int32_t  grad_col, grad_row, grad_mat;
    float    max_dp;
    float    dp;

    Gradient_f* grad;

    grad_col = *grad_col_in_out;
    grad_row = *grad_row_in_out;
    grad_mat = *grad_mat_in_out;
    
    grad         = &(map[ grad_mat ][ grad_row ][ grad_col ]);
    max_neighbor = 0;
    max_dp       = dir_x * neighbor_vectors[ 0 ][ 0 ] +
                   dir_y * neighbor_vectors[ 0 ][ 1 ] +
                   dir_z * neighbor_vectors[ 0 ][ 2 ];

    for (i = 1; i < 26; i++)
    {
        dp = dir_x * neighbor_vectors[ i ][ 0 ] +
             dir_y * neighbor_vectors[ i ][ 1 ] +
             dir_z * neighbor_vectors[ i ][ 2 ];

        /* Should have some stochastic tie-breaker here. */
        if (dp >= max_dp)
        {
            max_dp = dp;
            max_neighbor = i;
        }
    }

    grad_mat += neighbor_positions[ max_neighbor ][ 2 ];
    grad_row += neighbor_positions[ max_neighbor ][ 1 ];
    grad_col += neighbor_positions[ max_neighbor ][ 0 ];

    if (grad_mat == -1) 
        grad_mat = 0;
    else if (grad_mat == num_mats) 
        grad_mat = num_mats-1;
   
    if (grad_row == -1) 
        grad_row = 0;
    else if (grad_row == num_rows) 
        grad_row = num_rows-1;
   
    if (grad_col == -1) 
        grad_col = 0;
    else if (grad_col == num_cols) 
        grad_col = num_cols-1;

    *grad_col_in_out = grad_col;
    *grad_row_in_out = grad_row;
    *grad_mat_in_out = grad_mat;

    return &map[ grad_mat ][ grad_row ][ grad_col ];
}


static Gradient_f* get_maximal_along_gradient_f
(
    int32_t*      grad_mat_in_out, 
    int32_t*      grad_row_in_out, 
    int32_t*      grad_col_in_out, 
    Gradient_f*** map,
    uint32_t      num_mats,
    uint32_t      num_rows,
    uint32_t      num_cols
) 
{
    int32_t grad_mat, grad_row, grad_col;
    int32_t curr_grad_mat, curr_grad_row, curr_grad_col;
    int32_t next_grad_mat, next_grad_row, next_grad_col;
    int32_t prev_grad_mat, prev_grad_row, prev_grad_col;

    Gradient_f* curr_grad;
    Gradient_f* next_grad;
    Gradient_f* prev_grad;
    Gradient_f* max_grad;

    grad_mat = *grad_mat_in_out;
    grad_row = *grad_row_in_out;
    grad_col = *grad_col_in_out;

    curr_grad_mat = grad_mat;
    curr_grad_row = grad_row;
    curr_grad_col = grad_col;
    curr_grad     = &map[ curr_grad_mat ][ curr_grad_row ][ curr_grad_col ];

    prev_grad_mat = curr_grad_mat;
    prev_grad_row = curr_grad_row;
    prev_grad_col = curr_grad_col;
    prev_grad     = follow_gradient_f(&prev_grad_mat, &prev_grad_row, 
                                      &prev_grad_col, -(curr_grad->dcol), 
                                      -(curr_grad->drow), -(curr_grad->dmat), 
                                      map, num_mats, num_rows, num_cols);

    next_grad_mat = curr_grad_mat;
    next_grad_row = curr_grad_row;
    next_grad_col = curr_grad_col;
    next_grad     = follow_gradient_f(&next_grad_mat, &next_grad_row, 
                                      &next_grad_col, curr_grad->dcol, 
                                      curr_grad->drow, curr_grad->dmat, map, 
                                      num_mats, num_rows, num_cols);

    max_grad = NULL;
    while (max_grad == NULL)
    {
        if (curr_grad->mag >= next_grad->mag &&
            curr_grad->mag >= prev_grad->mag)
        {
            max_grad = curr_grad;
        }
        else if (curr_grad->mag >= prev_grad->mag)
        {
            curr_grad_mat = next_grad_mat;
            curr_grad_row = next_grad_row;
            curr_grad_col = next_grad_col;
            curr_grad     = next_grad;
        }
        else
        {
            curr_grad_mat = prev_grad_mat;
            curr_grad_row = prev_grad_row;
            curr_grad_col = prev_grad_col;
            curr_grad     = prev_grad;
        }

        prev_grad_mat = curr_grad_mat;
        prev_grad_row = curr_grad_row;
        prev_grad_col = curr_grad_col;
        prev_grad     = follow_gradient_f(&prev_grad_mat, &prev_grad_row, 
                                          &prev_grad_col, -(curr_grad->dcol), 
                                          -(curr_grad->drow), 
                                          -(curr_grad->dmat), map, 
                                          num_mats, num_rows, num_cols);

        next_grad_mat = curr_grad_mat;
        next_grad_row = curr_grad_row;
        next_grad_col = curr_grad_col;
        next_grad     = follow_gradient_f(&next_grad_mat, &next_grad_row, 
                                          &next_grad_col, curr_grad->dcol, 
                                          curr_grad->drow, curr_grad->dmat, 
                                          map, num_mats, num_rows, num_cols);
    }

    *grad_mat_in_out = curr_grad_mat;
    *grad_row_in_out = curr_grad_row;
    *grad_col_in_out = curr_grad_col;

    return max_grad;
}


static void follow_surface_f
(
    Gradient_f*   grad,
    int32_t       grad_mat,
    int32_t       grad_row,
    int32_t       grad_col, 
    Gradient_f*** map,
    uint32_t      num_mats,
    uint32_t      num_rows,
    uint32_t      num_cols,
    float         end_thresh
)
{
    /* Vectors orthogonal to the z-axis. */
    static const float orthogonal_vectors[ 8 ][ 3 ] = {
        {1,0,0}, {INV_SQRT_2,INV_SQRT_2,0},{0,1,0}, {-INV_SQRT_2,INV_SQRT_2,0},
        {-1,0,0}, {-INV_SQRT_2,-INV_SQRT_2,0}, {0,-1,0},
        {INV_SQRT_2,-INV_SQRT_2,0} };

    uint8_t i;
    float   x, y, z;
    float   ortho_x, ortho_y, ortho_z;
    float   sin_psi, cos_psi;
    float   sin_theta, cos_theta;
    float   mag_xy;

    Gradient_f* grad_to_follow = NULL;

    /* psi used to rotate around z-axis to put vector on the yz-plane. */
    mag_xy = sqrt(grad->dcol*grad->dcol +
                  grad->drow*grad->drow);
    if (mag_xy > 1.0e-8)
    {
        cos_psi = grad->drow / mag_xy;
        sin_psi = grad->dcol / mag_xy;
    }
    else
    {
        cos_psi = 0.0;
        sin_psi = 1.0;
    }

    /* theta used to rotate around x-axis to put vector onto the z-axis. */
    cos_theta = grad->dmat;
    sin_theta = mag_xy;

    /* Rotate each orthogonal vector and follow the gradient in that 
     * direction. */
    for (i = 0; i < 8; i++)
    {
        ortho_x = orthogonal_vectors[ i ][ 0 ];
        ortho_y = orthogonal_vectors[ i ][ 1 ];
        ortho_z = orthogonal_vectors[ i ][ 2 ];

        /* Rotate the orthagonals. */
        x = cos_psi*ortho_x + cos_theta*sin_psi*ortho_y + 
            sin_theta*sin_psi*ortho_z;
        y = -sin_psi*ortho_x + cos_theta*cos_psi*ortho_y + 
            cos_psi*sin_theta*ortho_z;
        z = -sin_theta*ortho_y + cos_theta*ortho_z;
    
        follow_gradient_f(&grad_mat, &grad_row, &grad_col, x, y, z, map, 
                num_mats, num_rows, num_cols);

        grad_to_follow = get_maximal_along_gradient_f(&grad_mat, &grad_row, 
                &grad_col, map, num_mats, num_rows, num_cols);

        if (!grad_to_follow->marked && grad_to_follow->mag >= end_thresh &&
                grad_mat > 0 && grad_mat < num_mats-1 &&
                grad_row > 0 && grad_row < num_rows-1 &&
                grad_col > 0 && grad_col < num_cols-1)
        {
            grad_to_follow->marked = 1;

            follow_surface_f(grad_to_follow, grad_mat, grad_row, grad_col, 
                    map, num_mats, num_rows, num_cols, end_thresh);
        }
    }
}


static void get_surface_points_from_gradient_map_f
(
    Surface_point_f** pts_out,
    uint32_t*         num_pts_out,
    const Matblock_f* m,
    Gradient_f***     map, 
    uint32_t          num_mats,
    uint32_t          num_rows, 
    uint32_t          num_cols,
    float             begin_thresh, 
    float             end_thresh
)
{
    int32_t  grad_mat;
    int32_t  grad_row;
    int32_t  grad_col;
    uint32_t mat, row, col;
    uint32_t num_pts;

    Gradient_f*      grad;
    Gradient_f*      max_grad;
    Surface_point_f* pts;

    for (mat = 0; mat < num_mats; mat++)
    {
        for (row = 0; row < num_rows; row++)
        {
            for (col = 0; col < num_cols; col++)
            {
                grad = &map[ mat ][ row ][ col ];

                if (!grad->marked && grad->mag >= begin_thresh)
                {
                    grad_mat = mat;
                    grad_row = row;
                    grad_col = col;

                    max_grad = get_maximal_along_gradient_f(&grad_mat, 
                            &grad_row, &grad_col, map, num_mats, num_rows, 
                            num_cols);

                    if (!max_grad->marked &&
                            grad_mat > 0 && grad_mat < num_mats-1 &&
                            grad_row > 0 && grad_row < num_rows-1 &&
                            grad_col > 0 && grad_col < num_cols-1)
                    {
                        max_grad->marked = 1;

                        follow_surface_f(max_grad, grad_mat, grad_row, 
                                grad_col, map, num_mats, num_rows, num_cols, 
                                end_thresh);
                    }
                }
            }
        }
    }

    num_pts = 0;
    for (mat = 0; mat < num_mats; mat++)
    {
        for (row = 0; row < num_rows; row++)
        {
            for (col = 0; col < num_cols; col++)
            {
                if (map[ mat ][ row ][ col ].marked)
                {
                    num_pts++;
                }
            }
        }
    }

    *num_pts_out = num_pts;
    *pts_out = realloc(*pts_out, num_pts*sizeof(Surface_point_f));
    if (num_pts > 0) assert(*pts_out);
    pts = *pts_out;

    for (mat = 0; mat < num_mats; mat++)
    {
        for (row = 0; row < num_rows; row++)
        {
            for (col = 0; col < num_cols; col++)
            {
                grad = &map[ mat ][ row ][ col ];
                if (grad->marked)
                {
                    pts->mat       = mat;
                    pts->row       = row;
                    pts->col       = col;
                    pts->dcol      = grad->dcol * grad->mag;
                    pts->drow      = grad->drow * grad->mag;
                    pts->dmat      = grad->dmat * grad->mag;
                    pts->intensity = m->elts[ mat ][ row ][ col ];
                    pts++;
                }
            }
        }
    }
}




Error* detect_imgblock_surface_points_f
(
    Surface_point_f** r_pts_out,
    uint32_t*         num_r_pts_out,
    Surface_point_f** g_pts_out,
    uint32_t*         num_g_pts_out,
    Surface_point_f** b_pts_out,
    uint32_t*         num_b_pts_out,
    const Imgblock_f* img,
    float             sigma, 
    float             grad_img_weight,
    float             grad_row_weight,
    float             grad_col_weight,
    float             begin_thresh, 
    float             end_thresh
)
{
    Matblock_f* m = NULL;
    Error*      e;

    create_matblock_from_imgblock_f(&m, img, 1.0, 0.0, 0.0);
    if ((e = detect_matblock_surface_points_f(r_pts_out, num_r_pts_out, m, 
                    sigma, grad_img_weight, grad_row_weight, grad_col_weight,
                    begin_thresh, end_thresh)) != NULL)
    {
        return e;
    }

    create_matblock_from_imgblock_f(&m, img, 0.0, 1.0, 0.0);
    if ((e = detect_matblock_surface_points_f(g_pts_out, num_g_pts_out, m, 
                    sigma, grad_img_weight, grad_row_weight, grad_col_weight,
                    begin_thresh, end_thresh)) != NULL)
    {
        return e;
    }

    create_matblock_from_imgblock_f(&m, img, 0.0, 0.0, 1.0);
    if ((e = detect_matblock_surface_points_f(b_pts_out, num_b_pts_out, m, 
                    sigma, grad_img_weight, grad_row_weight, grad_col_weight,
                    begin_thresh, end_thresh)) != NULL)
    {
        return e;
    }

    free_matblock_f(m);
    
    return NULL;
}

Error* detect_matblock_surface_points_f
(
    Surface_point_f** pts_out,
    uint32_t*         num_pts_out,
    const Matblock_f* m,
    float             sigma, 
    float             grad_mat_weight,
    float             grad_row_weight,
    float             grad_col_weight,
    float             begin_thresh, 
    float             end_thresh
)
{
    Gradient_f*** map = NULL;
    Error*        e;

    if ((e = create_gradient_map_f(&map, m, sigma, grad_mat_weight, 
                    grad_row_weight, grad_col_weight)) != NULL)
    {
        free(*pts_out); *pts_out = NULL;
        return e;
    }

    get_surface_points_from_gradient_map_f(pts_out, num_pts_out, m, map, 
            m->num_mats, m->num_rows, m->num_cols, begin_thresh, end_thresh);

    free(**map);
    free(*map);
    free(map);

    return NULL;
}

void sample_surface_points_f
(
    Surface_point_f**      pts_out, 
    uint32_t*              num_pts_out,
    const Surface_point_f* pts_in, 
    uint32_t               num_pts_in,
    float                  p
)
{
    uint32_t         num_pts;
    uint32_t         pt;
    Surface_point_f* pts;

    *pts_out = realloc(*pts_out, num_pts_in*sizeof(Surface_point_f));
    if (num_pts_in < 0) assert(*pts_out);

    pts     = *pts_out;
    num_pts = 0;

    for (pt = 0; pt < num_pts_in; pt++)
    {
        if (sample_bernoulli_pmf_d(p))
        {
            pts->mat       = pts_in[ pt ].mat;
            pts->row       = pts_in[ pt ].row;
            pts->col       = pts_in[ pt ].col;
            pts->dcol      = pts_in[ pt ].dcol;
            pts->drow      = pts_in[ pt ].drow;
            pts->dmat      = pts_in[ pt ].dmat;
            pts->intensity = pts_in[ pt ].intensity;
            pts++;
            num_pts++;
        }
    }

    *pts_out = realloc(*pts_out, num_pts*sizeof(Surface_point_f));
    if (num_pts > 0) assert(*pts_out);

    *num_pts_out = num_pts;
}

void color_surface_points_f
(
    Imgblock_f**           img_out,
    const Imgblock_f*      img_in,
    const Surface_point_f* pts, 
    uint32_t               num_pts,
    const Pixel_f*         pxl
)
{
    uint32_t    pt;
    Imgblock_f* img;

    copy_imgblock_f(img_out, img_in);
    img = *img_out;

    for (pt = 0; pt < num_pts; pt++)
    {
        img->pxls[ pts[ pt ].mat ][ pts[ pt ].row ][ pts[ pt ].col ] = *pxl;
    }
}

Error* read_surface_points_f
(
    Surface_point_f** pts_out,
    uint32_t*         num_pts_out,
    const char*       fname
)
{
    uint32_t num_pts;
    uint32_t p;
    FILE*    fp;

    Surface_point_f* pts;
    Surface_point_f* pt;

    if ((fp = fopen(fname, "r")) == NULL)
    {
        free(*pts_out); *pts_out = NULL;
        return JWSC_EIO(strerror(errno));
    }

    if (fscanf(fp, "num_pts=%u\n", &num_pts) != 1)
    {
        free(*pts_out); *pts_out = NULL;
        return JWSC_EARG("File not properly formatted to read surface points");
    }

    *num_pts_out = num_pts;
    *pts_out = realloc(*pts_out, num_pts*sizeof(Surface_point_f));
    if (num_pts > 0) assert(*pts_out);
    pts = *pts_out;

    for (p = 0; p < num_pts; p++)
    {
        pt = &((*pts_out)[ p ]);

        if (fscanf(fp, "col=%u row=%u mat=%u dcol=%f drow=%f dmat=%f intensity=%f\n", 
                    &(pt->col), &(pt->row), &(pt->mat),
                    &(pt->dcol), &(pt->drow), &(pt->dmat), 
                    &(pt->intensity)) != 7)
        {
            free(*pts_out); *pts_out = NULL;
            return JWSC_EARG("Surface points file not properly formatted");
        }
    }

    if (fclose(fp) != 0)
    {
        free(*pts_out); *pts_out = NULL;
        return JWSC_EIO(strerror(errno));
    }

    return NULL;
}

Error* write_surface_points_f
(
    const Surface_point_f* pts,
    uint32_t               num_pts,
    const char*            fname
)
{
    uint32_t p;
    FILE*    fp;

    const Surface_point_f* pt;

    if ((fp = fopen(fname, "w")) == NULL)
    {
        return JWSC_EIO(strerror(errno));
    }

    fprintf(fp, "num_pts=%u\n", num_pts);

    for (p = 0; p < num_pts; p++)
    {
        pt = &(pts[ p ]);

        fprintf(fp, "col=%u row=%u mat=%u", pt->col, pt->row, pt->mat);
        fprintf(fp, " dcol=%f drow=%f dmat=%f", pt->dcol, pt->drow, pt->dmat);
        fprintf(fp, " intensity=%f\n", pt->intensity);
    }

    if (fclose(fp) != 0)
    {
        return JWSC_EIO(strerror(errno));
    }

    return NULL;
}

void create_surface_patch_f
(
    Surface_patch_f**      patch_out,
    const Surface_point_f* point,
    float                  size,
    float                  x_scale,
    float                  y_scale,
    float                  z_scale
)
{
    double mag_xy;
    double cos_psi, sin_psi;
    double cos_theta, sin_theta;
    double pt_x, pt_y, pt_z;
    double rot_pt_x, rot_pt_y, rot_pt_z;

    uint32_t i;

    Surface_patch_f* patch;

    if (*patch_out == NULL)
    {
        assert(*patch_out = malloc(sizeof(Surface_patch_f)));
        for (i = 0; i < 4; i++)
        {
            (*patch_out)->pts[i] = NULL;
            create_vector_f(&((*patch_out)->pts[i]), 3);
        }
        (*patch_out)->normal = NULL;
        (*patch_out)->center = NULL;
        create_vector_f(&((*patch_out)->normal), 3);
        create_vector_f(&((*patch_out)->center), 3);
    }
    patch = *patch_out;

    patch->albedo = point->intensity;

    patch->center->elts[0] = point->col;
    patch->center->elts[1] = point->row;
    patch->center->elts[2] = point->mat;

    patch->normal->elts[0] = point->dcol;
    patch->normal->elts[1] = point->drow;
    patch->normal->elts[2] = point->dmat;
    normalize_vector_mag_f(&(patch->normal), patch->normal);

    /* Patch's starting orientation is centered at the origin on the x,y
     * plane. */
    patch->pts[0]->elts[0] = size; 
    patch->pts[0]->elts[1] = size; 
    patch->pts[0]->elts[2] = 0;

    patch->pts[1]->elts[0] = size; 
    patch->pts[1]->elts[1] =-size; 
    patch->pts[1]->elts[2] = 0;

    patch->pts[2]->elts[0] =-size; 
    patch->pts[2]->elts[1] =-size; 
    patch->pts[2]->elts[2] = 0;

    patch->pts[3]->elts[0] =-size; 
    patch->pts[3]->elts[1] = size; 
    patch->pts[3]->elts[2] = 0;

    /* Transorm the patch points according to the center and normal. */

    /* 1. The points on the patch need to be rotated by the normal vector.
     * psi used to rotate around z-axis to put points on the yz-plane. */
    mag_xy = sqrt(patch->normal->elts[0]*patch->normal->elts[0] +
                  patch->normal->elts[1]*patch->normal->elts[1]);
    if (mag_xy > 1.0e-8)
    {
        cos_psi = patch->normal->elts[1] / mag_xy;
        sin_psi = patch->normal->elts[0] / mag_xy;
    }
    else
    {
        cos_psi = 0.0;
        sin_psi = 1.0;
    }

    /* 2. Theta used to rotate around x-axis to put points onto the z-axis. */
    cos_theta = patch->normal->elts[2];
    sin_theta = mag_xy;

    /* Transform the patch points. */
    for (i = 0; i < 4; i++)
    {
        /* Rotate. */
        pt_x = patch->pts[ i ]->elts[0];
        pt_y = patch->pts[ i ]->elts[1];
        pt_z = patch->pts[ i ]->elts[2];

        rot_pt_x = cos_psi*pt_x + cos_theta*sin_psi*pt_y + 
                   sin_theta*sin_psi*pt_z;
        rot_pt_y = -sin_psi*pt_x + cos_theta*cos_psi*pt_y + 
                   cos_psi*sin_theta*pt_z;
        rot_pt_z = -sin_theta*pt_y + cos_theta*pt_z;

        patch->pts[ i ]->elts[0] = rot_pt_x;
        patch->pts[ i ]->elts[1] = rot_pt_y;
        patch->pts[ i ]->elts[2] = rot_pt_z;

        /* Translate. */
        add_vectors_f(&(patch->pts[ i ]), patch->pts[ i ], patch->center);

        /* Scale. */
        patch->pts[ i ]->elts[0] *= x_scale;
        patch->pts[ i ]->elts[1] *= y_scale;
        patch->pts[ i ]->elts[2] *= z_scale;
    }

    /* Finally, scale the position of the center point. */
    patch->center->elts[0] *= x_scale;
    patch->center->elts[1] *= y_scale;
    patch->center->elts[2] *= z_scale;
}

void free_surface_patch_f(Surface_patch_f* patch)
{
    free_vector_f(patch->pts[0]);
    free_vector_f(patch->pts[1]);
    free_vector_f(patch->pts[2]);
    free_vector_f(patch->pts[3]);
    free_vector_f(patch->normal);
    free_vector_f(patch->center);
    free(patch);
}