double dp_costs(
double *Q1, double *T1, int nsamps1,
double *Q2, double *T2, int nsamps2,
int dim,
double *tv1, int *idxv1, int ntv1,
double *tv2, int *idxv2, int ntv2,
double *E, int *P, double lam )
{
int sr, sc; /* source row and column */
int tr, tc; /* target row and column */
double w, cand_cost;
int i;
E[0] = 0.0;
for ( i=1; i<ntv1; E[i++]=1e6 );
for ( i=1; i<ntv2; E[ntv1*i++]=1e6 );
for ( tr=1; tr<ntv2; ++tr )
{
for ( tc=1; tc<ntv1; ++tc )
{
E[ntv1*tr + tc] = 1e6;
for ( i=0; i<DP_NBHD_COUNT; ++i )
{
sr = tr - dp_nbhd[i][0];
sc = tc - dp_nbhd[i][1];
if ( sr < 0 || sc < 0 ) continue;
w = dp_edge_weight( Q1, T1, nsamps1, Q2, T2, nsamps2, dim,
tv1[sc], tv1[tc], tv2[sr], tv2[tr], idxv1[sc], idxv2[sr], lam );
cand_cost = E[ntv1*sr+sc] + w;
if ( cand_cost < E[ntv1*tr+tc] )
{
E[ntv1*tr+tc] = cand_cost;
P[ntv1*tr+tc] = ntv1*sr + sc;
}
}
}
}
/*
for ( tr=1; tr<ntv2; ++tr )
{
for ( tc=1; tc<ntv1; ++tc )
{
printf( "E[%d,%d]=%0.3f, ", tr, tc, E[ntv1*tr+tc] );
printf( "P[%d,%d]=(%d,%d)\n", tr, tc, P[ntv1*tr+tc]/ntv1,
P[ntv1*tr+tc]%ntv1 );
}
}
*/
return E[ntv1*ntv2-1];
}
void dp_all_edge_weights(
double *Q1, double *T1, int nsamps1,
double *Q2, double *T2, int nsamps2,
int dim,
double *tv1, int *idxv1, int ntv1,
double *tv2, int *idxv2, int ntv2,
double *W, double lam )
{
int sr, sc; /* source row and column */
int tr, tc; /* target row and column */
int l1, l2, l3; /* for multidimensional array mapping */
int i;
for ( i=0; i<ntv1*ntv2*ntv1*ntv2; W[i++]=1e6 );
/* W is a ntv2 x ntv1 x ntv2 x ntv1 array.
* Weight of edge from (tv1[i],tv2[j]) to (tv1[k],tv2[l])
* (Cartesian coordinates) is in grid(j,i,l,k).
* Mapping:
* (j,i,l,k) :--> j*ntv1*ntv2*ntv1 +
* i*ntv2*ntv1 +
* l*ntv1 +
* k
*/
l1 = ntv1 * ntv2 * ntv1;
l2 = ntv2 * ntv1;
l3 = ntv1;
for ( tr=1; tr<ntv2; ++tr )
{
for ( tc=1; tc<ntv1; ++tc )
{
for ( i=0; i<DP_NBHD_COUNT; ++i )
{
sr = tr - dp_nbhd[i][0];
sc = tc - dp_nbhd[i][1];
if ( sr < 0 || sc < 0 ) continue;
/* grid(sr,sc,tr,tc) */
W[sr*l1+sc*l2+tr*l3+tc] =
dp_edge_weight( Q1, T1, nsamps1, Q2, T2, nsamps2, dim,
tv1[sc], tv1[tc], tv2[sr], tv2[tr], idxv1[sc], idxv2[sr], lam );
/*
printf( "(%0.2f,%0.2f) --> (%0.2f,%0.2f) = %0.2f\n",
a, c, b, d, grid[sr*l1+sc*l2+tr*l3+tc] );
*/
}
}
}
}
static void test_dp_edge_weight_basic1()
{
double T1[] = { 0.0, 1.0/6.0, 0.5, 4.0/6.0, 5.0/6.0, 1.0 };
double Q1[] = { 1.0, -1.0, 1.0, -1.0, 1.0 };
double T2[] = { 0.0, 2.0/6.0, 5.0/6.0, 1.0 };
double Q2[] = { -1.0, 1.0, -1.0 };
double tv1[] = { 0.0, 0.25, 0.5, 0.75, 1.0 };
double tv2[] = { 0.0, 0.25, 0.5, 0.75, 1.0 };
int nsamps1 = sizeof(T1) / sizeof(double);
int nsamps2 = sizeof(T2) / sizeof(double);
int ntv1 = sizeof(tv1) / sizeof(double);
int ntv2 = sizeof(tv2) / sizeof(double);
int dim = 1;
double W[ntv2*ntv1*ntv2*ntv1]; /* edge weight matrix */
double E[ntv2*ntv1]; /* DP cost matrix */
int P[ntv2*ntv1]; /* DP predecessor matrix */
int idxv1b[ntv1];
int idxv2b[ntv2];
double G[ntv1]; /* gamma function values */
double T[ntv1]; /* gamma parameter values */
int npts;
double a[] = { 0.25, 0.75, 0.0, 0.0, 0.25, 1.0/6.0 };
double b[] = { 0.75, 1.0, 0.75, 0.25, 0.5, 0.5 };
double c[] = { 0.25, 0.25, 0.0, 0.0, 0.0, 1.0/6.0 };
double d[] = { 0.5, 1.0, 0.25, 0.25, 0.25, 5.0/6.0 };
double expected[] = { 0.51430, 0.90377, 0.90377, 0.66667, 0.0, 1.4714 };
int ncases = sizeof(expected) / sizeof(double);
int idxv1a, idxv2a;
double actual;
int i;
for ( i=0; i<ncases; ++i )
{
/* Can't use dp_all_indexes() for this because a and c are
* not sorted in ascending order */
idxv1a = dp_lookup(T1,nsamps1,a[i]);
idxv2a = dp_lookup(T2,nsamps2,c[i]);
actual = dp_edge_weight (
Q1, T1, nsamps1,
Q2, T2, nsamps2,
dim, a[i], b[i], c[i], d[i], idxv1a, idxv2a );
CU_ASSERT_DOUBLE_EQUAL( actual, expected[i], 0.0001 );
if ( fabs(actual - expected[i]) > 0.0001 )
{
printf( "\nCase %d: expected %0.5f but got %0.5f\n", i, expected[i], actual );
}
}
dp_all_indexes(T1,nsamps1,tv1,ntv1,idxv1b);
dp_all_indexes(T2,nsamps2,tv2,ntv2,idxv2b);
dp_all_edge_weights(Q1,T1,6,Q2,T2,4,1,tv1,idxv1b,ntv1,tv2,idxv2b,ntv2,W);
dp_costs(Q1,T1,6,Q2,T2,4,1,tv1,idxv1b,ntv1,tv2,idxv2b,ntv2,E,P);
npts = dp_build_gamma( P, tv1, ntv1, tv2, ntv2, G, T );
printf( "gamma:\n" );
for ( i=0; i<npts; ++i )
{
printf( "(%0.2f,%0.2f)\n", T[i], G[i] );
}
}
