double optimalDirection(int lookahead, int x, int y, int v0x, int v0y, int *v1x, int *v1y) {
int vxtry, vytry;
double dTheta,tmp;
int dvx, dvy;
_Bool col;
int garbage;
col = collision(x,y);
if (col) {
v0x = 0;
v0y = 0;
//if (lookahead == initial_lookahead) {
// mexPrintf("COLLISION! %i, %i\n",x,y);
//}
}
if (lookahead > 0) {
dTheta = -900; // -infinity
for (dvx = -1; dvx <= 1; dvx++) {
for (dvy = -1; dvy <= 1; dvy++) {
vxtry = v0x + dvx;
vytry = v0y + dvy;
tmp = angledist(x,y,x+vxtry,y+vytry);
tmp += optimalDirection(lookahead-1,x+vxtry,y+vytry,vxtry,vytry,&garbage,&garbage);
if (tmp > dTheta) {
dTheta = tmp;
*v1x = vxtry;
*v1y = vytry;
}
}
}
} else { // Don't look ahead, just continue with this speed
dTheta = angledist(x,y,x+v0x,y+v0y);
*v1x = v0x;
*v1y = v0y;
}
if (col) {
dTheta -= M_PI_4; // Punishment if off course.
}
return dTheta;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// Input: Board, center, position, velocity
const mxArray *lookahead, *board_input, *center, *mxx, *mxv;
int x, y, vx, vy, lookahead_int;
// output: new velocity, dTheta
mxArray *newv, *dTheta;
int i,j;
int v1x, v1y;
if (nrhs != 5)
mexErrMsgIdAndTxt("optimalDirection", "Input arguments: board, center, initial positioni, initial velocity and lookahead.");
if (nlhs != 2)
mexErrMsgIdAndTxt("optimalDirection", "Two output arguments: acceleration and change of theta");
lookahead = prhs[0];
board_input = prhs[1];
center = prhs[2];
mxx = prhs[3];
mxv = prhs[4];
boardN = mxGetN(board_input); // Number of columns
boardM = mxGetM(board_input); // Number of rows
board = mxGetData(board_input);
double (*center_p)[2] = mxGetData(center);
center_x = (int) (*center_p)[0];
center_y = (int) (*center_p)[1];
//mexPrintf("center: %i, %i\n", center_x,center_y);
//printf("%i\n",collision(center_x,center_y));
double (*mxx_p)[2] = mxGetData(mxx);
double (*mxv_p)[2] = mxGetData(mxv);
x = (int) (*mxx_p)[0];
y = (int) (*mxx_p)[1];
vx = (int) (*mxv_p)[0];
vy = (int) (*mxv_p)[1];
//mexPrintf("x,y: %i, %i\n", x,y);
//printf("%i\n",collision(x,y));
double *lookahead_double = mxGetData(lookahead);
lookahead_int = (int) *lookahead_double;
initial_lookahead = lookahead_int;
//mexPrintf("%i\n", collision(42,31));
optimalDirection(lookahead_int, x, y, vx, vy, &v1x, &v1y);
// Give matlab the output:
newv = mxCreateDoubleMatrix(2,1,0);
dTheta = mxCreateDoubleMatrix(1,1,0);
double (*newv_p)[2] = mxGetData(newv);
(*newv_p)[0] = v1x;
(*newv_p)[1] = v1y;
double *dTheta_p = mxGetData(dTheta);
*dTheta_p = angledist(x,y,x+v1x,y+v1y);
if (collision(x+v1x,y+v1y)) {
*dTheta_p -= M_PI_4;
}
//mexPrintf("vnew = %i, %i\n", v1x, v1y);
plhs[0] = newv;
plhs[1] = dTheta;
}
void comm_findmaxq(int n, graph *g, double *evec, int uev,
int *e1, int *e2, double *maxq, int *kmax) {
int k,j, nl, dk, i1, i2, l1, l2, rl1, rl2, oldn, kk;
double *c, q;
double *coords[n];
community *comm[n];
community *c1, *c2, *cl;
distlink *dlink;
/* get coordinates from eigenvectors */
c = malloc(n*uev*sizeof(double));
for (k=0; k<n; k++) {
coords[k] = c + uev*k;
for (j=0; j<uev; j++)
coords[k][j] = evec[(j+1)*n+k];
}
/* calculate distance for all links */
nl = 0;
dlink = malloc(g->nlink*sizeof(distlink));
for (k=0; k<n; k++) {
for (j=0; j<g->node[k].deg; j++)
if (k < g->node[k].link[j]) {
dlink[nl].n1 = k;
dlink[nl].n2 = g->node[k].link[j];
dlink[nl].d = angledist(coords[k], coords[dlink[nl].n2], uev);
nl++;
}
}
free(c);
assert(g->nlink == nl);
qsort(dlink, nl, sizeof(distlink), compare_distlinks);
/* create single member communitities */
q = 0;
for (k=0; k<n; k++){
comm[k] = malloc(sizeof(community));
comm[k]->nmember = 1;
comm[k]->members = malloc(sizeof(int));
comm[k]->members[0] = k;
dk = g->node[k].deg;
comm[k]->nlink = dk;
comm[k]->linkto = malloc(dk*sizeof(int));
comm[k]->distk = malloc(dk*sizeof(int));
comm[k]->eij = malloc(dk*sizeof(double));
for (j=0; j<dk; j++) {
comm[k]->linkto[j] = g->node[k].link[j];
comm[k]->eij[j] = 0.5/nl;
}
comm[k]->ai = 0.5*dk/nl;
q -= comm[k]->ai*comm[k]->ai;
}
/* fill distk values */
for (k=0; k<nl; k++) {
c1 = comm[dlink[k].n1];
for (j=0; j<c1->nlink; j++) /* find link to n2 */
if (c1->linkto[j] == dlink[k].n2)
break;
assert(j<c1->nlink);
c1->distk[j] = k;
/* do the same for reverse link */
c2 = comm[dlink[k].n2];
for (j=0; j<c2->nlink; j++) /* find link to n1 */
if (c2->linkto[j] == dlink[k].n1)
break;
assert(j<c2->nlink);
c2->distk[j] = k;
}
*maxq = q;
*kmax = 0;
kk = 0;
for (k=0; k<nl; k++) {
if (dlink[k].d < 0)
continue;
e1[kk] = dlink[k].n1;
e2[kk] = dlink[k].n2;
c1 = comm[e1[kk]];
c2 = comm[e2[kk]];
assert(c1 != c2);
i1 = c1->members[0];
i2 = c2->members[0];
/* find link from c1 to c2 */
for (j=0; j<c1->nlink; j++)
if (c1->linkto[j] == i2)
break;
assert(j<c1->nlink); /* link found */
q += 2*(c1->eij[j] - c1->ai*c2->ai);
kk++;
if (q>*maxq) {
*maxq = q;
*kmax = kk;
}
/* add c1 and c2 data */
c1->ai += c2->ai;
for (l2=0; l2<c2->nlink; l2++) { /* adjusting links */
cl = comm[c2->linkto[l2]];
if (cl == c2)
continue;
for (rl2=0; rl2<cl->nlink; rl2++) /* find reverse link */
if (cl->linkto[rl2] == i2)
break;
assert(rl2<cl->nlink);
/* look for link to the same community in c1 */
for (l1=0; l1<c1->nlink; l1++)
if (c1->linkto[l1] == c2->linkto[l2])
break;
if (l1<c1->nlink) { /* found link to the same community */
for (rl1=0; rl1<cl->nlink; rl1++) /* find reverse link */
if (cl->linkto[rl1] == i1)
break;
assert(rl1<cl->nlink);
c1->eij[l1] += c2->eij[l2];
/* find shortest link */
if (dlink[c1->distk[l1]].d > dlink[c2->distk[l2]].d)
dlink[c2->distk[l2]].d = -1;
else {
dlink[c1->distk[l1]].d = -1;
c1->distk[l1] = c2->distk[l2];
}
cl->eij[rl1] += cl->eij[rl2]; /* add eij's */
cl->distk[rl1] = c1->distk[l1];
cl->eij[rl2] = cl->eij[cl->nlink-1]; /* delete link to c2 */
cl->linkto[rl2] = cl->linkto[cl->nlink-1];
cl->distk[rl2] = cl->distk[cl->nlink-1];
cl->nlink--;
cl->eij = realloc(cl->eij, (cl->nlink)*sizeof(double));
cl->linkto = realloc(cl->linkto, (cl->nlink)*sizeof(int));
cl->distk = realloc(cl->distk, (cl->nlink)*sizeof(int));
} else { /* link to the same community not found */
c1->nlink++;
c1->eij = realloc(c1->eij, (c1->nlink)*sizeof(double));
c1->linkto = realloc(c1->linkto, (c1->nlink)*sizeof(int));
c1->distk = realloc(c1->distk, (c1->nlink)*sizeof(int));
c1->eij[c1->nlink-1] = c2->eij[l2];
c1->linkto[c1->nlink-1] = c2->linkto[l2];
c1->distk[c1->nlink-1] = c2->distk[l2];
cl->linkto[rl2] = i1;
}
}
/* find c1's link to c2 */
for (l2=0; l2<c1->nlink; l2++)
if (c1->linkto[l2] == i2)
break;
if (l2<c1->nlink) { /* if link exists */
/* find link to c1 itself if it exists */
for (l1=0; l1<c1->nlink; l1++)
if (c1->linkto[l1] == i1)
break;
if (l1<c1->nlink) { /* previous link to c1 found */
c1->eij[l1] += c1->eij[l2]; /* add eij's */
c1->eij[l2] = c1->eij[c1->nlink-1]; /* delete link to c2 */
c1->linkto[l2] = c1->linkto[c1->nlink-1];
c1->distk[l2] = c1->distk[c1->nlink-1];
c1->nlink--;
c1->eij = realloc(c1->eij, (c1->nlink)*sizeof(double));
c1->linkto = realloc(c1->linkto, (c1->nlink)*sizeof(int));
c1->distk = realloc(c1->distk, (c1->nlink)*sizeof(int));
} else /* no previous links to c1 */
c1->linkto[l2] = i1;
}
/* add members */
oldn = c1->nmember;
c1->nmember += c2->nmember;
c1->members = realloc(c1->members, c1->nmember*sizeof(int));
for (j=0; j<c2->nmember; j++) {
c1->members[oldn+j] = c2->members[j];
comm[c2->members[j]] = c1;
}
free(c2->members);
free(c2->linkto);
free(c2->distk);
free(c2->eij);
free(c2);
}
assert(n==comm[0]->nmember);
free(comm[0]->members);
free(comm[0]->linkto);
free(comm[0]->distk);
free(comm[0]->eij);
free(comm[0]);
free(dlink);
}
