matthiasm@8: /*
matthiasm@8:  *  dpcore.c
matthiasm@8:  *  Core of dynamic programming/DTW calculation
matthiasm@8:  * 2003-04-02 dpwe@ee.columbia.edu
matthiasm@8:  * $Header: /Users/dpwe/projects/dtw/RCS/dpcore.c,v 1.3 2006/01/18 20:05:51 dpwe Exp $
matthiasm@8: % Copyright (c) 2003-05 Dan Ellis <dpwe@ee.columbia.edu>
matthiasm@8: % released under GPL - see file COPYRIGHT
matthiasm@8:  */
matthiasm@8:  
matthiasm@8: #include    <stdio.h>
matthiasm@8: #include    <math.h>
matthiasm@8: #include    <ctype.h>
matthiasm@8: #include    "mex.h"
matthiasm@8: 
matthiasm@8: /* #define DEBUG */
matthiasm@8: 
matthiasm@8: #define INF HUGE_VAL
matthiasm@8: 
matthiasm@8: void
matthiasm@8: mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
matthiasm@8: {
matthiasm@8:     int 	i,j;
matthiasm@8:     long   	pvl, pvb[16];
matthiasm@8: 
matthiasm@8: #ifdef DEBUG
matthiasm@8:     mexPrintf("dpcore: Got %d lhs args and %d rhs args.\n", 
matthiasm@8: 	      nlhs, nrhs); 
matthiasm@8:     for (i=0;i<nrhs;i++) {
matthiasm@8: 	mexPrintf("RHArg #%d is size %d x %d\n", 
matthiasm@8: 		  (long)i, mxGetM(prhs[i]), mxGetN(prhs[i]));
matthiasm@8:     }
matthiasm@8:     for (i=0;i<nlhs;i++)
matthiasm@8: 	if (plhs[i]) {
matthiasm@8: 	    mexPrintf("LHArg #%d is size %d x %d\n", 
matthiasm@8: 		      (long)i, mxGetM(plhs[i]), mxGetN(plhs[i]));
matthiasm@8: 	}
matthiasm@8: #endif /* DEBUG */
matthiasm@8: 
matthiasm@8:     if (nrhs < 1){
matthiasm@8: 	mexPrintf("dpcore  [D,P] = dpcore(S[,C])  dynamic programming core\n");
matthiasm@8: 	mexPrintf("           Calculate the best cost to every point in score\n");
matthiasm@8: 	mexPrintf("           cost matrix S; return it in D along with traceback\n");
matthiasm@8: 	mexPrintf("           indices in P. Optional C defines allowable steps\n");
matthiasm@8: 	mexPrintf("           and costs; default [1 1 1.0;1 0 1.0;0 1 1.0]\n");
matthiasm@8: 	return;
matthiasm@8:     }
matthiasm@8: 
matthiasm@8:     if (nlhs > 0){
matthiasm@8: 	mxArray  *DMatrix, *PMatrix;
matthiasm@8: 	int rows, cols, i, j, k, tb;
matthiasm@8: 	double *pM, *pD, *pP, *pC;
matthiasm@8: 	double d1, d2, d3, v;
matthiasm@8: 	double *costs;
matthiasm@8: 	int *steps;
matthiasm@8: 	int ncosts;
matthiasm@8: 
matthiasm@8: 	rows = mxGetM(prhs[0]);
matthiasm@8: 	cols = mxGetN(prhs[0]);
matthiasm@8: 	pM = mxGetPr(prhs[0]);
matthiasm@8: 
matthiasm@8: 	DMatrix = mxCreateDoubleMatrix(rows, cols, mxREAL);
matthiasm@8: 	pD = mxGetPr(DMatrix);
matthiasm@8: 	PMatrix = mxCreateDoubleMatrix(rows, cols, mxREAL);
matthiasm@8: 	pP = mxGetPr(PMatrix);
matthiasm@8: 	plhs[0] = DMatrix;
matthiasm@8: 	if (nlhs > 1) {
matthiasm@8: 	    plhs[1] = PMatrix;
matthiasm@8: 	}
matthiasm@8: 
matthiasm@8: 	/* setup costs */
matthiasm@8: 	if (nrhs == 1) {
matthiasm@8: 	    /* default C matrix */
matthiasm@8: 	    int ii;
matthiasm@8: 
matthiasm@8: 	    ncosts = 3;
matthiasm@8: 	    costs = (double *)malloc(ncosts*sizeof(double));
matthiasm@8: 	    for (ii = 0; ii<ncosts; ++ii) costs[ii] = 1.0;
matthiasm@8: 	    steps = (int *)malloc(ncosts*2*sizeof(int));
matthiasm@8: 	    steps[0] = 1;	steps[1] = 1;
matthiasm@8: 	    steps[2] = 1;	steps[3] = 0;
matthiasm@8: 	    steps[4] = 0;	steps[5] = 1;
matthiasm@8: 	} else {
matthiasm@8: 	    int ii, crows, ccols;
matthiasm@8: 	    crows = mxGetM(prhs[1]);
matthiasm@8: 	    ccols = mxGetN(prhs[1]);
matthiasm@8: 	    pC = mxGetPr(prhs[1]);
matthiasm@8: 	    /* mexPrintf("C has %d rows and %d cols\n", crows, ccols); */
matthiasm@8: 	    if (ccols != 3) {
matthiasm@8: 		mexPrintf("Cost matrix must have 3 cols (i step, j step, cost factor)\n");
matthiasm@8: 		return;
matthiasm@8: 	    }
matthiasm@8: 	    ncosts = crows;
matthiasm@8: 	    costs = (double *)malloc(ncosts*sizeof(double));
matthiasm@8: 	    steps = (int *)malloc(ncosts*2*sizeof(int));
matthiasm@8: 	    for (ii = 0; ii < ncosts; ++ii) {
matthiasm@8: 		steps[2*ii] = (int)(pC[ii]);
matthiasm@8: 		steps[2*ii+1] = (int)(pC[ii+crows]);
matthiasm@8: 		costs[ii] = pC[ii+2*crows];
matthiasm@8: 		/* mexPrintf("step=%d,%d cost=%f\n", steps[2*ii],steps[2*ii+1],costs[ii]); */
matthiasm@8: 	    }
matthiasm@8: 	}
matthiasm@8: 
matthiasm@8: 
matthiasm@8: 	/* do dp */
matthiasm@8: 	v = 0;	
matthiasm@8: 	tb = 1;	/* value to use for 0,0 */
matthiasm@8: 	for (j = 0; j < cols; ++j) {
matthiasm@8: 	    for (i = 0; i < rows; ++i) {
matthiasm@8: 		d1 = pM[i + j*rows];
matthiasm@8: 		for (k = 0; k < ncosts; ++k) {
matthiasm@8: 		    if ( i >= steps[2*k] && j >= steps[2*k+1] ) {
matthiasm@8: 			d2 = costs[k]*d1 + pD[(i-steps[2*k]) + (j-steps[2*k+1])*rows];
matthiasm@8: 			if (d2 < v) {
matthiasm@8: 			    v = d2;
matthiasm@8: 			    tb = k+1;
matthiasm@8: 			}
matthiasm@8: 		    }
matthiasm@8: 		}
matthiasm@8: 
matthiasm@8: 		pD[i + j*rows] = v;
matthiasm@8: 		pP[i + j*rows] = (double)tb;
matthiasm@8: 		v = INF;
matthiasm@8: 	    }
matthiasm@8: 	}
matthiasm@8: 	free((void *)costs);
matthiasm@8: 	free((void *)steps);
matthiasm@8:     }
matthiasm@8: 
matthiasm@8: #ifdef DEBUG
matthiasm@8:     mexPrintf("dpcore: returning...\n");
matthiasm@8: #endif /* DEBUG */
matthiasm@8: }
matthiasm@8: