enzyme/benchmarks/hand/hand.cpp

// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.

/*
 *   File "hand_d_tapenade_generated.c" is generated by Tapenade 3.14 (r7259) from this file.
 *   To reproduce such a generation you can use Tapenade CLI
 *   (can be downloaded from http://www-sop.inria.fr/tropics/tapenade/downloading.html)
 *
 *   Firstly, add a type declaration of Matrix and Triangle to the content of this file (Tapenade can't process a file
 *   with unknown types). You can both take this declaration from the files "hand.h" and "<repo root>/src/cpp/shared/defs.h" or
 *   copypaste the following lines removing asterisks:
 *
 *   typedef struct
 *   {
 *       int nrows;
 *       int ncols;
 *       double* data;
 *   } Matrix;
 *   typedef struct
 *   {
 *       int verts[3];
 *   } Triangle;
 *
 *   Tapenade can't process a code with #define constants defined both by a user or by a standard library, so, you have
 *   to replace all "NULL" insertions in the file by some numeric value (e.g. 42). Note, that this value has to be unique
 *   in this file because you'll have to replace it back with "NULL" in the generated file.
 *   After Tapenade CLI installing use the next command to generate a file:
 *
 *      tapenade -o hand_tapenade -head "hand_objective(err)/(theta) hand_objective_complicated(err)/(theta us)" hand.c
 *
 *   This will produce a file "hand_tapenade_d.c" which content will be the same as the content of the file "hand_d_tapenade_generated.c",
 *   except one-line header, Matrix and Triangle type declarations (which should be removed) and NULL replaced by some value that
 *   you have chosen (you have to replace it back with "NULL"). Moreover a log-file "hand_tapenade_b.msg" will be produced.
 *
 *   NOTE: the code in "hand_d_tapenade_generated.c" is wrong and won't work.
 *         REPAIRED SOURCE IS STORED IN THE FILE "hand_d.c".
 *         You can either use diff tool or read "hand_d.c" header to figure out what changes was performed to fix the code.
 *
 *   NOTE: you can also use Tapenade web server (http://tapenade.inria.fr:8080/tapenade/index.jsp)
 *         for generating but result can be slightly different.
 */

#include "../adbench/hand.h"

extern "C" {
#include "hand.h"

/*========================================================================*/
/*                            MATRIX METHODS                              */
/*========================================================================*/
// returns a new matrix of size nrows * ncols
Matrix* get_new_matrix(int nrows, int ncols)
{
    Matrix* mat = (Matrix*)malloc(sizeof(Matrix));

    mat->nrows = nrows;
    mat->ncols = ncols;
    mat->data = (double*)malloc(nrows * ncols * sizeof(double));

    return mat;
}



// return new empty matrix
Matrix* get_new_empty_matrix()
{
    Matrix* mat = (Matrix*)malloc(sizeof(Matrix));

    mat->nrows = 0;
    mat->ncols = 0;
    mat->data = NULL;

    return mat;
}



// disposes data matrix holds
void delete_matrix(Matrix* mat)
{
    if (mat->data != NULL)
    {
        free(mat->data);
    }

    free(mat);
}



// creates array of matricies
Matrix* get_matrix_array(int count)
{
    Matrix* result = (Matrix*)malloc(count * sizeof(Matrix));

    int i;
    for (i = 0; i < count; i++)
    {
        result[i].data = NULL;
        result[i].nrows = result[i].ncols = 0;
    }

    return result;
}



// disposes array of matricies
void delete_light_matrix_array(Matrix* matricies, int count)
{
    int i;
    for (i = 0; i < count; i++)
    {
        if (matricies[i].data != NULL)
        {
            free(matricies[i].data);
        }
    }

    free(matricies);
}



// sets a new size of a matrix
void resize(Matrix* mat, int nrows, int ncols)
{
    if (mat->nrows * mat->ncols != nrows * ncols)
    {
        if (mat->data != NULL)
        {
            free(mat->data);
        }

        if (nrows * ncols > 0)
        {
            mat->data = (double*)malloc(ncols * nrows * sizeof(double));
        }
        else
        {
            mat->data = NULL;
        }
    }

    mat->ncols = ncols;
    mat->nrows = nrows;
}



// multiplies matricies
void mat_mult(const Matrix* __restrict lhs, const Matrix* __restrict rhs, Matrix* __restrict out)
{
    int i, j, k;
    resize(out, lhs->nrows, rhs->ncols);
    for (i = 0; i < lhs->nrows; i++)
    {
        for (k = 0; k < rhs->ncols; k++)
        {
            out->data[i + k * out->nrows] = lhs->data[i + 0 * lhs->nrows] * rhs->data[0 + k * rhs->nrows];
            for (j = 1; j < lhs->ncols; j++)
            {
                out->data[i + k * out->nrows] += lhs->data[i + j * lhs->nrows] * rhs->data[j + k * rhs->nrows];
            }
        }
    }
}



// set matrix identity
void set_identity(Matrix* mat)
{
    int i_col, i_row;
    for (i_col = 0; i_col < mat->ncols; i_col++)
    {
        for (i_row = 0; i_row < mat->nrows; i_row++)
        {
            if (i_col == i_row)
            {
                mat->data[i_row + i_col * mat->nrows] = 1.0;
            }
            else
            {
                mat->data[i_row + i_col * mat->nrows] = 0.0;
            }
        }
    }
}



// fills matrix with the given value
void fill(Matrix* mat, double val)
{
    int i;
    for (i = 0; i < mat->ncols * mat->nrows; i++)
    {
        mat->data[i] = val;
    }
}



// set a block of the matrix with another matrix
void set_block(Matrix* mat, int row_off, int col_off, const Matrix* block)
{
    int i_col, i_row;
    for (i_col = 0; i_col < block->ncols; i_col++)
    {
        for (i_row = 0; i_row < block->nrows; i_row++)
        {
            mat->data[i_row + row_off + (i_col + col_off) * mat->nrows] = block->data[i_row + i_col * block->nrows];
        }
    }
}



// copies one matrix to another
void copy(Matrix* dst, const Matrix* src)
{
    if (dst->data != NULL)
    {
        free(dst->data);
    }

    dst->ncols = src->ncols;
    dst->nrows = src->nrows;
    dst->data = (double*)malloc(dst->ncols * dst->nrows * sizeof(double));

    int i;
    for (i = 0; i < dst->ncols * dst->nrows; i++)
    {
        dst->data[i] = src->data[i];
    }
}



/*========================================================================*/
/*                                   UTILS                                */
/*========================================================================*/
// sum of component squares
double square_sum(int n, double const* x)
{
    int i;
    double res = x[0] * x[0];
    for (i = 1; i < n; i++)
    {
        res = res + x[i] * x[i];
    }

    return res;
}



/*========================================================================*/
/*                              MAIN LOGIC                                */
/*========================================================================*/
void angle_axis_to_rotation_matrix(
    double const* angle_axis,
    Matrix* R
)
{
    double norm = sqrt(square_sum(3, angle_axis));
    if (norm < 0.0001)
    {
        set_identity(R);
        return;
    }

    double x = angle_axis[0] / norm;
    double y = angle_axis[1] / norm;
    double z = angle_axis[2] / norm;

    double s = sin(norm);
    double c = cos(norm);

    R->data[0 + 0 * R->nrows] = x * x + (1 - x * x) * c;      // first row
    R->data[0 + 1 * R->nrows] = x * y * (1 - c) - z * s;
    R->data[0 + 2 * R->nrows] = x * z * (1 - c) + y * s;

    R->data[1 + 0 * R->nrows] = x * y * (1 - c) + z * s;      // second row
    R->data[1 + 1 * R->nrows] = y * y + (1 - y * y) * c;
    R->data[1 + 2 * R->nrows] = y * z * (1 - c) - x * s;

    R->data[2 + 0 * R->nrows] = x * z * (1 - c) - y * s;      // third row
    R->data[2 + 1 * R->nrows] = z * y * (1 - c) + x * s;
    R->data[2 + 2 * R->nrows] = z * z + (1 - z * z) * c;
}



void apply_global_transform(
    const Matrix* pose_params,
    Matrix* positions
)
{
    int i, j;
    Matrix* R = get_new_matrix(3, 3);
    angle_axis_to_rotation_matrix(pose_params->data, R);

    for (i = 0; i < 3; i++)
    {
        for (j = 0; j < 3; j++)
        {
            R->data[j + i * R->nrows] *= pose_params->data[i + 1 * pose_params->nrows];
        }
    }

    Matrix* tmp = get_new_empty_matrix();
    mat_mult(R, positions, tmp);
    for (j = 0; j < positions->ncols; j++)
    {
        for (i = 0; i < positions->nrows; i++)
        {
            positions->data[i + j * positions->nrows] = tmp->data[i + j * tmp->nrows] + pose_params->data[i + 2 * pose_params->nrows];
        }
    }

    delete_matrix(R);
    delete_matrix(tmp);
}



void relatives_to_absolutes(
    int count,
    const Matrix* relatives,
    const int* parents,
    Matrix* absolutes
)
{
    int i;
    for (i = 0; i < count; i++)
    {
        if (parents[i] == -1)
        {
            copy(&absolutes[i], &relatives[i]);
        }
        else
        {
            mat_mult(&absolutes[parents[i]], &relatives[i], &absolutes[i]);
        }
    }
}



void euler_angles_to_rotation_matrix(
    double const* __restrict xzy,
    Matrix* __restrict R
)
{
    double tx = xzy[0];
    double ty = xzy[2];
    double tz = xzy[1];

    Matrix* Rx = get_new_matrix(3, 3);
    Matrix* Ry = get_new_matrix(3, 3);
    Matrix* Rz = get_new_matrix(3, 3);

    set_identity(Rx);
    Rx->data[1 + 1 * Rx->nrows] = cos(tx);
    Rx->data[2 + 1 * Rx->nrows] = sin(tx);
    Rx->data[1 + 2 * Rx->nrows] = -Rx->data[2 + 1 * Rx->nrows];
    Rx->data[2 + 2 * Rx->nrows] = Rx->data[1 + 1 * Rx->nrows];

    set_identity(Ry);
    Ry->data[0 + 0 * Ry->nrows] = cos(ty);
    Ry->data[0 + 2 * Ry->nrows] = sin(ty);
    Ry->data[2 + 0 * Ry->nrows] = -Ry->data[0 + 2 * Ry->nrows];
    Ry->data[2 + 2 * Ry->nrows] = Ry->data[0 + 0 * Ry->nrows];

    set_identity(Rz);
    Rz->data[0 + 0 * Rz->nrows] = cos(tz);
    Rz->data[1 + 0 * Rz->nrows] = sin(tz);
    Rz->data[0 + 1 * Rz->nrows] = -Rz->data[1 + 0 * Rz->nrows];
    Rz->data[1 + 1 * Rz->nrows] = Rz->data[0 + 0 * Rz->nrows];

    Matrix* tmp = get_new_empty_matrix();
    mat_mult(Rz, Ry, tmp);
    mat_mult(tmp, Rx, R);

    delete_matrix(Rx);
    delete_matrix(Ry);
    delete_matrix(Rz);
    delete_matrix(tmp);
}



void get_posed_relatives(
    int bone_count,
    const Matrix* __restrict base_relatives,
    const Matrix* __restrict pose_params,
    Matrix* __restrict relatives
)
{
    int i;
    int offset = 3;
    Matrix* tr = get_new_matrix(4, 4);
    Matrix* R = get_new_matrix(3, 3);

    for (i = 0; i < bone_count; i++)
    {
        set_identity(tr);

        euler_angles_to_rotation_matrix(pose_params->data + (i + offset) * pose_params->nrows, R);
        set_block(tr, 0, 0, R);

        mat_mult(&base_relatives[i], tr, &relatives[i]);
    }

    delete_matrix(tr);
    delete_matrix(R);
}

static inline void get_skinned_vertex_positions(
    int bone_count,
    const Matrix* __restrict base_relatives,
    const int* parents,
    const Matrix* __restrict inverse_base_absolutes,
    const Matrix* __restrict base_positions,
    const Matrix* __restrict weights,
    int is_mirrored,
    const Matrix* __restrict pose_params,
    Matrix* __restrict positions,
    int apply_global
)
{
    int i;

    Matrix* relatives = get_matrix_array(bone_count);
    Matrix* absolutes = get_matrix_array(bone_count);
    Matrix* transforms = get_matrix_array(bone_count);

    get_posed_relatives(bone_count, base_relatives, pose_params, relatives);
    relatives_to_absolutes(bone_count, relatives, parents, absolutes);

    // Get bone transforms->
    for (i = 0; i < bone_count; i++)
    {
        mat_mult(&absolutes[i], &inverse_base_absolutes[i], &transforms[i]);
    }

    // Transform vertices by necessary transforms-> + apply skinning
    resize(positions, 3, base_positions->ncols);
    fill(positions, 0.0);
    Matrix* curr_positions = get_new_matrix(4, base_positions->ncols);

    int i_bone, i_vert;
    for (i_bone = 0; i_bone < bone_count; i_bone++)
    {
        mat_mult(&transforms[i_bone], base_positions, curr_positions);
        for (i_vert = 0; i_vert < positions->ncols; i_vert++)
        {
            for (i = 0; i < 3; i++)
            {
                positions->data[i + i_vert * positions->nrows] +=
                    curr_positions->data[i + i_vert * curr_positions->nrows] *
                    weights->data[i_bone + i_vert * weights->nrows];
            }
        }
    }

    if (is_mirrored)
    {
        for (i = 0; i < positions->ncols; i++)
        {
            positions->data[0 + i * positions->nrows] *= -1;
        }
    }

    if (apply_global)
    {
        apply_global_transform(pose_params, positions);
    }

    delete_matrix(curr_positions);
    delete_light_matrix_array(relatives, bone_count);
    delete_light_matrix_array(absolutes, bone_count);
    delete_light_matrix_array(transforms, bone_count);
}



//% !!!!!!! fixed order pose_params !!!!!
//% 1) global_rotation 2) scale 3) global_translation
//% 4) wrist
//% 5) thumb1, 6)thumb2, 7) thumb3, 8) thumb4
//%       similarly: index, middle, ring, pinky
//%       end) forearm
void to_pose_params(
    int count,
    double const* __restrict theta,
    const char** __restrict bone_names,
    Matrix* __restrict pose_params
)
{
    int i;

    resize(pose_params, 3, count + 3);
    fill(pose_params, 0.0);

    for (i = 0; i < pose_params->nrows; i++)
    {
        pose_params->data[i] = theta[i];
        pose_params->data[i + 1 * pose_params->nrows] = 1.0;
        pose_params->data[i + 2 * pose_params->nrows] = theta[i + 3];
    }

    int i_theta = 6;
    int i_pose_params = 5;
    int n_fingers = 5;
    int i_finger;
    for (i_finger = 0; i_finger < n_fingers; i_finger++)
    {
        for (i = 2; i <= 4; i++)
        {
            pose_params->data[0 + i_pose_params * pose_params->nrows] = theta[i_theta];
            i_theta++;

            if (i == 2)
            {
                pose_params->data[1 + i_pose_params * pose_params->nrows] = theta[i_theta];
                i_theta++;
            }

            i_pose_params++;
        }

        i_pose_params++;
    }
}



void hand_objective(
    double const* __restrict theta,
    int bone_count,
    const char** __restrict bone_names,
    const int* __restrict parents,
    Matrix* __restrict base_relatives,
    Matrix* __restrict inverse_base_absolutes,
    Matrix* __restrict base_positions,
    Matrix* __restrict weights,
    const Triangle* __restrict triangles,
    int is_mirrored,
    int corresp_count,
    const int* __restrict correspondences,
    Matrix* points,
    double* __restrict err
)
{
    Matrix* pose_params = get_new_empty_matrix();
    to_pose_params(bone_count, theta, bone_names, pose_params);

    Matrix* vertex_positions = get_new_empty_matrix();
    get_skinned_vertex_positions(
        bone_count,
        base_relatives,
        parents,
        inverse_base_absolutes,
        base_positions,
        weights,
        is_mirrored,
        pose_params,
        vertex_positions,
        1
    );

    int i, j;
    for (i = 0; i < corresp_count; i++)
    {
        for (j = 0; j < 3; j++)
        {
            err[i * 3 + j] =
                points->data[j + i * points->nrows] -
                vertex_positions->data[j + correspondences[i] * vertex_positions->nrows];
        }
    }

    delete_matrix(pose_params);
    delete_matrix(vertex_positions);
}



void hand_objective_complicated(
    double const* theta,
    double const* us,
    int bone_count,
    const char** bone_names,
    const int* parents,
    Matrix* base_relatives,
    Matrix* inverse_base_absolutes,
    Matrix* base_positions,
    Matrix* weights,
    const Triangle* triangles,
    int is_mirrored,
    int corresp_count,
    const int* __restrict correspondences,
    Matrix* points,
    double* err
)
{
    Matrix* pose_params = get_new_empty_matrix();
    to_pose_params(bone_count, theta, bone_names, pose_params);

    Matrix* vertex_positions = get_new_empty_matrix();
    get_skinned_vertex_positions(
        bone_count,
        base_relatives,
        parents,
        inverse_base_absolutes,
        base_positions,
        weights,
        is_mirrored,
        pose_params,
        vertex_positions,
        1
    );

    int i, j;
    for (i = 0; i < corresp_count; i++)
    {
        const int* verts = triangles[correspondences[i]].verts;
        double const* u = &us[2 * i];
        for (j = 0; j < 3; j++)
        {
            double hand_point_coord =
                u[0] * vertex_positions->data[j + verts[0] * vertex_positions->nrows] +
                u[1] * vertex_positions->data[j + verts[1] * vertex_positions->nrows] +
                (1.0 - u[0] - u[1]) * vertex_positions->data[j + verts[2] * vertex_positions->nrows];

            err[i * 3 + j] = points->data[j + i * points->nrows] - hand_point_coord;
        }
    }
}

//*      tapenade -o hand_tapenade -head "hand_objective(err)/(theta) hand_objective_complicated(err)/(theta us)" hand.c

extern int enzyme_const;
extern int enzyme_dup;
extern int enzyme_dupnoneed;
void __enzyme_autodiff(...) noexcept;

// tapenade -o hand_tapenade -head "hand_objective(err)/(theta) hand_objective_complicated(err)/(theta us)" hand.c
void dhand_objective(
    double const* theta,
    double* dtheta,
    int bone_count,
    const char** bone_names,
    const int* parents,
    Matrix* base_relatives,
    Matrix* inverse_base_absolutes,
    Matrix* base_positions,
    Matrix* weights,
    const Triangle* triangles,
    int is_mirrored,
    int corresp_count,
    const int* correspondences,
    Matrix* points,
    double* err,
    double* derr
)
{
    __enzyme_autodiff(hand_objective,
        enzyme_dup, theta, dtheta,
        enzyme_const, bone_count,
        enzyme_const, bone_names,
        enzyme_const, parents,
        enzyme_const, base_relatives,
        enzyme_const, inverse_base_absolutes,
        enzyme_const, base_positions,
        enzyme_const, weights,
        enzyme_const, triangles,
        enzyme_const, is_mirrored,
        enzyme_const, corresp_count,
        enzyme_const, correspondences,
        enzyme_const, points,
        enzyme_dupnoneed, err, derr
    );
}

void dhand_objective_complicated(
    double const* theta,
    double* dtheta,
    double const* us,
    double* dus,
    int bone_count,
    const char** bone_names,
    const int* parents,
    Matrix* base_relatives,
    Matrix* inverse_base_absolutes,
    Matrix* base_positions,
    Matrix* weights,
    const Triangle* triangles,
    int is_mirrored,
    int corresp_count,
    const int* correspondences,
    Matrix* points,
    double* err,
    double* derr
) {
    __enzyme_autodiff(hand_objective_complicated,
        enzyme_dup, theta, dtheta,
        enzyme_dup, us, dus,
        enzyme_const, bone_count,
        enzyme_const, bone_names,
        enzyme_const, parents,
        enzyme_const, base_relatives,
        enzyme_const, inverse_base_absolutes,
        enzyme_const, base_positions,
        enzyme_const, weights,
        enzyme_const, triangles,
        enzyme_const, is_mirrored,
        enzyme_const, corresp_count,
        enzyme_const, correspondences,
        enzyme_const, points,
        enzyme_dupnoneed, err, derr
    );
}

}

#if 0
//! Tapenade Forward
extern "C" {

#include <adBuffer.h>

// Struct created by Tapenade for holding Matrix diff info.
typedef struct {
    double* data;
} Matrix_diff;

Matrix_diff* get_new_zero_matrix_diff(const Matrix* mat) /* TFIX */
{
    Matrix_diff* result = (Matrix_diff*)malloc(sizeof(Matrix_diff));
    result->data = (double*)malloc(mat->nrows * mat->ncols * sizeof(double));

    for (int i = 0; i < mat->nrows * mat->ncols; i++)
    {
        result->data[i] = 0.0;
    }

    return result;
}



// removes matrix diff
void delete_matrix_diff(Matrix_diff* md) /* TFIX */
{
    if (md->data != NULL)
    {
        free(md->data);
    }

    free(md);
}



// creates new matrix diff array initialized by zeros
Matrix_diff* get_new_zero_matrix_diff_array(int count, const Matrix* m_arr) /* TFIX */
{
    Matrix_diff* result = (Matrix_diff*)malloc(count * sizeof(Matrix_diff));

    int i;
    for (i = 0; i < count; i++)
    {
        if (m_arr[i].data != NULL)
        {
            result[i].data = (double*)malloc(m_arr[i].nrows * m_arr[i].ncols * sizeof(double));
        }
        else
        {
            result[i].data = NULL;
        }
    }

    return result;
}



// delete matrix diff array
void delete_matrix_diff_array(int count, Matrix_diff* md_arr) /* TFIX */
{
    int i;
    for (i = 0; i < count; i++)
    {
        if (md_arr[i].data != NULL)
        {
            free(md_arr[i].data);
        }
    }

    free(md_arr);
}





/*
  Differentiation of get_new_matrix in forward (tangent) mode:
   Plus diff mem management of: alloc(*mat).data:in-out get_new_matrix:in-out

========================================================================
                            MATRIX METHODS
========================================================================*/
// returns a new matrix of size nrows * ncols
Matrix_diff * get_new_matrix_d(int nrows, int ncols, Matrix **get_new_matrix)
{
    Matrix *mat;
    Matrix_diff *matd;
    matd = (Matrix_diff *)malloc(sizeof(Matrix_diff));
    mat = (Matrix *)malloc(sizeof(Matrix));
    mat->nrows = nrows;
    mat->ncols = ncols;
    matd->data = (double *)malloc(nrows*ncols*sizeof(double));
    mat->data = (double *)malloc(nrows*ncols*sizeof(double));
    *get_new_matrix = mat;
    return matd;
}

/*
  Differentiation of get_new_empty_matrix in forward (tangent) mode:
   Plus diff mem management of: alloc(*mat).data:in-out get_new_empty_matrix:in-out
*/
// return new empty matrix
Matrix_diff * get_new_empty_matrix_d(Matrix **get_new_empty_matrix) {
    Matrix *mat;
    Matrix_diff *matd;
    matd = (Matrix_diff *)malloc(sizeof(Matrix_diff));
    mat = (Matrix *)malloc(sizeof(Matrix));
    mat->nrows = 0;
    mat->ncols = 0;
    mat->data = NULL;
    *get_new_empty_matrix = mat;
    return matd;
}

/*
  Differentiation of delete_matrix in forward (tangent) mode:
   Plus diff mem management of: mat:out *mat.data:out
*/
// disposes data matrix holds
void delete_matrix_d0(Matrix *mat, Matrix_diff *matd) {
    if (mat->data != NULL) {
        free(matd->data);
        free(mat->data);
    }
    free(matd);
    free(mat);
}

/*
  Differentiation of delete_matrix in forward (tangent) mode:
   variations   of useful results: *(*mat.data)
   with respect to varying inputs: *(*mat.data)
   Plus diff mem management of: mat:in-out *mat.data:in-out
*/
// disposes data matrix holds
void delete_matrix_d(Matrix *mat, Matrix_diff *matd) {
    if (mat->data != NULL) {
        free(matd->data);
        free(mat->data);
    }
    free(matd);
    free(mat);
}

/*
  Differentiation of get_matrix_array in forward (tangent) mode:
   Plus diff mem management of: get_matrix_array:in-out
*/
// creates array of matricies
Matrix_diff * get_matrix_array_d(int count, Matrix **get_matrix_array) {
    Matrix *result;
    Matrix_diff *resultd;
    resultd = (Matrix_diff *)malloc(count*sizeof(Matrix_diff));
    result = (Matrix *)malloc(count*sizeof(Matrix));
    int i;
    for (i = 0; i < count; ++i) {
        result[i].data = NULL;
        result[i].ncols = 0;
        result[i].nrows = result[i].ncols;
    }
    *get_matrix_array = result;
    return resultd;
}

/*
  Differentiation of delete_light_matrix_array in forward (tangent) mode:
   variations   of useful results: *(*matricies.data)
   with respect to varying inputs: *(*matricies.data)
   Plus diff mem management of: matricies:in-out *matricies.data:out
*/
// disposes array of matricies
void delete_light_matrix_array_d(Matrix *matricies, Matrix_diff *matriciesd,
        int count) {
    int i;
    for (i = 0; i < count; ++i)
        if (matricies[i].data != NULL) {
            free(matriciesd[i].data);
            free(matricies[i].data);
        }
    free(matriciesd);
    free(matricies);
}

/*
  Differentiation of resize in forward (tangent) mode:
   Plus diff mem management of: *mat.data:in-out
*/
// sets a new size of a matrix
void resize_d0(Matrix *mat, Matrix_diff *matd, int nrows, int ncols) {
    if (mat->nrows*mat->ncols != nrows*ncols) {
        if (mat->data != NULL) {
            free(matd->data);
            free(mat->data);
        }
        if (nrows*ncols > 0) {
            matd->data = (double *)malloc(ncols*nrows*sizeof(double));
            mat->data = (double *)malloc(ncols*nrows*sizeof(double));
        } else
            mat->data = NULL;
    }
    mat->ncols = ncols;
    mat->nrows = nrows;
}

/*
  Differentiation of resize in forward (tangent) mode:
   variations   of useful results: *(*mat.data)
   with respect to varying inputs: *(*mat.data)
   Plus diff mem management of: mat:in *mat.data:in-out
*/
// sets a new size of a matrix
void resize_d(Matrix *mat, Matrix_diff *matd, int nrows, int ncols) {
    if (mat->nrows*mat->ncols != nrows*ncols) {
        if (mat->data != NULL) {
            free(matd->data);
            free(mat->data);
        }
        if (nrows*ncols > 0) {
            matd->data = (double *)malloc(ncols*nrows*sizeof(double));
            mat->data = (double *)malloc(ncols*nrows*sizeof(double));
        } else
            mat->data = NULL;
    }
    mat->ncols = ncols;
    mat->nrows = nrows;
}

/*
  Differentiation of mat_mult in forward (tangent) mode:
   variations   of useful results: alloc(*(*mat.data)) *(*out.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(*out.data)
                *(*lhs.data) *(*rhs.data)
   Plus diff mem management of: out:in *out.data:in-out lhs:in
                *lhs.data:in rhs:in *rhs.data:in
*/
// multiplies matricies
void mat_mult_d(const Matrix *lhs, const Matrix_diff *lhsd, const Matrix *rhs,
        const Matrix_diff *rhsd, Matrix *out, Matrix_diff *outd) {
    int i, j, k;
    resize_d(out, outd, lhs->nrows, rhs->ncols);
    for (i = 0; i < lhs->nrows; ++i)
        for (k = 0; k < rhs->ncols; ++k) {
            outd->data[i + k*out->nrows] = lhsd->data[i+0*lhs->nrows]*rhs->
                data[0+k*rhs->nrows] + lhs->data[i+0*lhs->nrows]*rhsd->data[0+
                k*rhs->nrows];
            out->data[i + k*out->nrows] = lhs->data[i+0*lhs->nrows]*rhs->data[
                0+k*rhs->nrows];
            for (j = 1; j < lhs->ncols; ++j) {
                outd->data[i + k*out->nrows] = outd->data[i + k*out->nrows] +
                    lhsd->data[i+j*lhs->nrows]*rhs->data[j+k*rhs->nrows] + lhs
                    ->data[i+j*lhs->nrows]*rhsd->data[j+k*rhs->nrows];
                out->data[i + k*out->nrows] += lhs->data[i+j*lhs->nrows]*rhs->
                data[j+k*rhs->nrows];
            }
        }
}

/*
  Differentiation of set_identity in forward (tangent) mode:
   variations   of useful results: *(*mat.data)
   with respect to varying inputs: *(*mat.data)
   Plus diff mem management of: mat:in *mat.data:in
*/
// set matrix identity
void set_identity_d(Matrix *mat, Matrix_diff *matd) {
    int i_col, i_row;
    for (i_col = 0; i_col < mat->ncols; ++i_col)
        for (i_row = 0; i_row < mat->nrows; ++i_row)
            if (i_col == i_row) {
                matd->data[i_row + i_col*mat->nrows] = 0.0;
                mat->data[i_row + i_col*mat->nrows] = 1.0;
            } else {
                matd->data[i_row + i_col*mat->nrows] = 0.0;
                mat->data[i_row + i_col*mat->nrows] = 0.0;
            }
}

/*
  Differentiation of fill in forward (tangent) mode:
   variations   of useful results: *(*mat.data)
   with respect to varying inputs: *(*mat.data)
   Plus diff mem management of: mat:in *mat.data:in
*/
// fills matrix with the given value
void fill_d(Matrix *mat, Matrix_diff *matd, double val) {
    int i;
    for (i = 0; i < mat->ncols*mat->nrows; ++i) {
        matd->data[i] = 0.0;
        mat->data[i] = val;
    }
}

// fills matrix with the given value
void fill_nodiff(Matrix *mat, double val) {
    int i;
    for (i = 0; i < mat->ncols*mat->nrows; ++i)
        mat->data[i] = val;
}

/*
  Differentiation of set_block in forward (tangent) mode:
   variations   of useful results: *(*mat.data)
   with respect to varying inputs: *(*block.data) *(*mat.data)
   Plus diff mem management of: block:in *block.data:in mat:in
                *mat.data:in
*/
// set a block of the matrix with another matrix
void set_block_d(Matrix *mat, Matrix_diff *matd, int row_off, int col_off,
        const Matrix *block, const Matrix_diff *blockd) {
    int i_col, i_row;
    for (i_col = 0; i_col < block->ncols; ++i_col)
        for (i_row = 0; i_row < block->nrows; ++i_row) {
            matd->data[i_row + row_off + (i_col+col_off)*mat->nrows] = blockd
                ->data[i_row + i_col*block->nrows];
            mat->data[i_row + row_off + (i_col+col_off)*mat->nrows] = block->
                data[i_row + i_col*block->nrows];
        }
}

/*
  Differentiation of copy in forward (tangent) mode:
   variations   of useful results: alloc(*(*dst.data)) *(*dst.data)
   with respect to varying inputs: alloc(*(*dst.data)) *(*src.data)
                *(*dst.data)
   Plus diff mem management of: src:in *src.data:in dst:in *dst.data:in-out
*/
// copies one matrix to another
void copy_d(Matrix *dst, Matrix_diff *dstd, const Matrix *src, const
        Matrix_diff *srcd) {
    int ii1;
    if (dst->data != NULL) {
        free(dstd->data);
        free(dst->data);
    }
    dst->ncols = src->ncols;
    dst->nrows = src->nrows;
    dstd->data = (double *)malloc(dst->ncols*dst->nrows*sizeof(double));
    for (ii1 = 0; ii1 < dst->ncols*dst->nrows; ++ii1)
        dstd->data[ii1] = 0.0;
    dst->data = (double *)malloc(dst->ncols*dst->nrows*sizeof(double));
    int i;
    for (i = 0; i < dst->ncols*dst->nrows; ++i) {
        dstd->data[i] = srcd->data[i];
        dst->data[i] = src->data[i];
    }
}

/*
  Differentiation of square_sum in forward (tangent) mode:
   variations   of useful results: square_sum
   with respect to varying inputs: *x
   Plus diff mem management of: x:in

========================================================================
                                   UTILS
========================================================================*/
// sum of component squares
double square_sum_d(int n, const double *x, const double *xd, double *
        square_sum) {
    int i;
    double res = x[0]*x[0];
    double resd = xd[0]*x[0] + x[0]*xd[0];
    for (i = 1; i < n; ++i) {
        resd = resd + xd[i]*x[i] + x[i]*xd[i];
        res = res + x[i]*x[i];
    }
    *square_sum = res;
    return resd;
}

/*
  Differentiation of angle_axis_to_rotation_matrix in forward (tangent) mode:
   variations   of useful results: *(*R.data)
   with respect to varying inputs: *angle_axis *(*R.data)
   Plus diff mem management of: angle_axis:in R:in *R.data:in

========================================================================
                              MAIN LOGIC
========================================================================*/
void angle_axis_to_rotation_matrix_d(const double *angle_axis, const double *
        angle_axisd, Matrix *R, Matrix_diff *Rd) {
    double norm;
    double normd;
    double result1;
    double result1d;
    result1d = square_sum_d(3, angle_axis, angle_axisd, &result1);
    normd = (result1 == 0.0 ? 0.0 : result1d/(2.0*sqrt(result1)));
    norm = sqrt(result1);
    if (norm < 0.0001)
        set_identity_d(R, Rd);
    else {
        double x = angle_axis[0]/norm;
        double xd = (angle_axisd[0]*norm-angle_axis[0]*normd)/(norm*norm);
        double y = angle_axis[1]/norm;
        double yd = (angle_axisd[1]*norm-angle_axis[1]*normd)/(norm*norm);
        double z = angle_axis[2]/norm;
        double zd = (angle_axisd[2]*norm-angle_axis[2]*normd)/(norm*norm);
        double s;
        double sd;
        sd = normd*cos(norm);
        s = sin(norm);
        double c;
        double cd;
        cd = -(normd*sin(norm));
        c = cos(norm);
        Rd->data[0 + 0*R->nrows] = xd*x + x*xd + (-(xd*x)-x*xd)*c + (1-x*x)*cd
        ;
        R->data[0 + 0*R->nrows] = x*x + (1-x*x)*c;
        // first row
        Rd->data[0 + 1*R->nrows] = (xd*y+x*yd)*(1-c) - x*y*cd - zd*s - z*sd;
        R->data[0 + 1*R->nrows] = x*y*(1-c) - z*s;
        Rd->data[0 + 2*R->nrows] = (xd*z+x*zd)*(1-c) - x*z*cd + yd*s + y*sd;
        R->data[0 + 2*R->nrows] = x*z*(1-c) + y*s;
        Rd->data[1 + 0*R->nrows] = (xd*y+x*yd)*(1-c) - x*y*cd + zd*s + z*sd;
        R->data[1 + 0*R->nrows] = x*y*(1-c) + z*s;
        // second row
        Rd->data[1 + 1*R->nrows] = yd*y + y*yd + (-(yd*y)-y*yd)*c + (1-y*y)*cd
        ;
        R->data[1 + 1*R->nrows] = y*y + (1-y*y)*c;
        Rd->data[1 + 2*R->nrows] = (yd*z+y*zd)*(1-c) - y*z*cd - xd*s - x*sd;
        R->data[1 + 2*R->nrows] = y*z*(1-c) - x*s;
        Rd->data[2 + 0*R->nrows] = (xd*z+x*zd)*(1-c) - x*z*cd - yd*s - y*sd;
        R->data[2 + 0*R->nrows] = x*z*(1-c) - y*s;
        // third row
        Rd->data[2 + 1*R->nrows] = (zd*y+z*yd)*(1-c) - z*y*cd + xd*s + x*sd;
        R->data[2 + 1*R->nrows] = z*y*(1-c) + x*s;
        Rd->data[2 + 2*R->nrows] = zd*z + z*zd + (-(zd*z)-z*zd)*c + (1-z*z)*cd
        ;
        R->data[2 + 2*R->nrows] = z*z + (1-z*z)*c;
    }
}

/*
  Differentiation of apply_global_transform in forward (tangent) mode:
   variations   of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*positions.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*pose_params.data)
                *(*positions.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                pose_params:in *pose_params.data:in positions:in
                *positions.data:in
*/
void apply_global_transform_d(const Matrix *pose_params, const Matrix_diff *
        pose_paramsd, Matrix *positions, Matrix_diff *positionsd) {
    int i, j;
    Matrix *R;
    Matrix_diff *Rd;
    Rd = get_new_matrix_d(3, 3, &R);
    angle_axis_to_rotation_matrix_d(pose_params->data, pose_paramsd->data, R,
                                    Rd);
    for (i = 0; i < 3; ++i)
        for (j = 0; j < 3; ++j) {
            Rd->data[j + i*R->nrows] = Rd->data[j+i*R->nrows]*pose_params->
                data[i+1*pose_params->nrows] + R->data[j+i*R->nrows]*
                pose_paramsd->data[i+1*pose_params->nrows];
            R->data[j + i*R->nrows] *= pose_params->data[i + 1*pose_params->
            nrows];
        }
    Matrix *tmp;
    Matrix_diff *tmpd;
    tmpd = get_new_empty_matrix_d(&tmp);
    mat_mult_d(R, Rd, positions, positionsd, tmp, tmpd);
    for (j = 0; j < positions->ncols; ++j)
        for (i = 0; i < positions->nrows; ++i) {
            positionsd->data[i + j*positions->nrows] = tmpd->data[i + j*tmp->
                nrows] + pose_paramsd->data[i + 2*pose_params->nrows];
            positions->data[i + j*positions->nrows] = tmp->data[i + j*tmp->
                nrows] + pose_params->data[i + 2*pose_params->nrows];
        }
    delete_matrix_d(R, Rd);
    delete_matrix_d(tmp, tmpd);
}

/*
  Differentiation of relatives_to_absolutes in forward (tangent) mode:
   variations   of useful results: alloc(*(*dst.data)) alloc(*(*mat.data))
                *(*absolutes.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(*absolutes.data)
                *(*relatives.data)
   Plus diff mem management of: absolutes:in *absolutes.data:out
                relatives:in
*/
void relatives_to_absolutes_d(int count, const Matrix *relatives, const
        Matrix_diff *relativesd, const int *parents, Matrix *absolutes,
        Matrix_diff *absolutesd) {
    int i;
    for (i = 0; i < count; ++i)
        if (parents[i] == -1)
            copy_d(&(absolutes[i]), &(absolutesd[i]), &(relatives[i]), &(
                   relativesd[i]));
        else
            mat_mult_d(&(absolutes[parents[i]]), &(absolutesd[parents[i]]), &(
                       relatives[i]), &(relativesd[i]), &(absolutes[i]), &(
                       absolutesd[i]));
}

/*
  Differentiation of euler_angles_to_rotation_matrix in forward (tangent) mode:
   variations   of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*R.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *xzy *(*R.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                xzy:in R:in *R.data:in-out
*/
void euler_angles_to_rotation_matrix_d(const double *xzy, const double *xzyd,
        Matrix *R, Matrix_diff *Rd) {
    double tx = xzy[0];
    double txd = xzyd[0];
    double ty = xzy[2];
    double tyd = xzyd[2];
    double tz = xzy[1];
    double tzd = xzyd[1];
    Matrix *Rx;
    Matrix_diff *Rxd;
    Rxd = get_new_matrix_d(3, 3, &Rx);
    Matrix *Ry;
    Matrix_diff *Ryd;
    Ryd = get_new_matrix_d(3, 3, &Ry);
    Matrix *Rz;
    Matrix_diff *Rzd;
    Rzd = get_new_matrix_d(3, 3, &Rz);
    set_identity_d(Rx, Rxd);
    Rxd->data[1 + 1*Rx->nrows] = -(txd*sin(tx));
    Rx->data[1 + 1*Rx->nrows] = cos(tx);
    Rxd->data[2 + 1*Rx->nrows] = txd*cos(tx);
    Rx->data[2 + 1*Rx->nrows] = sin(tx);
    Rxd->data[1 + 2*Rx->nrows] = -Rxd->data[2+1*Rx->nrows];
    Rx->data[1 + 2*Rx->nrows] = -Rx->data[2+1*Rx->nrows];
    Rxd->data[2 + 2*Rx->nrows] = Rxd->data[1 + 1*Rx->nrows];
    Rx->data[2 + 2*Rx->nrows] = Rx->data[1 + 1*Rx->nrows];
    set_identity_d(Ry, Ryd);
    Ryd->data[0 + 0*Ry->nrows] = -(tyd*sin(ty));
    Ry->data[0 + 0*Ry->nrows] = cos(ty);
    Ryd->data[0 + 2*Ry->nrows] = tyd*cos(ty);
    Ry->data[0 + 2*Ry->nrows] = sin(ty);
    Ryd->data[2 + 0*Ry->nrows] = -Ryd->data[0+2*Ry->nrows];
    Ry->data[2 + 0*Ry->nrows] = -Ry->data[0+2*Ry->nrows];
    Ryd->data[2 + 2*Ry->nrows] = Ryd->data[0 + 0*Ry->nrows];
    Ry->data[2 + 2*Ry->nrows] = Ry->data[0 + 0*Ry->nrows];
    set_identity_d(Rz, Rzd);
    Rzd->data[0 + 0*Rz->nrows] = -(tzd*sin(tz));
    Rz->data[0 + 0*Rz->nrows] = cos(tz);
    Rzd->data[1 + 0*Rz->nrows] = tzd*cos(tz);
    Rz->data[1 + 0*Rz->nrows] = sin(tz);
    Rzd->data[0 + 1*Rz->nrows] = -Rzd->data[1+0*Rz->nrows];
    Rz->data[0 + 1*Rz->nrows] = -Rz->data[1+0*Rz->nrows];
    Rzd->data[1 + 1*Rz->nrows] = Rzd->data[0 + 0*Rz->nrows];
    Rz->data[1 + 1*Rz->nrows] = Rz->data[0 + 0*Rz->nrows];
    Matrix *tmp;
    Matrix_diff *tmpd;
    tmpd = get_new_empty_matrix_d(&tmp);
    mat_mult_d(Rz, Rzd, Ry, Ryd, tmp, tmpd);
    mat_mult_d(tmp, tmpd, Rx, Rxd, R, Rd);
    delete_matrix_d(Rx, Rxd);
    delete_matrix_d(Ry, Ryd);
    delete_matrix_d(Rz, Rzd);
    delete_matrix_d(tmp, tmpd);
}

/*
  Differentiation of get_posed_relatives in forward (tangent) mode:
   variations   of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*relatives.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                *(*pose_params.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                pose_params:in *pose_params.data:in base_relatives:in
                relatives:in
*/
void get_posed_relatives_d(int bone_count, const Matrix *base_relatives, const
        Matrix_diff *base_relativesd, const Matrix *pose_params, const
        Matrix_diff *pose_paramsd, Matrix *relatives, Matrix_diff *relativesd)
{
    int i, j; /* TFIX */
    int offset = 3;
    Matrix *tr;
    Matrix_diff *trd;
    trd = get_new_matrix_d(4, 4, &tr);
    Matrix *R;
    Matrix_diff *Rd;
    Rd = get_new_matrix_d(3, 3, &R);
    /* TFIX */
    for (i = 0; i < bone_count; ++i) {
        set_identity_d(tr, trd);
        euler_angles_to_rotation_matrix_d(pose_params->data + (i+offset)*
                                          pose_params->nrows, pose_paramsd->
                                          data + (i+offset)*pose_params->nrows
                                          , R, Rd);
        set_block_d(tr, trd, 0, 0, R, Rd);
        for (j = 0; j < base_relatives->nrows * base_relatives->ncols; j++) /* TFIX */
            base_relativesd[i].data[j] = 0.0;
        mat_mult_d(&(base_relatives[i]), &(base_relativesd[i]), tr, trd, &(
                   relatives[i]), &(relativesd[i]));
    }
    delete_matrix_d(tr, trd);
    delete_matrix_d(R, Rd);
}

/*
  Differentiation of get_skinned_vertex_positions in forward (tangent) mode:
   variations   of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                *(*positions.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                *(*pose_params.data) *(*positions.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                pose_params:in *pose_params.data:in base_positions:in
                *base_positions.data:in positions:in *positions.data:in-out
                inverse_base_absolutes:in
*/
void get_skinned_vertex_positions_d(int bone_count, const Matrix *
        base_relatives, const int *parents, const Matrix *
        inverse_base_absolutes, const Matrix_diff *inverse_base_absolutesd,
        const Matrix *base_positions, const Matrix_diff *base_positionsd,
        const Matrix *weights, int is_mirrored, const Matrix *pose_params,
        const Matrix_diff *pose_paramsd, Matrix *positions, Matrix_diff *
        positionsd, int apply_global) {
    int i, j; /* TFIX */
    Matrix *relatives;
    Matrix_diff *relativesd;
    Matrix_diff* base_relativesd = get_new_zero_matrix_diff_array(bone_count, base_relatives); /* TFIX */
    relativesd = get_matrix_array_d(bone_count, &relatives);
    Matrix *absolutes;
    Matrix_diff *absolutesd;
    absolutesd = get_matrix_array_d(bone_count, &absolutes);
    Matrix *transforms;
    Matrix_diff *transformsd;
    transformsd = get_matrix_array_d(bone_count, &transforms);
    get_posed_relatives_d(bone_count, base_relatives, base_relativesd,
                          pose_params, pose_paramsd, relatives, relativesd);
    relatives_to_absolutes_d(bone_count, relatives, relativesd, parents,
                             absolutes, absolutesd);
    // Get bone transforms->
    for (i = 0; i < bone_count; ++i) {
        for (j = 0; j < inverse_base_absolutes[i].nrows * inverse_base_absolutes[i].ncols; j++) /* TFIX */
            inverse_base_absolutesd[i].data[j] = 0.0;
        mat_mult_d(&(absolutes[i]), &(absolutesd[i]), &(inverse_base_absolutes
                   [i]), &(inverse_base_absolutesd[i]), &(transforms[i]), &(
                   transformsd[i]));
    }
    // Transform vertices by necessary transforms-> + apply skinning
    resize_d(positions, positionsd, 3, base_positions->ncols);
    fill_d(positions, positionsd, 0.0);
    Matrix *curr_positions;
    Matrix_diff *curr_positionsd;
    curr_positionsd = get_new_matrix_d(4, base_positions->ncols, &
                                       curr_positions);
    int i_bone, i_vert;
    for (i_bone = 0; i_bone < bone_count; ++i_bone) {
        for (i = 0; i < base_positions->nrows * base_positions->ncols; i++) /* TFIX */
            base_positionsd->data[i] = 0.0;
        mat_mult_d(&(transforms[i_bone]), &(transformsd[i_bone]),
                   base_positions, base_positionsd, curr_positions,
                   curr_positionsd);
        for (i_vert = 0; i_vert < positions->ncols; ++i_vert)
            for (i = 0; i < 3; ++i) {
                positionsd->data[i + i_vert*positions->nrows] = positionsd->
                    data[i + i_vert*positions->nrows] + weights->data[i_bone+
                    i_vert*weights->nrows]*curr_positionsd->data[i+i_vert*
                    curr_positions->nrows];
                positions->data[i + i_vert*positions->nrows] += curr_positions
                ->data[i+i_vert*curr_positions->nrows]*weights->data[i_bone+
                i_vert*weights->nrows];
            }
    }
    if (is_mirrored)
        for (i = 0; i < positions->ncols; ++i) {
            positionsd->data[0 + i*positions->nrows] = -positionsd->data[0+i*
                positions->nrows];
            positions->data[0 + i*positions->nrows] *= -1;
        }
    if (apply_global)
        apply_global_transform_d(pose_params, pose_paramsd, positions,
                                 positionsd);
    delete_matrix_d(curr_positions, curr_positionsd);
    delete_light_matrix_array_d(relatives, relativesd, bone_count);
    delete_light_matrix_array_d(absolutes, absolutesd, bone_count);
    delete_light_matrix_array_d(transforms, transformsd, bone_count);
    delete_matrix_diff_array(bone_count, base_relativesd); /* TFIX */ // Added to dispose memory allocated in repaired code
}

/*
  Differentiation of to_pose_params in forward (tangent) mode:
   variations   of useful results: alloc(*(*mat.data)) *(*pose_params.data)
   with respect to varying inputs: *theta
   Plus diff mem management of: pose_params:in *pose_params.data:in-out
                theta:in
*/
//% !!!!!!! fixed order pose_params !!!!!
//% 1) global_rotation 2) scale 3) global_translation
//% 4) wrist
//% 5) thumb1, 6)thumb2, 7) thumb3, 8) thumb4
//%       similarly: index, middle, ring, pinky
//%       end) forearm
void to_pose_params_d(int count, const double *theta, const double *thetad,
        const char **bone_names, Matrix *pose_params, Matrix_diff *
        pose_paramsd) {
    int i;
    resize_d0(pose_params, pose_paramsd, 3, count + 3);
    fill_d(pose_params, pose_paramsd, 0.0); /* TFIX */
    for (i = 0; i < pose_params->nrows; ++i) {
        pose_paramsd->data[i] = thetad[i];
        pose_params->data[i] = theta[i];
        pose_paramsd->data[i + 1*pose_params->nrows] = 0.0;
        pose_params->data[i + 1*pose_params->nrows] = 1.0;
        pose_paramsd->data[i + 2*pose_params->nrows] = thetad[i + 3];
        pose_params->data[i + 2*pose_params->nrows] = theta[i + 3];
    }
    int i_theta = 6;
    int i_pose_params = 5;
    int n_fingers = 5;
    int i_finger;
    for (i_finger = 0; i_finger < n_fingers; ++i_finger) {
        for (i = 2; i < 5; ++i) {
            pose_paramsd->data[0 + i_pose_params*pose_params->nrows] = thetad[
                i_theta];
            pose_params->data[0 + i_pose_params*pose_params->nrows] = theta[
                i_theta];
            i_theta++;
            if (i == 2) {
                pose_paramsd->data[1 + i_pose_params*pose_params->nrows] =
                    thetad[i_theta];
                pose_params->data[1 + i_pose_params*pose_params->nrows] =
                    theta[i_theta];
                i_theta++;
            }
            i_pose_params++;
        }
        i_pose_params++;
    }
}

/*
  Differentiation of hand_objective in forward (tangent) mode:
   variations   of useful results: *err
   with respect to varying inputs: *theta
   RW status of diff variables: *err:out *theta:in
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                err:in base_positions:in *base_positions.data:in
                theta:in
*/
void hand_objective_d(const double *theta, double *thetad, int
        bone_count, const char **bone_names, const int *parents, Matrix *
        base_relatives, Matrix *inverse_base_absolutes, Matrix *base_positions
        , Matrix *weights, const Triangle * /* TFIX */
        triangles, int is_mirrored, int corresp_count, const int *
        correspondences, Matrix *points, double *err, double *errd) {
    Matrix_diff* base_positionsd = get_new_zero_matrix_diff(base_positions); /* TFIX */ //  Added instead of function parameter
    Matrix *pose_params;
    Matrix_diff *pose_paramsd;
    Matrix_diff* inverse_base_absolutesd = get_new_zero_matrix_diff_array(bone_count, inverse_base_absolutes); /* TFIX */
    pose_paramsd = get_new_empty_matrix_d(&pose_params);
    to_pose_params_d(bone_count, theta, thetad, bone_names, pose_params,
                     pose_paramsd);
    Matrix *vertex_positions;
    Matrix_diff *vertex_positionsd;
    vertex_positionsd = get_new_empty_matrix_d(&vertex_positions);
    get_skinned_vertex_positions_d(bone_count, base_relatives, parents,
                                   inverse_base_absolutes,
                                   inverse_base_absolutesd, base_positions,
                                   base_positionsd, weights, is_mirrored,
                                   pose_params, pose_paramsd, vertex_positions
                                   , vertex_positionsd, 1);
    int i, j;
    *errd = 0.0;
    for (i = 0; i < corresp_count; ++i)
        for (j = 0; j < 3; ++j) {
            errd[i*3 + j] = -vertex_positionsd->data[j+correspondences[i]*
                vertex_positions->nrows];
            err[i*3 + j] = points->data[j + i*points->nrows] -
                vertex_positions->data[j + correspondences[i]*vertex_positions
                ->nrows];
        }
    delete_matrix_d0(pose_params, pose_paramsd);
    delete_matrix_d0(vertex_positions, vertex_positionsd);
    delete_matrix_diff(base_positionsd); /* TFIX */ // Added to dispose memory allocated in repaired code
    delete_matrix_diff_array(bone_count, inverse_base_absolutesd);
}

/*
  Differentiation of hand_objective_complicated in forward (tangent) mode:
   variations   of useful results: *err
   with respect to varying inputs: *us *theta
   RW status of diff variables: *err:out *us:in *theta:in
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                err:in us:in base_positions:in *base_positions.data:in
                theta:in
*/
void hand_objective_complicated_d(const double *theta, double *thetad,
        const double *us, double *usd, int bone_count, const char **
        bone_names, const int *parents, Matrix *base_relatives, Matrix *
        inverse_base_absolutes, Matrix *base_positions, /* TFIX */
        Matrix *weights, const Triangle *triangles, int
        is_mirrored, int corresp_count, const int *correspondences, Matrix *
        points, double *err, double *errd) {
    Matrix_diff* base_positionsd = get_new_zero_matrix_diff(base_positions); /* TFIX */ // Added instead of function parameter
    Matrix *pose_params;
    Matrix_diff *pose_paramsd;
    Matrix_diff* inverse_base_absolutesd = get_new_zero_matrix_diff_array(bone_count, inverse_base_absolutes); /* TFIX */
    pose_paramsd = get_new_empty_matrix_d(&pose_params);
    to_pose_params_d(bone_count, theta, thetad, bone_names, pose_params,
                     pose_paramsd);
    Matrix *vertex_positions;
    Matrix_diff *vertex_positionsd;
    vertex_positionsd = get_new_empty_matrix_d(&vertex_positions);
    get_skinned_vertex_positions_d(bone_count, base_relatives, parents,
                                   inverse_base_absolutes,
                                   inverse_base_absolutesd, base_positions,
                                   base_positionsd, weights, is_mirrored,
                                   pose_params, pose_paramsd, vertex_positions
                                   , vertex_positionsd, 1);
    int i, j;
    *errd = 0.0;
    for (i = 0; i < corresp_count; ++i) {
        const int *verts = triangles[correspondences[i]].verts;
        double *u = const_cast<double*>(&(us[2*i]));
        double *ud = &(usd[2*i]);
        for (j = 0; j < 3; ++j) {
            double hand_point_coord = u[0]*vertex_positions->data[j+verts[0]*
            vertex_positions->nrows] + u[1]*vertex_positions->data[j+verts[1]*
            vertex_positions->nrows] + (1.0-u[0]-u[1])*vertex_positions->data[
            j+verts[2]*vertex_positions->nrows];
            double hand_point_coordd = ud[0]*vertex_positions->data[j+verts[0]
            *vertex_positions->nrows] + u[0]*vertex_positionsd->data[j+verts[0
            ]*vertex_positions->nrows] + ud[1]*vertex_positions->data[j+verts[
            1]*vertex_positions->nrows] + u[1]*vertex_positionsd->data[j+verts
            [1]*vertex_positions->nrows] + (-ud[0]-ud[1])*vertex_positions->
            data[j+verts[2]*vertex_positions->nrows] + (1.0-u[0]-u[1])*
            vertex_positionsd->data[j+verts[2]*vertex_positions->nrows];
            errd[i*3 + j] = -hand_point_coordd;
            err[i*3 + j] = points->data[j + i*points->nrows] -
                hand_point_coord;
        }
    }

    delete_matrix_diff(base_positionsd); /* TFIX */ // Added to dispose memory allocated in repaired code
    delete_matrix_diff_array(bone_count, inverse_base_absolutesd);
}

}
#else

extern "C" {
    #include <adBuffer.h>


// Struct created by Tapenade for holding Matrix diff info.
typedef struct {
    double* data;
} Matrix_diff;
/*
  Differentiation of get_new_matrix in reverse (adjoint) mode:
   Plus diff mem management of: alloc(*mat).data:in-out get_new_matrix:in-out

========================================================================
                            MATRIX METHODS
========================================================================*/
// returns a new matrix of size nrows * ncols
void get_new_matrix_b(int nrows, int ncols, Matrix_diff *get_new_matrixb) {
    Matrix *mat;
    Matrix_diff *matb;
    int ii1;
    Matrix *get_new_matrix;
    matb = (Matrix_diff *)malloc(sizeof(Matrix_diff));
    matb->data = NULL;
    mat = (Matrix *)malloc(sizeof(Matrix));
    pushPointer8(matb->data);
    matb->data = (double *)malloc(nrows*ncols*sizeof(double));
    for (ii1 = 0; ii1 < nrows*ncols; ++ii1)
        matb->data[ii1] = 0.0;
    pushPointer8(mat->data);
    mat->data = (double *)malloc(nrows*ncols*sizeof(double));
    get_new_matrixb = matb;
    free(mat->data);
    popPointer8((void **)&(mat->data));
    free(matb->data);
    popPointer8((void **)&(matb->data));
    free(mat);
    free(matb);
}

/*========================================================================
                            MATRIX METHODS
========================================================================*/
// returns a new matrix of size nrows * ncols
Matrix * get_new_matrix_c(int nrows, int ncols) {
    Matrix *mat;
    mat = (Matrix *)malloc(sizeof(Matrix));
    mat->nrows = nrows;
    mat->ncols = ncols;
    mat->data = (double *)malloc(nrows*ncols*sizeof(double));
    return mat;
}

/*
  Differentiation of get_new_empty_matrix in reverse (adjoint) mode:
   Plus diff mem management of: alloc(*mat).data:in-out get_new_empty_matrix:in-out
*/
// return new empty matrix
void get_new_empty_matrix_b(Matrix_diff *get_new_empty_matrixb) {
    Matrix *mat;
    Matrix_diff *matb;
    Matrix *get_new_empty_matrix;
    matb = (Matrix_diff *)malloc(sizeof(Matrix_diff));
    matb->data = NULL;
    mat = (Matrix *)malloc(sizeof(Matrix));
    matb->data = (double *)0;
    get_new_empty_matrixb = matb;
    free(mat);
    free(matb);
}

// return new empty matrix
Matrix * get_new_empty_matrix_c() {
    Matrix *mat;
    mat = (Matrix *)malloc(sizeof(Matrix));
    mat->nrows = 0;
    mat->ncols = 0;
    mat->data = (double *)0;
    return mat;
}

/*
  Differentiation of delete_matrix in reverse (adjoint) mode:
   Plus diff mem management of: mat:out *mat.data:in-out
*/
// disposes data matrix holds
void delete_matrix_b(Matrix *mat, Matrix_diff *matb) {}

// disposes data matrix holds
void delete_matrix_c(Matrix *mat) {
    if (mat->data != (double *)0)
        free(mat->data);
    free(mat);
}

/*
  Differentiation of get_matrix_array in reverse (adjoint) mode:
   Plus diff mem management of: get_matrix_array:in-out
*/
// creates array of matricies
void get_matrix_array_b(int count, Matrix_diff *get_matrix_arrayb) {
    Matrix *result;
    Matrix_diff *resultb;
    int ii1;
    Matrix *get_matrix_array;
    resultb = (Matrix_diff *)malloc(count*sizeof(Matrix_diff));
    for (ii1 = 0; ii1 < count; ++ii1)
        resultb[ii1].data = NULL;
    result = (Matrix *)malloc(count*sizeof(Matrix));
    int i;
  label100:
    ;
    get_matrix_arrayb = resultb;
    free(result);
    free(resultb);
}

// creates array of matricies
Matrix * get_matrix_array_c(int count) {
    Matrix *result;
    result = (Matrix *)malloc(count*sizeof(Matrix));
    int i;
    for (i = 0; i < count; ++i) {
        result[i].data = (double *)0;
        result[i].ncols = 0;
        result[i].nrows = result[i].ncols;
    }
    return result;
}

/*
  Differentiation of delete_light_matrix_array in reverse (adjoint) mode:
   Plus diff mem management of: matricies:out *matricies.data:out
*/
// disposes array of matricies
void delete_light_matrix_array_b(Matrix *matricies, Matrix_diff *matriciesb,
        int count) {
    int i;
    pushPointer8(matriciesb);
    pushPointer8(matricies);
    popPointer8((void **)&matricies);
    popPointer8((void **)&matriciesb);
}

// disposes array of matricies
void delete_light_matrix_array_c(Matrix *matricies, int count) {
    int i;
    for (i = 0; i < count; ++i)
        if (matricies[i].data != (void *)0)
            free(matricies[i].data);
    free(matricies);
}

/*
  Differentiation of resize in reverse (adjoint) mode:
   Plus diff mem management of: *mat.data:in-out
*/
// sets a new size of a matrix
void resize_b(Matrix *mat, Matrix_diff *matb, int nrows, int ncols) {
    int chunklength;
    void *diffchunkold;
    void *chunkold;
    int ii1;
    int branch;
    if (mat->nrows*mat->ncols != nrows*ncols) {
        if (mat->data != (void *)0) {
            pushPointer8(matb->data);
            free(matb->data);
            pushPointer8(mat->data);
            free(mat->data);
            pushInteger4(chunklength);
            pushControl1b(0);
        } else
            pushControl1b(1);
        if (nrows*ncols > 0) {
            pushPointer8(matb->data);
            matb->data = (double *)malloc(ncols*nrows*sizeof(double));
            for (ii1 = 0; ii1 < ncols*nrows; ++ii1)
                matb->data[ii1] = 0.0;
            pushPointer8(mat->data);
            mat->data = (double *)malloc(ncols*nrows*sizeof(double));
            free(mat->data);
            popPointer8((void **)&(mat->data));
            free(matb->data);
            popPointer8((void **)&(matb->data));
        } else {
            pushPointer8(matb->data);
            matb->data = (double *)0;
            popPointer8((void **)&(matb->data));
        }
        popControl1b(&branch);
        if (branch == 0) {
            popInteger4(&chunklength);
            mat->data = (double *)malloc(8*chunklength);
            popPointer8((void **)&chunkold);
            matb->data = (double *)malloc(8*chunklength);
            popPointer8((void **)&diffchunkold);
        }
    }
}

// sets a new size of a matrix
void resize_c(Matrix *mat, int nrows, int ncols) {
    if (mat->nrows*mat->ncols != nrows*ncols) {
        if (mat->data != (void *)0)
            free(mat->data);
        if (nrows*ncols > 0)
            mat->data = (double *)malloc(ncols*nrows*sizeof(double));
        else
            mat->data = (double *)0;
    }
    mat->ncols = ncols;
    mat->nrows = nrows;
}

/*
  Differentiation of mat_mult in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*mat.data)) *(*out.data)
                *(*lhs.data) *(*rhs.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(*out.data)
                *(*lhs.data) *(*rhs.data)
   Plus diff mem management of: out:in *out.data:in-out lhs:in
                *lhs.data:in rhs:in *rhs.data:in
*/
// multiplies matricies
void mat_mult_b(const Matrix *lhs, Matrix_diff *lhsb, const Matrix *rhs,
        Matrix_diff *rhsb, Matrix *out, Matrix_diff *outb) {
    int i, j, k;
    pushInteger4(out->nrows);
    pushInteger4(out->ncols);
    pushPointer8(out->data);
    resize_c(out, lhs->nrows, rhs->ncols);
    for (i = 0; i < lhs->nrows; ++i)
        for (k = 0; k < rhs->ncols; ++k) {
            out->data[i + k*out->nrows] = lhs->data[i+0*lhs->nrows]*rhs->data[
                0+k*rhs->nrows];
            for (j = 1; j < lhs->ncols; ++j)
                out->data[i + k*out->nrows] = out->data[i + k*out->nrows] +
                    lhs->data[i+j*lhs->nrows]*rhs->data[j+k*rhs->nrows];
        }
    for (i = lhs->nrows-1; i > -1; --i)
        for (k = rhs->ncols-1; k > -1; --k) {
            for (j = lhs->ncols-1; j > 0; --j) {
                lhsb->data[i + j*lhs->nrows] = lhsb->data[i + j*lhs->nrows] +
                    rhs->data[j+k*rhs->nrows]*outb->data[i+k*out->nrows];
                rhsb->data[j + k*rhs->nrows] = rhsb->data[j + k*rhs->nrows] +
                    lhs->data[i+j*lhs->nrows]*outb->data[i+k*out->nrows];
            }
            lhsb->data[i + 0*lhs->nrows] = lhsb->data[i + 0*lhs->nrows] + rhs
                ->data[0+k*rhs->nrows]*outb->data[i+k*out->nrows];
            rhsb->data[0 + k*rhs->nrows] = rhsb->data[0 + k*rhs->nrows] + lhs
                ->data[i+0*lhs->nrows]*outb->data[i+k*out->nrows];
            outb->data[i + k*out->nrows] = 0.0;
        }
    popPointer8((void **)&(out->data));
    popInteger4(&(out->ncols));
    popInteger4(&(out->nrows));
    resize_b(out, outb, lhs->nrows, rhs->ncols);
}

// multiplies matricies
void mat_mult_c(const Matrix *lhs, const Matrix *rhs, Matrix *out) {
    int i, j, k;
    resize_c(out, lhs->nrows, rhs->ncols);
    for (i = 0; i < lhs->nrows; ++i)
        for (k = 0; k < rhs->ncols; ++k) {
            out->data[i + k*out->nrows] = lhs->data[i+0*lhs->nrows]*rhs->data[
                0+k*rhs->nrows];
            for (j = 1; j < lhs->ncols; ++j)
                out->data[i + k*out->nrows] += lhs->data[i+j*lhs->nrows]*rhs->
                data[j+k*rhs->nrows];
        }
}

/*
  Differentiation of set_identity in reverse (adjoint) mode:
   gradient     of useful results: *(*mat.data)
   with respect to varying inputs: *(*mat.data)
   Plus diff mem management of: mat:in *mat.data:in
*/
// set matrix identity
void set_identity_b(Matrix *mat, Matrix_diff *matb) {
    int i_col, i_row;
    int branch;
    for (i_col = 0; i_col < mat->ncols; ++i_col)
        for (i_row = 0; i_row < mat->nrows; ++i_row)
            if (i_col == i_row)
                pushControl1b(1);
            else
                pushControl1b(0);
    for (i_col = mat->ncols-1; i_col > -1; --i_col)
        for (i_row = mat->nrows-1; i_row > -1; --i_row) {
            popControl1b(&branch);
            if (branch == 0)
                matb->data[i_row + i_col*mat->nrows] = 0.0;
            else
                matb->data[i_row + i_col*mat->nrows] = 0.0;
        }
}

// set matrix identity
void set_identity_c(Matrix *mat) {
    int i_col, i_row;
    for (i_col = 0; i_col < mat->ncols; ++i_col)
        for (i_row = 0; i_row < mat->nrows; ++i_row)
            if (i_col == i_row)
                mat->data[i_row + i_col*mat->nrows] = 1.0;
            else
                mat->data[i_row + i_col*mat->nrows] = 0.0;
}

/*
  Differentiation of fill in reverse (adjoint) mode:
   gradient     of useful results: *(*mat.data)
   with respect to varying inputs: *(*mat.data)
   Plus diff mem management of: mat:in *mat.data:in
*/
// fills matrix with the given value
void fill_b(Matrix *mat, Matrix_diff *matb, double val) {
    int i;
    for (i = mat->ncols*mat->nrows-1; i > -1; --i)
        matb->data[i] = 0.0;
}

// fills matrix with the given value
void fill_c(Matrix *mat, double val) {
    int i;
    for (i = 0; i < mat->ncols*mat->nrows; ++i)
        mat->data[i] = val;
}

/*
  Differentiation of set_block in reverse (adjoint) mode:
   gradient     of useful results: *(*block.data) *(*mat.data)
   with respect to varying inputs: *(*block.data) *(*mat.data)
   Plus diff mem management of: block:in *block.data:in mat:in
                *mat.data:in
*/
// set a block of the matrix with another matrix
void set_block_b(Matrix *mat, Matrix_diff *matb, int row_off, int col_off,
        const Matrix *block, Matrix_diff *blockb) {
    int i_col, i_row;
    for (i_col = block->ncols-1; i_col > -1; --i_col)
        for (i_row = block->nrows-1; i_row > -1; --i_row) {
            blockb->data[i_row + i_col*block->nrows] = blockb->data[i_row +
                i_col*block->nrows] + matb->data[i_row + row_off + (i_col+
                col_off)*mat->nrows];
            matb->data[i_row + row_off + (i_col+col_off)*mat->nrows] = 0.0;
        }
}

// set a block of the matrix with another matrix
void set_block_c(Matrix *mat, int row_off, int col_off, const Matrix *block) {
    int i_col, i_row;
    for (i_col = 0; i_col < block->ncols; ++i_col)
        for (i_row = 0; i_row < block->nrows; ++i_row)
            mat->data[i_row + row_off + (i_col+col_off)*mat->nrows] = block->
                data[i_row + i_col*block->nrows];
}

/*
  Differentiation of copy in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*dst.data)) *(*src.data)
                *(*dst.data)
   with respect to varying inputs: alloc(*(*dst.data)) *(*src.data)
                *(*dst.data)
   Plus diff mem management of: src:in *src.data:in dst:in *dst.data:in-out
*/
// copies one matrix to another
void copy_b(Matrix *dst, Matrix_diff *dstb, const Matrix *src, Matrix_diff *
        srcb) {
    int chunklength;
    int ii1;
    void *diffchunkold;
    void *chunkold;
    int branch;
    if (dst->data != (void *)0) {
        pushPointer8(dstb->data);
        free(dstb->data);
        pushPointer8(dst->data);
        free(dst->data);
        pushInteger4(chunklength);
        pushControl1b(0);
    } else
        pushControl1b(1);
    pushInteger4(dst->ncols);
    dst->ncols = src->ncols;
    pushInteger4(dst->nrows);
    dst->nrows = src->nrows;
    dstb->data = (double *)malloc(dst->ncols*dst->nrows*sizeof(double));
    for (ii1 = 0; ii1 < dst->ncols*dst->nrows; ++ii1)
        dstb->data[ii1] = 0.0;
    pushPointer8(dst->data);
    dst->data = (double *)malloc(dst->ncols*dst->nrows*sizeof(double));
    int i;
    for (i = dst->ncols*dst->nrows-1; i > -1; --i) {
        srcb->data[i] = srcb->data[i] + dstb->data[i];
        dstb->data[i] = 0.0;
    }
    free(dst->data);
    popPointer8((void **)&(dst->data));
    free(dstb->data);
    popInteger4(&(dst->nrows));
    popInteger4(&(dst->ncols));
    popControl1b(&branch);
    if (branch == 0) {
        popInteger4(&chunklength);
        dst->data = (double *)malloc(8*chunklength);
        popPointer8((void **)&chunkold);
        dstb->data = (double *)malloc(8*chunklength);
        popPointer8((void **)&diffchunkold);
        for (ii1 = 0; ii1 < chunklength; ++ii1)
            dstb->data[ii1] = 0.0;
    }
}

// copies one matrix to another
void copy_c(Matrix *dst, const Matrix *src) {
    if (dst->data != (void *)0)
        free(dst->data);
    dst->ncols = src->ncols;
    dst->nrows = src->nrows;
    dst->data = (double *)malloc(dst->ncols*dst->nrows*sizeof(double));
    int i;
    for (i = 0; i < dst->ncols*dst->nrows; ++i)
        dst->data[i] = src->data[i];
}

/*
  Differentiation of square_sum in reverse (adjoint) mode:
   gradient     of useful results: *x square_sum
   with respect to varying inputs: *x
   Plus diff mem management of: x:in

========================================================================
                                   UTILS
========================================================================*/
// sum of component squares
void square_sum_b(int n, const double *x, double *xb, double square_sumb) {
    int i;
    double res = x[0]*x[0];
    double resb = 0.0;
    double square_sum;
    resb = square_sumb;
    for (i = n-1; i > 0; --i)
        xb[i] = xb[i] + 2*x[i]*resb;
    xb[0] = xb[0] + 2*x[0]*resb;
}

/*========================================================================
                                   UTILS
========================================================================*/
// sum of component squares
double square_sum_c(int n, const double *x) {
    int i;
    double res = x[0]*x[0];
    for (i = 1; i < n; ++i)
        res = res + x[i]*x[i];
    return res;
}

/*
  Differentiation of angle_axis_to_rotation_matrix in reverse (adjoint) mode:
   gradient     of useful results: *angle_axis *(*R.data)
   with respect to varying inputs: *angle_axis *(*R.data)
   Plus diff mem management of: angle_axis:in R:in *R.data:in

========================================================================
                              MAIN LOGIC
========================================================================*/
void angle_axis_to_rotation_matrix_b(const double *angle_axis, double *
        angle_axisb, Matrix *R, Matrix_diff *Rb) {
    double norm;
    double normb;
    double result1;
    double result1b;
    result1 = square_sum_c(3, angle_axis);
    norm = sqrt(result1);
    if (norm < 0.0001) {
        set_identity_b(R, Rb);
        normb = 0.0;
    } else {
        double x = angle_axis[0]/norm;
        double xb = 0.0;
        double y = angle_axis[1]/norm;
        double yb = 0.0;
        double z = angle_axis[2]/norm;
        double zb = 0.0;
        double s;
        double sb;
        s = sin(norm);
        double c;
        double cb;
        c = cos(norm);
        // first row
        // second row
        // third row
        zb = (2*z-2*z*c)*Rb->data[2+2*R->nrows];
        cb = (1-z*z)*Rb->data[2+2*R->nrows];
        Rb->data[2 + 2*R->nrows] = 0.0;
        zb = zb + y*(1-c)*Rb->data[2+1*R->nrows];
        yb = z*(1-c)*Rb->data[2+1*R->nrows];
        cb = cb - z*y*Rb->data[2+1*R->nrows];
        xb = s*Rb->data[2+1*R->nrows];
        sb = x*Rb->data[2+1*R->nrows];
        Rb->data[2 + 1*R->nrows] = 0.0;
        xb = xb + z*(1-c)*Rb->data[2+0*R->nrows];
        zb = zb + x*(1-c)*Rb->data[2+0*R->nrows];
        cb = cb - x*z*Rb->data[2+0*R->nrows];
        yb = yb - s*Rb->data[2+0*R->nrows];
        sb = sb - y*Rb->data[2+0*R->nrows];
        Rb->data[2 + 0*R->nrows] = 0.0;
        yb = yb + z*(1-c)*Rb->data[1+2*R->nrows];
        zb = zb + y*(1-c)*Rb->data[1+2*R->nrows];
        cb = cb - y*z*Rb->data[1+2*R->nrows];
        xb = xb - s*Rb->data[1+2*R->nrows];
        sb = sb - x*Rb->data[1+2*R->nrows];
        Rb->data[1 + 2*R->nrows] = 0.0;
        yb = yb + (2*y-2*y*c)*Rb->data[1+1*R->nrows];
        cb = cb + (1-y*y)*Rb->data[1+1*R->nrows];
        Rb->data[1 + 1*R->nrows] = 0.0;
        xb = xb + y*(1-c)*Rb->data[1+0*R->nrows];
        yb = yb + x*(1-c)*Rb->data[1+0*R->nrows];
        cb = cb - x*y*Rb->data[1+0*R->nrows];
        zb = zb + s*Rb->data[1+0*R->nrows];
        sb = sb + z*Rb->data[1+0*R->nrows];
        Rb->data[1 + 0*R->nrows] = 0.0;
        xb = xb + z*(1-c)*Rb->data[0+2*R->nrows];
        zb = zb + x*(1-c)*Rb->data[0+2*R->nrows];
        cb = cb - x*z*Rb->data[0+2*R->nrows];
        yb = yb + s*Rb->data[0+2*R->nrows];
        sb = sb + y*Rb->data[0+2*R->nrows];
        Rb->data[0 + 2*R->nrows] = 0.0;
        xb = xb + y*(1-c)*Rb->data[0+1*R->nrows];
        yb = yb + x*(1-c)*Rb->data[0+1*R->nrows];
        cb = cb - x*y*Rb->data[0+1*R->nrows];
        zb = zb - s*Rb->data[0+1*R->nrows];
        sb = sb - z*Rb->data[0+1*R->nrows];
        Rb->data[0 + 1*R->nrows] = 0.0;
        xb = xb + (2*x-2*x*c)*Rb->data[0+0*R->nrows];
        cb = cb + (1-x*x)*Rb->data[0+0*R->nrows];
        Rb->data[0 + 0*R->nrows] = 0.0;
        normb = cos(norm)*sb - sin(norm)*cb - angle_axis[2]*zb/(norm*norm) -
            angle_axis[1]*yb/(norm*norm) - angle_axis[0]*xb/(norm*norm);
        angle_axisb[2] = angle_axisb[2] + zb/norm;
        angle_axisb[1] = angle_axisb[1] + yb/norm;
        angle_axisb[0] = angle_axisb[0] + xb/norm;
    }
    result1b = (result1 == 0.0 ? 0.0 : normb/(2.0*sqrt(result1)));
    square_sum_b(3, angle_axis, angle_axisb, result1b);
}

/*========================================================================
                              MAIN LOGIC
========================================================================*/
void angle_axis_to_rotation_matrix_c(const double *angle_axis, Matrix *R) {
    double norm;
    double result1;
    result1 = square_sum_c(3, angle_axis);
    norm = sqrt(result1);
    if (norm < 0.0001) {
        set_identity_c(R);
        return;
    } else {
        double x = angle_axis[0]/norm;
        double y = angle_axis[1]/norm;
        double z = angle_axis[2]/norm;
        double s;
        s = sin(norm);
        double c;
        c = cos(norm);
        R->data[0 + 0*R->nrows] = x*x + (1-x*x)*c;
        // first row
        R->data[0 + 1*R->nrows] = x*y*(1-c) - z*s;
        R->data[0 + 2*R->nrows] = x*z*(1-c) + y*s;
        R->data[1 + 0*R->nrows] = x*y*(1-c) + z*s;
        // second row
        R->data[1 + 1*R->nrows] = y*y + (1-y*y)*c;
        R->data[1 + 2*R->nrows] = y*z*(1-c) - x*s;
        R->data[2 + 0*R->nrows] = x*z*(1-c) - y*s;
        // third row
        R->data[2 + 1*R->nrows] = z*y*(1-c) + x*s;
        R->data[2 + 2*R->nrows] = z*z + (1-z*z)*c;
    }
}

/*
  Differentiation of apply_global_transform in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*pose_params.data)
                *(*positions.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*pose_params.data)
                *(*positions.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                pose_params:in *pose_params.data:in positions:in
                *positions.data:in
*/
void apply_global_transform_b(const Matrix *pose_params, Matrix_diff *
        pose_paramsb, Matrix *positions, Matrix_diff *positionsb) {
    int i, j;
    Matrix *R;
    Matrix_diff *Rb;
    int branch;
    R = get_new_matrix_c(3, 3);
    angle_axis_to_rotation_matrix_c(pose_params->data, R);
    for (i = 0; i < 3; ++i)
        for (j = 0; j < 3; ++j) {
            pushReal8(R->data[j + i*R->nrows]);
            R->data[j + i*R->nrows] = R->data[j+i*R->nrows]*pose_params->data[
                i+1*pose_params->nrows];
        }
    Matrix *tmp;
    Matrix_diff *tmpb;
    tmp = get_new_empty_matrix_c();
    pushInteger4(tmp->nrows);
    pushInteger4(tmp->ncols);
    if (tmp->data) {
        pushReal8(*tmp->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    pushPointer8(tmp->data);
    mat_mult_c(R, positions, tmp);
    delete_matrix_b(tmp, tmpb);
    delete_matrix_b(R, Rb);
    for (j = positions->ncols-1; j > -1; --j)
        for (i = positions->nrows-1; i > -1; --i) {
            tmpb->data[i + j*tmp->nrows] = tmpb->data[i + j*tmp->nrows] +
                positionsb->data[i + j*positions->nrows];
            pose_paramsb->data[i + 2*pose_params->nrows] = pose_paramsb->data[
                i + 2*pose_params->nrows] + positionsb->data[i + j*positions->
                nrows];
            positionsb->data[i + j*positions->nrows] = 0.0;
        }
    popPointer8((void **)&(tmp->data));
    popControl1b(&branch);
    if (branch == 1)
        popReal8(tmp->data);
    popInteger4(&(tmp->ncols));
    popInteger4(&(tmp->nrows));
    mat_mult_b(R, Rb, positions, positionsb, tmp, tmpb);
    get_new_empty_matrix_b(tmpb);
    for (i = 2; i > -1; --i)
        for (j = 2; j > -1; --j) {
            popReal8(&(R->data[j + i*R->nrows]));
            pose_paramsb->data[i + 1*pose_params->nrows] = pose_paramsb->data[
                i + 1*pose_params->nrows] + R->data[j+i*R->nrows]*Rb->data[j+i
                *R->nrows];
            Rb->data[j + i*R->nrows] = pose_params->data[i+1*pose_params->
                nrows]*Rb->data[j+i*R->nrows];
        }
    angle_axis_to_rotation_matrix_b(pose_params->data, pose_paramsb->data, R,
                                    Rb);
    get_new_matrix_b(3, 3, Rb);
}

void apply_global_transform_c(const Matrix *pose_params, Matrix *positions) {
    int i, j;
    Matrix *R;
    R = get_new_matrix_c(3, 3);
    angle_axis_to_rotation_matrix_c(pose_params->data, R);
    for (i = 0; i < 3; ++i)
        for (j = 0; j < 3; ++j)
            R->data[j + i*R->nrows] *= pose_params->data[i + 1*pose_params->
            nrows];
    Matrix *tmp;
    tmp = get_new_empty_matrix_c();
    mat_mult_c(R, positions, tmp);
    for (j = 0; j < positions->ncols; ++j)
        for (i = 0; i < positions->nrows; ++i)
            positions->data[i + j*positions->nrows] = tmp->data[i + j*tmp->
                nrows] + pose_params->data[i + 2*pose_params->nrows];
    delete_matrix_c(R);
    delete_matrix_c(tmp);
}

/*
  Differentiation of relatives_to_absolutes in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*dst.data)) alloc(*(*mat.data))
                *(*absolutes.data) *(*relatives.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(*absolutes.data)
                *(*relatives.data)
   Plus diff mem management of: absolutes:in *absolutes.data:out
                relatives:in
*/
void relatives_to_absolutes_b(int count, const Matrix *relatives, Matrix_diff
        *relativesb, const int *parents, Matrix *absolutes, Matrix_diff *
        absolutesb) {
    int i;
    int branch;
    for (i = 0; i < count; ++i)
        if (parents[i] == -1) {
            pushInteger4(absolutes[i].nrows);
            pushInteger4(absolutes[i].ncols);
            pushPointer8(absolutes[i].data);
            copy_c(&(absolutes[i]), &(relatives[i]));
            pushControl1b(1);
        } else {
            pushInteger4(absolutes[i].nrows);
            pushInteger4(absolutes[i].ncols);
            pushReal8(*absolutes[i].data);
            pushPointer8(absolutes[i].data);
            pushReal8(*absolutes[parents[i]].data);
            mat_mult_c(&(absolutes[parents[i]]), &(relatives[i]), &(absolutes[
                       i]));
            pushControl1b(0);
        }
    for (i = count-1; i > -1; --i) {
        popControl1b(&branch);
        if (branch == 0) {
            popReal8(absolutes[parents[i]].data);
            popPointer8((void **)&(absolutes[i].data));
            popReal8(absolutes[i].data);
            popInteger4(&(absolutes[i].ncols));
            popInteger4(&(absolutes[i].nrows));
            mat_mult_b(&(absolutes[parents[i]]), &(absolutesb[parents[i]]), &(
                       relatives[i]), &(relativesb[i]), &(absolutes[i]), &(
                       absolutesb[i]));
        } else {
            popPointer8((void **)&(absolutes[i].data));
            popInteger4(&(absolutes[i].ncols));
            popInteger4(&(absolutes[i].nrows));
            copy_b(&(absolutes[i]), &(absolutesb[i]), &(relatives[i]), &(
                   relativesb[i]));
        }
    }
}

void relatives_to_absolutes_c(int count, const Matrix *relatives, const int *
        parents, Matrix *absolutes) {
    int i;
    for (i = 0; i < count; ++i)
        if (parents[i] == -1)
            copy_c(&(absolutes[i]), &(relatives[i]));
        else
            mat_mult_c(&(absolutes[parents[i]]), &(relatives[i]), &(absolutes[
                       i]));
}

/*
  Differentiation of euler_angles_to_rotation_matrix in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*R.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*R.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                R:in *R.data:in-out
*/
void euler_angles_to_rotation_matrix_b(const double *xzy, Matrix *R,
        Matrix_diff *Rb) {
    double tx = xzy[0];
    double ty = xzy[2];
    double tz = xzy[1];
    Matrix *Rx;
    Matrix_diff *Rxb;
    double tmp0;
    double tmpb;
    double tmp1;
    double tmpb0;
    double tmp2;
    double tmpb1;
    double tmp3;
    double tmpb2;
    double tmp4;
    double tmpb3;
    double tmp5;
    double tmpb4;
    int branch;
    Rx = get_new_matrix_c(3, 3);
    Matrix *Ry;
    Matrix_diff *Ryb;
    Ry = get_new_matrix_c(3, 3);
    Matrix *Rz;
    Matrix_diff *Rzb;
    Rz = get_new_matrix_c(3, 3);
    set_identity_c(Rx);
    Rx->data[1 + 1*Rx->nrows] = cos(tx);
    Rx->data[2 + 1*Rx->nrows] = sin(tx);
    tmp0 = -Rx->data[2+1*Rx->nrows];
    Rx->data[1 + 2*Rx->nrows] = tmp0;
    tmp1 = Rx->data[1 + 1*Rx->nrows];
    Rx->data[2 + 2*Rx->nrows] = tmp1;
    set_identity_c(Ry);
    Ry->data[0 + 0*Ry->nrows] = cos(ty);
    Ry->data[0 + 2*Ry->nrows] = sin(ty);
    tmp2 = -Ry->data[0+2*Ry->nrows];
    Ry->data[2 + 0*Ry->nrows] = tmp2;
    tmp3 = Ry->data[0 + 0*Ry->nrows];
    Ry->data[2 + 2*Ry->nrows] = tmp3;
    set_identity_c(Rz);
    Rz->data[0 + 0*Rz->nrows] = cos(tz);
    Rz->data[1 + 0*Rz->nrows] = sin(tz);
    tmp4 = -Rz->data[1+0*Rz->nrows];
    Rz->data[0 + 1*Rz->nrows] = tmp4;
    tmp5 = Rz->data[0 + 0*Rz->nrows];
    Rz->data[1 + 1*Rz->nrows] = tmp5;
    Matrix *tmp;
    Matrix_diff *tmpb5;
    tmp = get_new_empty_matrix_c();
    pushInteger4(tmp->nrows);
    pushInteger4(tmp->ncols);
    if (tmp->data) {
        pushReal8(*tmp->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    pushPointer8(tmp->data);
    mat_mult_c(Rz, Ry, tmp);
    delete_matrix_b(tmp, tmpb5);
    delete_matrix_b(Rz, Rzb);
    delete_matrix_b(Ry, Ryb);
    delete_matrix_b(Rx, Rxb);
    mat_mult_b(tmp, tmpb5, Rx, Rxb, R, Rb);
    popPointer8((void **)&(tmp->data));
    popControl1b(&branch);
    if (branch == 1)
        popReal8(tmp->data);
    popInteger4(&(tmp->ncols));
    popInteger4(&(tmp->nrows));
    mat_mult_b(Rz, Rzb, Ry, Ryb, tmp, tmpb5);
    get_new_empty_matrix_b(tmpb5);
    tmpb4 = Rzb->data[1 + 1*Rz->nrows];
    Rzb->data[1 + 1*Rz->nrows] = 0.0;
    Rzb->data[0 + 0*Rz->nrows] = Rzb->data[0 + 0*Rz->nrows] + tmpb4;
    tmpb3 = Rzb->data[0 + 1*Rz->nrows];
    Rzb->data[0 + 1*Rz->nrows] = 0.0;
    Rzb->data[1 + 0*Rz->nrows] = 0.0;
    Rzb->data[0 + 0*Rz->nrows] = 0.0;
    set_identity_b(Rz, Rzb);
    tmpb2 = Ryb->data[2 + 2*Ry->nrows];
    Ryb->data[2 + 2*Ry->nrows] = 0.0;
    Ryb->data[0 + 0*Ry->nrows] = Ryb->data[0 + 0*Ry->nrows] + tmpb2;
    tmpb1 = Ryb->data[2 + 0*Ry->nrows];
    Ryb->data[2 + 0*Ry->nrows] = 0.0;
    Ryb->data[0 + 2*Ry->nrows] = 0.0;
    Ryb->data[0 + 0*Ry->nrows] = 0.0;
    set_identity_b(Ry, Ryb);
    tmpb0 = Rxb->data[2 + 2*Rx->nrows];
    Rxb->data[2 + 2*Rx->nrows] = 0.0;
    Rxb->data[1 + 1*Rx->nrows] = Rxb->data[1 + 1*Rx->nrows] + tmpb0;
    tmpb = Rxb->data[1 + 2*Rx->nrows];
    Rxb->data[1 + 2*Rx->nrows] = 0.0;
    Rxb->data[2 + 1*Rx->nrows] = 0.0;
    Rxb->data[1 + 1*Rx->nrows] = 0.0;
    set_identity_b(Rx, Rxb);
    get_new_matrix_b(3, 3, Rzb);
    get_new_matrix_b(3, 3, Ryb);
    get_new_matrix_b(3, 3, Rxb);
}

void euler_angles_to_rotation_matrix_c(const double *xzy, Matrix *R) {
    double tx = xzy[0];
    double ty = xzy[2];
    double tz = xzy[1];
    Matrix *Rx;
    Rx = get_new_matrix_c(3, 3);
    Matrix *Ry;
    Ry = get_new_matrix_c(3, 3);
    Matrix *Rz;
    Rz = get_new_matrix_c(3, 3);
    set_identity_c(Rx);
    Rx->data[1 + 1*Rx->nrows] = cos(tx);
    Rx->data[2 + 1*Rx->nrows] = sin(tx);
    Rx->data[1 + 2*Rx->nrows] = -Rx->data[2+1*Rx->nrows];
    Rx->data[2 + 2*Rx->nrows] = Rx->data[1 + 1*Rx->nrows];
    set_identity_c(Ry);
    Ry->data[0 + 0*Ry->nrows] = cos(ty);
    Ry->data[0 + 2*Ry->nrows] = sin(ty);
    Ry->data[2 + 0*Ry->nrows] = -Ry->data[0+2*Ry->nrows];
    Ry->data[2 + 2*Ry->nrows] = Ry->data[0 + 0*Ry->nrows];
    set_identity_c(Rz);
    Rz->data[0 + 0*Rz->nrows] = cos(tz);
    Rz->data[1 + 0*Rz->nrows] = sin(tz);
    Rz->data[0 + 1*Rz->nrows] = -Rz->data[1+0*Rz->nrows];
    Rz->data[1 + 1*Rz->nrows] = Rz->data[0 + 0*Rz->nrows];
    Matrix *tmp;
    tmp = get_new_empty_matrix_c();
    mat_mult_c(Rz, Ry, tmp);
    mat_mult_c(tmp, Rx, R);
    delete_matrix_c(Rx);
    delete_matrix_c(Ry);
    delete_matrix_c(Rz);
    delete_matrix_c(tmp);
}

/*
  Differentiation of get_posed_relatives in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                alloc(*(*mat.data)) *(alloc(*mat).data) *(*relatives.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                *pose_params.data:in base_relatives:in relatives:in
*/
void get_posed_relatives_b(int bone_count, const Matrix *base_relatives,
        Matrix_diff *base_relativesb, const Matrix *pose_params, Matrix_diff *
        pose_paramsb, Matrix *relatives, Matrix_diff *relativesb) {
    int i;
    int offset = 3;
    Matrix *tr;
    Matrix_diff *trb;
    double* arg1;
    int branch;
    tr = get_new_matrix_c(4, 4);
    Matrix *R;
    Matrix_diff *Rb;
    R = get_new_matrix_c(3, 3);
    for (i = 0; i < bone_count; ++i) {
        set_identity_c(tr);
        arg1 = pose_params->data + (i+offset)*pose_params->nrows;
        if (arg1) {
            pushReal8(*arg1);
            pushControl1b(1);
        } else
            pushControl1b(0);
        pushInteger4(R->nrows);
        pushInteger4(R->ncols);
        if (R->data) {
            pushReal8(*R->data);
            pushControl1b(1);
        } else
            pushControl1b(0);
        pushPointer8(R->data);
        euler_angles_to_rotation_matrix_c(arg1, R);
        set_block_c(tr, 0, 0, R);
        pushInteger4(relatives[i].nrows);
        pushInteger4(relatives[i].ncols);
        pushReal8(*relatives[i].data);
        pushPointer8(relatives[i].data);
        if (tr->data) {
            pushReal8(*tr->data);
            pushControl1b(1);
        } else
            pushControl1b(0);
        mat_mult_c(&(base_relatives[i]), tr, &(relatives[i]));
    }
    delete_matrix_b(R, Rb);
    delete_matrix_b(tr, trb);
    for (i = bone_count-1; i > -1; --i) {
        popControl1b(&branch);
        if (branch == 1)
            popReal8(tr->data);
        popPointer8((void **)&(relatives[i].data));
        popReal8(relatives[i].data);
        popInteger4(&(relatives[i].ncols));
        popInteger4(&(relatives[i].nrows));
        *base_relativesb->data = 0.0;
        mat_mult_b(&(base_relatives[i]), &(base_relativesb[i]), tr, trb, &(
                   relatives[i]), &(relativesb[i]));
        set_block_b(tr, trb, 0, 0, R, Rb);
        arg1 = pose_params->data + (i+offset)*pose_params->nrows;
        popPointer8((void **)&(R->data));
        popControl1b(&branch);
        if (branch == 1)
            popReal8(R->data);
        popInteger4(&(R->ncols));
        popInteger4(&(R->nrows));
        euler_angles_to_rotation_matrix_b(arg1, R, Rb);
        popControl1b(&branch);
        if (branch == 1)
            popReal8(arg1);
        set_identity_b(tr, trb);
    }
    get_new_matrix_b(3, 3, Rb);
    get_new_matrix_b(4, 4, trb);
}

void get_posed_relatives_c(int bone_count, const Matrix *base_relatives, const
        Matrix *pose_params, Matrix *relatives) {
    int i;
    int offset = 3;
    Matrix *tr;
    double* arg1;
    tr = get_new_matrix_c(4, 4);
    Matrix *R;
    R = get_new_matrix_c(3, 3);
    for (i = 0; i < bone_count; ++i) {
        set_identity_c(tr);
        arg1 = pose_params->data + (i+offset)*pose_params->nrows;
        euler_angles_to_rotation_matrix_c(arg1, R);
        set_block_c(tr, 0, 0, R);
        mat_mult_c(&(base_relatives[i]), tr, &(relatives[i]));
    }
    delete_matrix_c(tr);
    delete_matrix_c(R);
}

/*
  Differentiation of get_skinned_vertex_positions in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*mat.data)) *(alloc(*mat).data)
                *(*pose_params.data) *(*positions.data)
   with respect to varying inputs: alloc(*(*mat.data)) *(alloc(*mat).data)
                *(*pose_params.data) *(*positions.data)
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                pose_params:in *pose_params.data:in base_positions:in
                *base_positions.data:in positions:in *positions.data:in-out
                inverse_base_absolutes:in
*/
void get_skinned_vertex_positions_b(int bone_count, const Matrix *
        base_relatives, const int *parents, const Matrix *
        inverse_base_absolutes, Matrix_diff *inverse_base_absolutesb, const
        Matrix *base_positions, Matrix_diff *base_positionsb, const Matrix *
        weights, int is_mirrored, const Matrix *pose_params, Matrix_diff *
        pose_paramsb, Matrix *positions, Matrix_diff *positionsb, int
        apply_global) {
    int i;
    Matrix *relatives;
    Matrix_diff *relativesb;
    int branch;
    double tmp;
    double tmpb;
    Matrix_diff *base_relativesb;
    relatives = get_matrix_array_c(bone_count);
    Matrix *absolutes;
    Matrix_diff *absolutesb;
    absolutes = get_matrix_array_c(bone_count);
    Matrix *transforms;
    Matrix_diff *transformsb;
    pushInteger4(relatives->nrows);
    pushInteger4(relatives->ncols);
    if (relatives->data) {
        pushReal8(*relatives->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    pushPointer8(relatives->data);
    get_posed_relatives_c(bone_count, base_relatives, pose_params, relatives);
    pushInteger4(absolutes->nrows);
    pushInteger4(absolutes->ncols);
    if (absolutes->data) {
        pushReal8(*absolutes->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    pushPointer8(absolutes->data);
    if (relatives->data) {
        pushReal8(*relatives->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    relatives_to_absolutes_c(bone_count, relatives, parents, absolutes);
    // Get bone transforms->
    for (i = 0; i < bone_count; ++i) {
        pushInteger4(transforms[i].nrows);
        pushInteger4(transforms[i].ncols);
        if (transforms[i].data) {
            pushReal8(*transforms[i].data);
            pushControl1b(1);
        } else
            pushControl1b(0);
        pushPointer8(transforms[i].data);
        if (absolutes[i].data) {
            pushReal8(*absolutes[i].data);
            pushControl1b(1);
        } else
            pushControl1b(0);
        mat_mult_c(&(absolutes[i]), &(inverse_base_absolutes[i]), &(transforms
                   [i]));
    }
    // Transform vertices by necessary transforms-> + apply skinning
    resize_c(positions, 3, base_positions->ncols);
    fill_c(positions, 0.0);
    Matrix *curr_positions;
    Matrix_diff *curr_positionsb;
    curr_positions = get_new_matrix_c(4, base_positions->ncols);
    int i_bone, i_vert;
    for (i_bone = 0; i_bone < bone_count; ++i_bone) {
        pushInteger4(curr_positions->nrows);
        pushInteger4(curr_positions->ncols);
        if (curr_positions->data) {
            pushReal8(*curr_positions->data);
            pushControl1b(1);
        } else
            pushControl1b(0);
        pushPointer8(curr_positions->data);
        if (transforms[i_bone].data) {
            pushReal8(*transforms[i_bone].data);
            pushControl1b(1);
        } else
            pushControl1b(0);
        mat_mult_c(&(transforms[i_bone]), base_positions, curr_positions);
        for (i_vert = 0; i_vert < positions->ncols; ++i_vert)
            for (i = 0; i < 3; ++i) {
                tmp = positions->data[i + i_vert*positions->nrows] +
                    curr_positions->data[i+i_vert*curr_positions->nrows]*
                    weights->data[i_bone+i_vert*weights->nrows];
                positions->data[i + i_vert*positions->nrows] = tmp;
            }
    }
    if (is_mirrored) {
        for (i = 0; i < positions->ncols; ++i)
            positions->data[0 + i*positions->nrows] = positions->data[0+i*
                positions->nrows]*-1;
        pushControl1b(0);
    } else
        pushControl1b(1);
    if (apply_global)
        pushControl1b(0);
    else
        pushControl1b(1);
    delete_light_matrix_array_b(transforms, transformsb, bone_count);
    delete_light_matrix_array_b(absolutes, absolutesb, bone_count);
    delete_light_matrix_array_b(relatives, relativesb, bone_count);
    delete_matrix_b(curr_positions, curr_positionsb);
    popControl1b(&branch);
    if (branch == 0)
        apply_global_transform_b(pose_params, pose_paramsb, positions,
                                 positionsb);
    popControl1b(&branch);
    if (branch == 0)
        for (i = positions->ncols-1; i > -1; --i)
            positionsb->data[0 + i*positions->nrows] = -positionsb->data[0+i*
                positions->nrows];
    for (i_bone = bone_count-1; i_bone > -1; --i_bone) {
        for (i_vert = positions->ncols-1; i_vert > -1; --i_vert)
            for (i = 2; i > -1; --i) {
                tmpb = positionsb->data[i + i_vert*positions->nrows];
                positionsb->data[i + i_vert*positions->nrows] = tmpb;
                curr_positionsb->data[i + i_vert*curr_positions->nrows] =
                    curr_positionsb->data[i + i_vert*curr_positions->nrows] +
                    weights->data[i_bone+i_vert*weights->nrows]*tmpb;
            }
        popControl1b(&branch);
        if (branch == 1)
            popReal8(transforms[i_bone].data);
        popPointer8((void **)&(curr_positions->data));
        popControl1b(&branch);
        if (branch == 1)
            popReal8(curr_positions->data);
        popInteger4(&(curr_positions->ncols));
        popInteger4(&(curr_positions->nrows));
        *base_positionsb->data = 0.0;
        mat_mult_b(&(transforms[i_bone]), &(transformsb[i_bone]),
                   base_positions, base_positionsb, curr_positions,
                   curr_positionsb);
    }
    get_new_matrix_b(4, base_positions->ncols, curr_positionsb);
    fill_b(positions, positionsb, 0.0);
    resize_b(positions, positionsb, 3, base_positions->ncols);
    for (i = bone_count-1; i > -1; --i) {
        popControl1b(&branch);
        if (branch == 1)
            popReal8(absolutes[i].data);
        popPointer8((void **)&(transforms[i].data));
        popControl1b(&branch);
        if (branch == 1)
            popReal8(transforms[i].data);
        popInteger4(&(transforms[i].ncols));
        popInteger4(&(transforms[i].nrows));
        *inverse_base_absolutesb->data = 0.0;
        mat_mult_b(&(absolutes[i]), &(absolutesb[i]), &(inverse_base_absolutes
                   [i]), &(inverse_base_absolutesb[i]), &(transforms[i]), &(
                   transformsb[i]));
    }
    popControl1b(&branch);
    if (branch == 1)
        popReal8(relatives->data);
    popPointer8((void **)&(absolutes->data));
    popControl1b(&branch);
    if (branch == 1)
        popReal8(absolutes->data);
    popInteger4(&(absolutes->ncols));
    popInteger4(&(absolutes->nrows));
    relatives_to_absolutes_b(bone_count, relatives, relativesb, parents,
                             absolutes, absolutesb);
    popPointer8((void **)&(relatives->data));
    popControl1b(&branch);
    if (branch == 1)
        popReal8(relatives->data);
    popInteger4(&(relatives->ncols));
    popInteger4(&(relatives->nrows));
    get_posed_relatives_b(bone_count, base_relatives, base_relativesb,
                          pose_params, pose_paramsb, relatives, relativesb);
    get_matrix_array_b(bone_count, transformsb);
    get_matrix_array_b(bone_count, absolutesb);
}

void get_skinned_vertex_positions_c(int bone_count, const Matrix *
        base_relatives, const int *parents, const Matrix *
        inverse_base_absolutes, const Matrix *base_positions, const Matrix *
        weights, int is_mirrored, const Matrix *pose_params, Matrix *positions
        , int apply_global) {
    int i;
    Matrix *relatives;
    relatives = get_matrix_array_c(bone_count);
    Matrix *absolutes;
    absolutes = get_matrix_array_c(bone_count);
    Matrix *transforms;
    transforms = get_matrix_array_c(bone_count);
    get_posed_relatives_c(bone_count, base_relatives, pose_params, relatives);
    relatives_to_absolutes_c(bone_count, relatives, parents, absolutes);
    // Get bone transforms->
    for (i = 0; i < bone_count; ++i)
        mat_mult_c(&(absolutes[i]), &(inverse_base_absolutes[i]), &(transforms
                   [i]));
    // Transform vertices by necessary transforms-> + apply skinning
    resize_c(positions, 3, base_positions->ncols);
    fill_c(positions, 0.0);
    Matrix *curr_positions;
    curr_positions = get_new_matrix_c(4, base_positions->ncols);
    int i_bone, i_vert;
    for (i_bone = 0; i_bone < bone_count; ++i_bone) {
        mat_mult_c(&(transforms[i_bone]), base_positions, curr_positions);
        for (i_vert = 0; i_vert < positions->ncols; ++i_vert)
            for (i = 0; i < 3; ++i)
                positions->data[i + i_vert*positions->nrows] += curr_positions
                ->data[i+i_vert*curr_positions->nrows]*weights->data[i_bone+
                i_vert*weights->nrows];
    }
    if (is_mirrored)
        for (i = 0; i < positions->ncols; ++i)
            positions->data[0 + i*positions->nrows] *= -1;
    if (apply_global)
        apply_global_transform_c(pose_params, positions);
    delete_matrix_c(curr_positions);
    delete_light_matrix_array_c(relatives, bone_count);
    delete_light_matrix_array_c(absolutes, bone_count);
    delete_light_matrix_array_c(transforms, bone_count);
}

/*
  Differentiation of to_pose_params in reverse (adjoint) mode:
   gradient     of useful results: alloc(*(*mat.data)) *(*pose_params.data)
   with respect to varying inputs: *theta
   Plus diff mem management of: pose_params:in *pose_params.data:in-out
                theta:in
*/
//% !!!!!!! fixed order pose_params !!!!!
//% 1) global_rotation 2) scale 3) global_translation
//% 4) wrist
//% 5) thumb1, 6)thumb2, 7) thumb3, 8) thumb4
//%       similarly: index, middle, ring, pinky
//%       end) forearm
void to_pose_params_b(int count, const double *theta, double *thetab, const
        char **bone_names, Matrix *pose_params, Matrix_diff *pose_paramsb) {
    int i;
    int arg1;
    int branch;
    arg1 = count + 3;
    resize_c(pose_params, 3, arg1);
    for (i = 0; i < pose_params->nrows; ++i) {
        pose_params->data[i] = theta[i];
        pose_params->data[i + 1*pose_params->nrows] = 1.0;
    }
    int i_theta = 6;
    int i_pose_params = 5;
    int n_fingers = 5;
    int i_finger;
    for (i_finger = 0; i_finger < n_fingers; ++i_finger) {
        for (i = 2; i < 5; ++i) {
            pose_params->data[0 + i_pose_params*pose_params->nrows] = theta[
                i_theta];
            i_theta++;
            if (i == 2) {
                pose_params->data[1 + i_pose_params*pose_params->nrows] =
                    theta[i_theta];
                i_theta++;
                pushControl1b(0);
            } else
                pushControl1b(1);
            i_pose_params++;
        }
        i_pose_params++;
    }
    *thetab = 0.0;
    for (i_finger = n_fingers-1; i_finger > -1; --i_finger) {
        --i_pose_params;
        for (i = 4; i > 1; --i) {
            --i_pose_params;
            popControl1b(&branch);
            if (branch == 0) {
                --i_theta;
                thetab[i_theta] = thetab[i_theta] + pose_paramsb->data[1 +
                    i_pose_params*pose_params->nrows];
                pose_paramsb->data[1 + i_pose_params*pose_params->nrows] = 0.0
                ;
            }
            --i_theta;
            thetab[i_theta] = thetab[i_theta] + pose_paramsb->data[0 +
                i_pose_params*pose_params->nrows];
            pose_paramsb->data[0 + i_pose_params*pose_params->nrows] = 0.0;
        }
    }
    for (i = pose_params->nrows-1; i > -1; --i) {
        thetab[i + 3] = thetab[i + 3] + pose_paramsb->data[i + 2*pose_params->
            nrows];
        pose_paramsb->data[i + 2*pose_params->nrows] = 0.0;
        pose_paramsb->data[i + 1*pose_params->nrows] = 0.0;
        thetab[i] = thetab[i] + pose_paramsb->data[i];
        pose_paramsb->data[i] = 0.0;
    }
    resize_b(pose_params, pose_paramsb, 3, arg1);
}

//% !!!!!!! fixed order pose_params !!!!!
//% 1) global_rotation 2) scale 3) global_translation
//% 4) wrist
//% 5) thumb1, 6)thumb2, 7) thumb3, 8) thumb4
//%       similarly: index, middle, ring, pinky
//%       end) forearm
void to_pose_params_c(int count, const double *theta, const char **bone_names,
        Matrix *pose_params) {
    int i;
    int arg1;
    arg1 = count + 3;
    resize_c(pose_params, 3, arg1);
    fill_c(pose_params, 0.0);
    for (i = 0; i < pose_params->nrows; ++i) {
        pose_params->data[i] = theta[i];
        pose_params->data[i + 1*pose_params->nrows] = 1.0;
        pose_params->data[i + 2*pose_params->nrows] = theta[i + 3];
    }
    int i_theta = 6;
    int i_pose_params = 5;
    int n_fingers = 5;
    int i_finger;
    for (i_finger = 0; i_finger < n_fingers; ++i_finger) {
        for (i = 2; i < 5; ++i) {
            pose_params->data[0 + i_pose_params*pose_params->nrows] = theta[
                i_theta];
            i_theta++;
            if (i == 2) {
                pose_params->data[1 + i_pose_params*pose_params->nrows] =
                    theta[i_theta];
                i_theta++;
            }
            i_pose_params++;
        }
        i_pose_params++;
    }
}


/*
  Differentiation of hand_objective in reverse (adjoint) mode:
   gradient     of useful results: *err
   with respect to varying inputs: *err *theta
   RW status of diff variables: *err:in-out *theta:out
   Plus diff mem management of: alloc(*mat).data:in-out alloc(*mat).data:in-out
                err:in base_positions:in *base_positions.data:in
                theta:in
*/
void hand_objective_b(const double *theta, double *thetab, int bone_count,
        const char **bone_names, const int *parents, Matrix *base_relatives,
        Matrix *inverse_base_absolutes, Matrix *base_positions, Matrix *weights, const Triangle *triangles, int
        is_mirrored, int corresp_count, const int *correspondences, Matrix *
        points, double *err, double *errb) {
    Matrix_diff *base_positionsb;
    Matrix *pose_params;
    Matrix_diff *pose_paramsb;
    int branch;
    Matrix_diff *inverse_base_absolutesb;
    pose_params = get_new_empty_matrix_c();
    pushInteger4(pose_params->nrows);
    pushInteger4(pose_params->ncols);
    pushPointer8(pose_params->data);
    to_pose_params_c(bone_count, theta, bone_names, pose_params);
    Matrix *vertex_positions;
    Matrix_diff *vertex_positionsb;
    vertex_positions = get_new_empty_matrix_c();
    pushInteger4(vertex_positions->nrows);
    pushInteger4(vertex_positions->ncols);
    if (vertex_positions->data) {
        pushReal8(*vertex_positions->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    pushPointer8(vertex_positions->data);
    pushInteger4(pose_params->nrows);
    pushInteger4(pose_params->ncols);
    if (pose_params->data) {
        pushReal8(*pose_params->data);
        pushControl1b(1);
    } else
        pushControl1b(0);
    pushPointer8(pose_params->data);
    get_skinned_vertex_positions_c(bone_count, base_relatives, parents,
                                   inverse_base_absolutes, base_positions,
                                   weights, is_mirrored, pose_params,
                                   vertex_positions, 1);
    int i, j;
    delete_matrix_b(vertex_positions, vertex_positionsb);
    delete_matrix_b(pose_params, pose_paramsb);
    for (i = corresp_count-1; i > -1; --i)
        for (j = 2; j > -1; --j) {
            vertex_positionsb->data[j + correspondences[i]*vertex_positions->
            nrows] = vertex_positionsb->data[j + correspondences[i]*
                vertex_positions->nrows] - errb[i*3 + j];
            errb[i*3 + j] = 0.0;
        }
    popPointer8((void **)&(pose_params->data));
    popControl1b(&branch);
    if (branch == 1)
        popReal8(pose_params->data);
    popInteger4(&(pose_params->ncols));
    popInteger4(&(pose_params->nrows));
    popPointer8((void **)&(vertex_positions->data));
    popControl1b(&branch);
    if (branch == 1)
        popReal8(vertex_positions->data);
    popInteger4(&(vertex_positions->ncols));
    popInteger4(&(vertex_positions->nrows));
    get_skinned_vertex_positions_b(bone_count, base_relatives, parents,
                                   inverse_base_absolutes,
                                   inverse_base_absolutesb, base_positions,
                                   base_positionsb, weights, is_mirrored,
                                   pose_params, pose_paramsb, vertex_positions
                                   , vertex_positionsb, 1);
    get_new_empty_matrix_b(vertex_positionsb);
    popPointer8((void **)&(pose_params->data));
    popInteger4(&(pose_params->ncols));
    popInteger4(&(pose_params->nrows));
    to_pose_params_b(bone_count, theta, thetab, bone_names, pose_params,
                     pose_paramsb);
    get_new_empty_matrix_b(pose_paramsb);
}


}

#endif