/** genRay **/
vector_t genRay(scene_t *scene, int column, int row)
{
//initialize the vector for the world
vector_t vector;
//obtain the window
window_t *window = (window_t*)scene->window->entDerived;
//obtain the viewpoint
point_t viewpoint = ((window_t*)scene->window->entDerived)->viewPoint;
// calculations for world coordinates
double x = ((double)(column) / (double)(scene->picture->columns - 1)) *
window->windowWidth;
x -= window->windowWidth/2.0;
double y = ((double)(row) / (double)(scene->picture->rows - 1)) *
window->windowHeight;
y -= window->windowHeight/2.0;
//assign values to vector
vector.x = x;
vector.y = y;
vector.z = 0;
//compute distance between the points
vector_t result;
result = vec_subtract(vector, viewpoint);
//return the normalized vector
return vec_normalize(result);
} /* End genRay */
COLOR_T illuminate (SCENE_T scene, RAY_T ray, VP_T int_pt, VP_T normal, int object) {
COLOR_T color;
COLOR_T input_color = scene.objs[object].color;
if(scene.objs[object].checkerboard) {
if(((int)floor(int_pt.x) + (int)floor(int_pt.y) + (int)floor(int_pt.z)) & 1) {
input_color = scene.objs[object].color2;
}
}
//ambient
color.R = 0.1 * scene.objs[object].color.R;
color.G = 0.1 * scene.objs[object].color.G;
color.B = 0.1 * scene.objs[object].color.B;
if(!test_shadow(scene, int_pt, normal, object)) {
//diffuse
VP_T L = vec_subtract(scene.light.location, int_pt);
double dL = vec_len(L);
L = vec_normalize(L);
double dot_L = vec_dot(normal, L);
double c1 = 0.002,
c2 = 0.02,
c3 = 0.2;
double light_atten = 1.0 / (c1 * dL * dL +
c2 * dL + c3);
if (dot_L > 0) {
color.R += dot_L * input_color.R * light_atten;
color.G += dot_L * input_color.G * light_atten;
color.B += dot_L * input_color.B * light_atten;
//specular
VP_T R;
R.x = L.x - 2 * dot_L * normal.x;
R.y = L.y - 2 * dot_L * normal.y;
R.z = L.z - 2 * dot_L * normal.z;
R = vec_normalize(R);
double dot_R = vec_dot(R, ray.direction);
if (dot_R > 0) {
color.R += pow(dot_R, 100);
color.G += pow(dot_R, 100);
color.B += pow(dot_R, 100);
}
}
}
return color;
}
double weigh_rule(const rule_t* rule,const double* vec) {
int i,j;
double res = 0;
int n = rule->size;
double* t = calloc(sizeof(double),n);
vec_subtract(n,vec,rule->vmean,t);
for(i=0;i<n;i++) {
if((rule->bitvector | (1 << i)) != rule->bitvector) {
double tmp = 0;
for(j=0;j<n;j++) {
tmp+=rule->covar[i*n+j]*t[j];
}
res += t[i]*tmp;
}
}
free(t);
return exp(-0.5*res);
}
vec<N,S>
operator-(vec<N,S> const& u, vec<N,S> const& v) {
return vec_subtract(u, v);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double M;
double log_thr;
int Xs,D,Ys,i,j,ep=0;
int maxentep;
int pxy_size;
double mx;
Matrix phi,psi,phi_tr,psi_tr,phi_exp_d,psi_exp_d,psi_exp_m,phi_exp_m,grad_phi,grad_psi;
double *px0,*py0,*px,*py,*pa,*pb,*grad_a,*grad_b;
double Z,loglik;
int xi,yi,di;
int b_cond; /* Says if the model is conditioned on x, or joint */
double *p0_i,*p0_j,*p0_val;
int Np0;
MatFromMLab(prhs[0],&phi); /* Size NX,D */
MatFromMLab(prhs[1],&psi); /* Size NX,D */
MatFromMLab(prhs[2],&phi_tr); /* Size D,NX */
MatFromMLab(prhs[3],&psi_tr); /* Size D,NY */
px0 = mxGetPr(prhs[4]);
py0 = mxGetPr(prhs[5]);
MatFromMLab(prhs[6],&phi_exp_d);
MatFromMLab(prhs[7],&psi_exp_d);
pa = mxGetPr(prhs[8]);
pb = mxGetPr(prhs[9]);
b_cond = *mxGetPr(prhs[10]);
p0_i = mxGetPr(prhs[11]);
p0_j = mxGetPr(prhs[12]);
p0_val = mxGetPr(prhs[13]);
Xs = mxGetM(prhs[0]);
D = mxGetN(prhs[0]);
Ys = mxGetM(prhs[1]);
Np0 = mxGetNumberOfElements(prhs[13]);
pxy_size = Xs*Ys;
plhs[0] = mxCreateDoubleMatrix(D,Xs,mxREAL);
plhs[1] = mxCreateDoubleMatrix(D,Ys,mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,Xs,mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,Ys,mxREAL);
plhs[4] = mxCreateDoubleMatrix(1,1,mxREAL);
grad_phi.dat = mxGetPr(plhs[0]);
grad_psi.dat = mxGetPr(plhs[1]);
grad_phi.nx = D;
grad_phi.ny = Xs;
grad_phi.nall = Xs*D;
grad_psi.nx = D;
grad_psi.ny = Ys;
grad_psi.nall = Ys*D;
grad_a = mxGetPr(plhs[2]);
grad_b = mxGetPr(plhs[3]);
AllocMat(D,Ys,&phi_exp_m);
AllocMat(D,Xs,&psi_exp_m);
px = malloc(Xs*sizeof(double));
py = malloc(Ys*sizeof(double));
if (px==NULL || py==NULL)
printf("Error in allocation\n");
if (!b_cond)
mat_logemb_model(phi_tr, psi_tr, pa, pb, phi_exp_m, psi_exp_m, px, py, p0_i, p0_j, p0_val, Np0, mxGetPr(plhs[4]),b_cond) ;
else
{
/* Run logemb model one time to get the x partition function in px */
mat_logemb_model(phi_tr, psi_tr, pa, pb, phi_exp_m, psi_exp_m, px, py, p0_i, p0_j, p0_val, Np0, mxGetPr(plhs[4]),b_cond) ;
/* Create new x factor */
for (xi=0; xi<Xs; xi++)
grad_a[xi] = pa[xi]-log(px[xi])+log(px0[xi]);
mat_logemb_model(phi_tr, psi_tr, grad_a, pb, phi_exp_m, psi_exp_m, px, py, p0_i, p0_j, p0_val, Np0, mxGetPr(plhs[4]),0) ;
}
/* Matlab line: diag(px)*phi-pxy*psi - (diag(px0)*phi-pxy0*psi);
/* equivalent to diag(px-px0)*phi+psi_expect_d - psi_expect_m */
vec_subtract(px,px0,Xs);
mat_set(grad_phi,phi_tr);
/* printf("\n Phi1 Max=%g MaxPx=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall),mat_max_abs(px,Xs)); */
mat_mul_cols(grad_phi,px);
/* printf("\n Phi2 Max=%g MaxPx=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall),mat_max_abs(px,Xs)); */
mat_subtract(psi_exp_d.dat,psi_exp_m.dat,psi_exp_d.nall);
/* printf("\n Phi3 Max=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall)); */
mat_add(grad_phi.dat,psi_exp_d.dat,psi_exp_d.nall);
/* printf("\n Phi4 Max=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall)); */
vec_subtract(py,py0,Ys);
mat_set(grad_psi,psi_tr);
mat_mul_cols(grad_psi,py);
mat_subtract(phi_exp_d.dat,phi_exp_m.dat,phi_exp_d.nall);
mat_add(grad_psi.dat,phi_exp_d.dat,phi_exp_d.nall);
/* Generate gradient for a and b. Remember px already has px subtracted. Just need to flip sign */
memcpy(grad_a,px,Xs*sizeof(double));
vec_mul_scalar(grad_a,-1,Xs);
memcpy(grad_b,py,Ys*sizeof(double));
vec_mul_scalar(grad_b,-1,Ys);
free(psi_exp_m.dat);
free(phi_exp_m.dat);
free(px);
free(py);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double M;
double log_thr;
int Xs,D,Ys,i,j,ep=0;
int maxentep;
int pxy_size;
double mx;
Matrix pxy,phi,psi,phi_tr,psi_tr,phi_exp_d,psi_exp_d,psi_exp_m,phi_exp_m,grad_phi,grad_psi;
double *px0,*py0,*px,*py,*pa,*pb,*grad_a,*grad_b;
double Z;
int xi,yi,di;
MatFromMLab(prhs[0],&phi); /* Size NX,D */
MatFromMLab(prhs[1],&psi); /* Size NX,D */
MatFromMLab(prhs[2],&phi_tr); /* Size D,NX */
MatFromMLab(prhs[3],&psi_tr); /* Size D,NY */
px0 = mxGetPr(prhs[4]);
py0 = mxGetPr(prhs[5]);
MatFromMLab(prhs[6],&phi_exp_d);
MatFromMLab(prhs[7],&psi_exp_d);
pa = mxGetPr(prhs[8]);
pb = mxGetPr(prhs[9]);
Xs = mxGetM(prhs[0]);
D = mxGetN(prhs[0]);
Ys = mxGetM(prhs[1]);
pxy_size = Xs*Ys;
plhs[0] = mxCreateDoubleMatrix(Xs,D,mxREAL);
plhs[1] = mxCreateDoubleMatrix(D,Ys,mxREAL);
plhs[2] = mxCreateDoubleMatrix(Xs,Ys,mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,Xs,mxREAL);
plhs[4] = mxCreateDoubleMatrix(1,Ys,mxREAL);
grad_phi.dat = mxGetPr(plhs[0]);
grad_psi.dat = mxGetPr(plhs[1]);
grad_phi.nx = Xs;
grad_phi.ny = D;
grad_phi.nall = Xs*D;
grad_psi.nx = D;
grad_psi.ny = Ys;
grad_psi.nall = Ys*D;
/* Will hold the models distribution */
pxy.dat = mxGetPr(plhs[2]);
pxy.nx = Xs;
pxy.ny = Ys;
pxy.nall = Xs*Ys;
grad_a = mxGetPr(plhs[3]);
grad_b = mxGetPr(plhs[4]);
AllocMat(D,Ys,&phi_exp_m);
AllocMat(Xs,D,&psi_exp_m);
px = malloc(Xs*sizeof(double));
py = malloc(Ys*sizeof(double));
mat_logemb_model(phi_tr,psi_tr,pa,pb,pxy);
Z = mat_el_exp_sum(pxy.dat, pxy_size);
mat_el_mul_scalar(pxy.dat, 1/Z, pxy_size);
mat_sum_rows(pxy,px);
mat_sum_cols(pxy,py);
mat_mul_a_tr_b(phi,pxy,phi_exp_m);
mat_mul(pxy,psi,psi_exp_m);
/* Matlab line: diag(px)*phi-pxy*psi - (diag(px0)*phi-pxy0*psi);
/* equivalent to diag(px-px0)*phi+psi_expect_d - psi_expect_m */
vec_subtract(px,px0,Xs);
mat_set(grad_phi,phi);
mat_mul_rows(grad_phi,px);
mat_subtract(psi_exp_d.dat,psi_exp_m.dat,psi_exp_d.nall);
mat_add(grad_phi.dat,psi_exp_d.dat,psi_exp_d.nall);
vec_subtract(py,py0,Ys);
mat_set(grad_psi,psi_tr);
mat_mul_cols(grad_psi,py);
mat_subtract(phi_exp_d.dat,phi_exp_m.dat,phi_exp_d.nall);
mat_add(grad_psi.dat,phi_exp_d.dat,phi_exp_d.nall);
mat_el_log(pxy.dat,pxy.nall);
/* Generate gradient for a and b. Remember px already has px subtracted. Just need to flip sign */
memcpy(grad_a,px,Xs*sizeof(double));
vec_mul_scalar(grad_a,-1,Xs);
memcpy(grad_b,py,Ys*sizeof(double));
vec_mul_scalar(grad_b,-1,Ys);
free(psi_exp_m.dat);
free(phi_exp_m.dat);
free(px);
free(py);
}
/* The objective function for the optimization problem
* x The vector at which to evaluate the objective function.
*/
static double objective(const double *x, const region_params_t *params)
{
int cur = 1, prev = 0;
vec_t p[2], v[2], d[2];
vec_t a;
double omega[2];
double domega;
double theta;
double y = 0.0;
double cons = 0.0; /* Constraint terms */
double penalty = 0.0; /* Penalty terms */
int ccount; /* Number of constraints */
int pcount; /* Number of penalties */
int i, j, k;
const double *l, *r;
const int iters = 5;
const double h = params->dt / iters;
p[prev] = params->p_0;
v[prev] = params->v_0;
d[prev] = params->d_0;
omega[prev] = params->omega_0;
l = x;
r = x+params->N;
pcount = 0;
ccount = 0;
#define CONSTRAINT(x, y) do{double c_=ib(params->constraint_mu,x,y)/(ccount+1);\
double cc_=(ccount?(cons/(1.0+1.0/ccount)):0);\
cons=c_+cc_;ccount++;}while(0)
#define PENALTY(x, y) do{double p_=qp(params->penalty_mu,x,y)/(pcount+1);\
double pc_=(pcount?(penalty/(1.0+1.0/pcount)):0);\
penalty=p_+pc_;pcount++;}while(0)
for (i = 1; i <= params->N; i++) {
for (j = 0; j < iters; j++) {
/* Enforce unit direction */
d[prev] = vec_normalized(d[prev]);
/* Integrate the angular acceleration over the time step to
* get angular velocity */
domega = 2*(l[i-1] - r[i-1])/(params->radius * params->mass);
omega[cur] = omega[prev] + h * domega;
#if USE_MACLAURIN
/* Evaluate our Maclaurin series approximation to the movement of
* our object */
maclaurin(h, 7, d[prev], v[prev], p[prev], l[i-1], r[i-1],
omega[prev], params->mass, params->radius, &d[cur], &v[cur],
&p[cur]);
#else
/* direction */
theta = 0.5 * h * (omega[cur] + omega[prev]);
d[cur].x = d[prev].x * cos(theta) - d[prev].y * sin(theta);
d[cur].y = d[prev].x * sin(theta) + d[prev].y * cos(theta);
/* acceleration */
a = vec_scale((l[i-1] + r[i-1]) / params->mass, d[prev]);
/* velocity */
v[cur] = vec_add(v[prev], vec_scale(h, a));
/* position */
p[cur] = vec_add(p[prev], vec_add(vec_scale(h*h*0.5, a), vec_scale(h, v[prev])));
#endif
/* Minimize the distance from the goal at the start of each time step */
y += vec_sqnorm(vec_subtract(p[cur], params->p_n));
//y += square(p[cur].x - params->p_n.x);
//y += square(p[cur].y - params->p_n.y);
/* Feasible region constraints */
for (k = 0; k < params->polycount; k++) {
double dist = line_dist(params->poly[k],
params->poly[(k+1)%params->polycount], p[cur]);
PENALTY(params->radius, dist);
}
prev = cur;
cur = 1 - cur;
}
}
CONSTRAINT(square(l[0] - params->l_0), square(params->dt));
CONSTRAINT(square(r[0] - params->r_0), square(params->dt));
PENALTY(vec_sqnorm(v[prev]), 0);
PENALTY(vec_sqnorm(vec_subtract(p[prev], params->p_n)), 0);
PENALTY(square(omega[prev]), 0);
for (i = 1; i < params->N; i++) {
CONSTRAINT(square(l[i] - l[i-1]), square(params->dt));
CONSTRAINT(square(r[i] - r[i-1]), square(params->dt));
}
#undef CONSTRAINT
#undef PENALTY
y += cons + penalty;
return y;
}
