void compute_new_heading(int which)
{
int i, j, k, numcent = 0;
double xa, ya, xb, yb, xc, yc, xd, yd, xt, yt;
double mindist, mx = 0, my = 0, d;
double cosangle, cosvangle, costemp;
double xtemp, ytemp, maxr, u, v;
/* This is the maximum distance in which any rule is activated. */
maxr = MAX(rviso, MAX(rcopy, MAX(rcent, rvoid)));
/* These two values are used to see if a boid can "see" another
* boid in various ways.
*/
cosangle = cos(angle / 2);
cosvangle = cos(vangle / 2);
/* These are the accumulated change vectors for the four rules. */
xa = ya = xb = yb = xc = yc = xd = yd = 0;
/* For every boid... */
for(i = 0; i < num; i++) {
/* Don't include self for computing new heading. */
if(i == which) continue;
/* Since we want boids to "see" each other around the borders of
* the screen, we need to check if a boid on the left edge is
* actually "close" to a boid on the right edge, etc. We do this
* by searching over nine relative displacements of boid(i) and
* pick the one that is closest to boid(which). These coordinates
* are then used for all remaining calculations.
*/
mindist = 10e10;
for(j = -width; j <= width; j += width)
for(k = -height; k <= height; k += height) {
d = DIST(xp[i] + j, yp[i] + k, xp[which], yp[which]);
if(d < mindist) {
mindist = d;
mx = xp[i] + j;
my = yp[i] + k;
}
}
/* If that distance is farther than any of the rule radii,
* then skip.
*/
if(mindist > maxr) continue;
/* Make a vector from boid(which) to boid(i). */
xtemp = mx - xp[which]; ytemp = my - yp[which];
/* Calculate the cosine of the velocity vector of boid(which)
* and the vector from boid(which) to boid(i).
*/
costemp = DOT(xv[which], yv[which], xtemp, ytemp) /
(LEN(xv[which], yv[which]) * LEN(xtemp, ytemp));
/* If this cosine is less than the cosine of one half
* of the boid's eyesight, i.e., boid(which) cannot see
* boid(i), then skip.
*/
if(costemp < cosangle) continue;
/* If the distance between the two boids is within the radius
* of the centering rule, but outside of the radius of the
* avoidance rule, then attempt to center in on boid(i).
*/
if(mindist <= rcent && mindist > rvoid) {
xa += mx - xp[which];
ya += my - yp[which];
numcent++;
}
/* If we are close enough to copy, but far enough to avoid,
* then copy boid(i)'s velocity.
*/
if(mindist <= rcopy && mindist > rvoid) {
xb += xv[i];
yb += yv[i];
}
/* If we are within collision range, then try to avoid boid(i). */
if(mindist <= rvoid) {
/* Calculate the vector which moves boid(which) away from boid(i). */
xtemp = xp[which] - mx;
ytemp = yp[which] - my;
/* Make the length of the avoidance vector inversely proportional
* to the distance between the two boids.
*/
d = 1 / LEN(xtemp, ytemp);
xtemp *= d; ytemp *= d;
xc += xtemp; yc += ytemp;
}
/* If boid(i) is within rviso distance and the angle between this boid's
* velocity vector and the boid(i)'s position relative to this boid is
* less than vangle, then try to move so that vision is restored.
*/
if(mindist <= rviso && cosvangle < costemp) {
/* Calculate the vector which moves boid(which) away from boid(i). */
xtemp = xp[which] - mx;
ytemp = yp[which] - my;
/* Calculate another vector that is orthogonal to the previous,
* But try to make it in the same general direction of boid(which)'s
* direction of movement.
*/
u = v = 0;
if(xtemp != 0 && ytemp != 0) {
u = sqrt(SQR(ytemp / xtemp) / (1 + SQR(ytemp / xtemp)));
v = -xtemp * u / ytemp;
}
else if(xtemp != 0)
u = 1;
else if(ytemp != 0)
v = 1;
if((xv[which] * u + yv[which] * v) < 0) {
u = -u; v = -v;
}
/* Add the vector that moves away from boid(i). */
u = xp[which] - mx + u;
v = yp[which] - my + v;
/* Make this vector's length inversely proportional to the
* distance between the two boids.
*/
d = LEN(xtemp, ytemp);
if(d != 0) {
u /= d; v /= d;
}
xd += u; yd += v;
}
}
/* Avoid centering on only one other boid;
* it makes you look aggressive!
*/
if(numcent < 2)
xa = ya = 0;
/* Normalize all big vectors. */
if(LEN(xa, ya) > 1.0) norm(&xa, &ya);
if(LEN(xb, yb) > 1.0) norm(&xb, &yb);
if(LEN(xc, yc) > 1.0) norm(&xc, &yc);
if(LEN(xd, yd) > 1.0) norm(&xd, &yd);
/* Compute the composite trajectory based on all of the rules. */
xt = xa * wcent + xb * wcopy + xc * wvoid + xd * wviso;
yt = ya * wcent + yb * wcopy + yc * wvoid + yd * wviso;
/* Optionally add some noise. */
if(wrand > 0) {
xt += random_range(-1, 1) * wrand;
yt += random_range(-1, 1) * wrand;
}
/* Update the velocity and renormalize if it is too small. */
xnv[which] = xv[which] * ddt + xt * (1 - ddt);
ynv[which] = yv[which] * ddt + yt * (1 - ddt);
d = LEN(xnv[which], ynv[which]);
if(d < minv) {
xnv[which] *= minv / d;
ynv[which] *= minv / d;
}
}
int
main(int argc, char *argv[]) {
char *me, *outS;
hestOpt *hopt;
hestParm *hparm;
airArray *mop;
char *err;
Nrrd *nin, *nhist;
double vmin, vmax, rmax, val, cent[2], rad;
int bins[2], sx, sy, xi, yi, ridx, hidx, rbins, hbins;
NrrdRange *range;
double (*lup)(const void *v, size_t I), *hist;
me = argv[0];
mop = airMopNew();
hparm = hestParmNew();
hopt = NULL;
airMopAdd(mop, hparm, (airMopper)hestParmFree, airMopAlways);
hestOptAdd(&hopt, "i", "nin", airTypeOther, 1, 1, &nin, NULL,
"input image", NULL, NULL, nrrdHestNrrd);
hestOptAdd(&hopt, "b", "rbins hbins", airTypeInt, 2, 2, bins, NULL,
"# of histogram bins: radial and value");
hestOptAdd(&hopt, "min", "value", airTypeDouble, 1, 1, &vmin, "nan",
"Value at low end of histogram. Defaults to lowest value "
"found in input nrrd.");
hestOptAdd(&hopt, "max", "value", airTypeDouble, 1, 1, &vmax, "nan",
"Value at high end of histogram. Defaults to highest value "
"found in input nrrd.");
hestOptAdd(&hopt, "rmax", "max radius", airTypeDouble, 1, 1, &rmax, "nan",
"largest radius to include in histogram");
hestOptAdd(&hopt, "c", "center x, y", airTypeDouble, 2, 2, cent, NULL,
"The center point around which to build radial histogram");
hestOptAdd(&hopt, "o", "filename", airTypeString, 1, 1, &outS, "-",
"file to write histogram to");
hestParseOrDie(hopt, argc-1, argv+1, hparm,
me, histradInfo, AIR_TRUE, AIR_TRUE, AIR_TRUE);
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
if (2 != nin->dim) {
fprintf(stderr, "%s: need 2-D input (not %d-D)\n", me, nin->dim);
airMopError(mop); return 1;
}
rbins = bins[0];
hbins = bins[1];
nhist = nrrdNew();
airMopAdd(mop, nhist, (airMopper)nrrdNuke, airMopAlways);
if (nrrdMaybeAlloc_va(nhist, nrrdTypeDouble, 2,
AIR_CAST(size_t, rbins),
AIR_CAST(size_t, hbins))) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't allocate histogram:\n%s", me, err);
airMopError(mop); return 1;
}
if (!( AIR_EXISTS(vmin) && AIR_EXISTS(vmax) )) {
range = nrrdRangeNewSet(nin, nrrdStateBlind8BitRange);
airMopAdd(mop, range, (airMopper)nrrdRangeNix, airMopAlways);
vmin = AIR_EXISTS(vmin) ? vmin : range->min;
vmax = AIR_EXISTS(vmax) ? vmax : range->max;
}
#define DIST(x0, y0, x1, y1) (sqrt((x0-x1)*(x0-x1) + (y0-y1)*(y0-y1)))
sx = nin->axis[0].size;
sy = nin->axis[1].size;
if (!AIR_EXISTS(rmax)) {
rmax = 0;
rmax = AIR_MAX(rmax, DIST(cent[0], cent[1], 0, 0));
rmax = AIR_MAX(rmax, DIST(cent[0], cent[1], sx-1, 0));
rmax = AIR_MAX(rmax, DIST(cent[0], cent[1], 0, sy-1));
rmax = AIR_MAX(rmax, DIST(cent[0], cent[1], sx-1, sy-1));
}
lup = nrrdDLookup[nin->type];
hist = (double*)(nhist->data);
for (xi=0; xi<sx; xi++) {
for (yi=0; yi<sy; yi++) {
rad = DIST(cent[0], cent[1], xi, yi);
if (!AIR_IN_OP(0, rad, rmax)) {
continue;
}
val = lup(nin->data, xi + sx*yi);
if (!AIR_IN_OP(vmin, val, vmax)) {
continue;
}
ridx = airIndex(0, rad, rmax, rbins);
hidx = airIndex(vmin, val, vmax, hbins);
hist[ridx + rbins*hidx] += 1;
}
}
nhist->axis[0].min = 0;
nhist->axis[0].max = rmax;
nhist->axis[1].min = vmin;
nhist->axis[1].max = vmax;
if (nrrdSave(outS, nhist, NULL)) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't save output:\n%s", me, err);
airMopError(mop); return 1;
}
airMopOkay(mop);
exit(0);
}
void src3d(float *time0, float *slow0, int nz, int nx, int ny, float h, float ox, float oy, float oz, int *pxs, int *pys, int *pzs, int *cube)
{
int
srctype=1, /* if 1, source is a point;
2, source is on the walls of the data volume;
3, source on wall, time field known; */
srcwall, /* if 1, source on x=0 wall, if 2, on x=nx-1 wall
if 3, source on y=0 wall, if 4, on y=ny-1 wall
if 5, source on z=0 wall, if 6, on z=nz-1 wall */
xs, /* shot x position (in grid points) */
ys, /* shot y position */
zs, /* shot depth */
xx, yy, zz, /* Used to loop around xs, ys, zs coordinates */
ii, i, j, k,
wfint, ofint,
nxy, nyz, nxz, nxyz, nwall,
NCUBE=2;
float
fxs, /* shot position in X (in real units)*/
fys, /* shot position in Y (in real units)*/
fzs, /* shot position in Z (in real units)*/
*wall,
/* maximum offset (real units) to compute */
/* used in linear velocity gradient cube source */
rx, ry, rz, dvz, dv, v0,
rzc, rxyc, rz1, rxy1, rho, theta1, theta2,
xsrc1, ysrc1, zsrc1;
char
*oldtfile, /* file through which old travel times are input */
*wallfile; /* file containing input wall values of traveltimes */
if(!getparint("NCUBE",&NCUBE)) NCUBE=2;
if(!getparint("srctype",&srctype)) srctype=1;
if(srctype==1) {
if(!getparfloat("xsrc1",&xsrc1)) verr("xsrc1 not given");
if(!getparfloat("ysrc1",&ysrc1)) verr("ysrc1 not given");
if(!getparfloat("zsrc1",&zsrc1)) verr("zsrc1 not given");
fxs = (xsrc1-ox)/h;
fys = (ysrc1-oy)/h;
fzs = (zsrc1-oz)/h;
xs = (int)(fxs + 0.5);
ys = (int)(fys + 0.5);
zs = (int)(fzs + 0.5);
if(xs<2 || ys<2 || zs<2 || xs>nx-3 || ys>ny-3 || zs>nz-3){
vwarn("Source near an edge, beware of traveltime errors");
vwarn("for raypaths that travel parallel to edge ");
vwarn("while wavefronts are strongly curved, (JV, 8/17/88)\n");
}
*pxs = xs; *pys = ys, *pzs = zs, *cube = NCUBE;
}
else if (srctype==2) {
if (!getparint("srcwall",&srcwall)) verr("srcwall not given");
if (!getparstring("wallfile",&wallfile)) verr("wallfile not given");
if((wfint=open(wallfile,O_RDONLY,0664))<=1) {
fprintf(stderr,"cannot open %s\n",wallfile);
exit(-1);
}
}
else if (srctype==3) {
if (!getparint("srcwall",&srcwall)) verr("srcwall not given");
if (!getparstring("oldtfile",&oldtfile)) verr("oldtfile not given");
if((ofint=open(oldtfile,O_RDONLY,0664))<=1) {
fprintf(stderr,"cannot open %s\n",oldtfile);
exit(-1);
}
}
else {
verr("ERROR: incorrect value of srctype");
}
nxy = nx * ny;
nyz = ny * nz;
nxz = nx * nz;
nxyz = nx * ny * nz;
/* SET TIMES TO DUMMY VALUE */
for(i=0;i<nxyz;i++) time0[i] = 1.0e10;
if (srctype == 1) { /* VIDALE'S POINT SOURCE */
/* FILL IN CUBE AROUND SOURCE POINT */
/* HOLE'S NEW LINEAR VELOCITY GRADIENT CUBE (APRIL 1991)*/
v0 = h/s0(xs,ys,zs);
for (xx = xs-NCUBE; xx <= xs+NCUBE; xx++) {
if (xx < 0 || xx >= nx) continue;
for (yy = ys-NCUBE; yy <= ys+NCUBE; yy++) {
if (yy < 0 || yy >= ny) continue;
for (zz = zs-NCUBE; zz <= zs+NCUBE; zz++) {
if (zz < 0 || zz >= nz) continue;
if (zz == zs)
dvz = 1/s0(xx,yy,zz+1)-1/s0(xs,ys,zs);
else
dvz = (1/s0(xx,yy,zz)-1/s0(xs,ys,zs))/(zz-zs);
dv = fabs(dvz);
if (dv == 0.) {
t0(xx,yy,zz) = s0(xs,ys,zs)*DIST(fxs,fys,fzs,xx,yy,zz);
continue;
}
rzc = -v0/dv;
rx = h*(xx - fxs);
ry = h*(yy - fys);
rz = h*(zz - fzs);
rz1 = rz*dvz/dv;
rxy1 = sqrt(rx*rx+ry*ry+rz*rz-rz1*rz1);
if (rxy1<=h/1.e6)
t0(xx,yy,zz) = fabs(log((v0+dv*rz1)/v0)/dv);
else {
rxyc = (rz1*rz1+rxy1*rxy1-2*rz1*rzc)/(2*rxy1);
rho = sqrt(rzc*rzc+rxyc*rxyc);
theta1 = asin(-rzc/rho);
/* can't handle asin(1.) ! */
if (fabs(rz1-rzc)>=rho) rho=1.0000001*fabs(rz1-rzc);
theta2 = asin((rz1-rzc)/rho);
if (rxyc<0) theta1=M_PI-theta1;
if (rxyc<rxy1) theta2=M_PI-theta2;
t0(xx,yy,zz) = log(tan(theta2/2)/tan(theta1/2)) / dv;
}
}
}
}
}
else if (srctype == 2) { /* HOLE'S EXTERNAL SOURCE */
/* FILL IN WALLS' TIMES FROM EXTERNAL DATAFILE */
read (wfint,wall,4*nwall); /* READ X=0 WALL */
if (wall[0]>-1.e-20) {
ii = 0;
for (k=0; k<nz; k++) {
for (j=0; j<ny; j++) {
t0(0,j,k) = wall[ii];
ii++;
}
}
}
read (wfint,wall,4*nwall); /* READ X=NX-1 WALL */
if (wall[0]>-1.e-20) {
ii = 0;
for (k=0; k<nz; k++) {
for (j=0; j<ny; j++) {
t0(nx-1,j,k) = wall[ii];
ii++;
}
}
}
read (wfint,wall,4*nwall); /* READ Y=0 WALL */
if (wall[0]>-1.e-20) {
ii = 0;
for (k=0; k<nz; k++) {
for (i=0; i<nx; i++) {
t0(i,0,k) = wall[ii];
ii++;
}
}
}
read (wfint,wall,4*nwall); /* READ Y=NY-1 WALL */
if (wall[0]>-1.e-20) {
ii = 0;
for (k=0; k<nz; k++) {
for (i=0; i<nx; i++) {
t0(i,ny-1,k) = wall[ii];
ii++;
}
}
}
read (wfint,wall,4*nwall); /* READ Z=0 WALL */
if (wall[0]>-1.e-20) {
ii = 0;
for (j=0; j<ny; j++) {
for (i=0; i<nx; i++) {
t0(i,j,0) = wall[ii];
ii++;
}
}
}
read (wfint,wall,4*nwall); /* READ Z=NZ-1 WALL */
if (wall[0]>-1.e-20) {
ii = 0;
for (j=0; j<ny; j++) {
for (i=0; i<nx; i++) {
t0(i,j,nz-1) = wall[ii];
ii++;
}
}
}
}
else if (srctype == 3) { /* HOLE'S REDO OLD TIMES */
/* READ IN OLD TIME FILE */
if (srctype == 3) read(ofint,time0,nxyz*4);
}
return;
}
void
_pl_x_draw_elliptic_arc (R___(Plotter *_plotter) plPoint p0, plPoint p1, plPoint pc)
{
double radius;
double theta0, theta1;
int startangle, anglerange;
int x_orientation, y_orientation;
int xorigin, yorigin;
unsigned int squaresize_x, squaresize_y;
/* axes flipped? (by default y-axis is, due to X's flipped-y convention) */
x_orientation = (_plotter->drawstate->transform.m[0] >= 0 ? 1 : -1);
y_orientation = (_plotter->drawstate->transform.m[3] >= 0 ? 1 : -1);
/* radius of circular arc in user frame is distance to p0, and also to p1 */
radius = DIST(pc, p0);
/* location of `origin' (upper left corner of bounding rect. on display)
and width and height; X's flipped-y convention affects these values */
xorigin = IROUND(XD(pc.x - x_orientation * radius,
pc.y - y_orientation * radius));
yorigin = IROUND(YD(pc.x - x_orientation * radius,
pc.y - y_orientation * radius));
squaresize_x = (unsigned int)IROUND(XDV(2 * x_orientation * radius, 0.0));
squaresize_y = (unsigned int)IROUND(YDV(0.0, 2 * y_orientation * radius));
theta0 = _xatan2 (-y_orientation * (p0.y - pc.y),
x_orientation * (p0.x - pc.x)) / M_PI;
theta1 = _xatan2 (-y_orientation * (p1.y - pc.y),
x_orientation * (p1.x - pc.x)) / M_PI;
void calc_tidal_forces(struct halo_stash *h, double a1, double a2)
{
int64_t i,j, num_halo_pos=0;
float dt = (scale_to_years(a2) - scale_to_years(a1))/1.0e6; //In Myr
float range, av_a = (a1+a2)/2.0;
float inv_rs, r,m, dx, dy, dz, r3, acc;
float rvir,nearest_range;
struct fast3tree_results *nearest;
struct tree_halo *h1, *h2;
float acorr = 1; //pow(av_a, 0.7);
float conv_const = av_a*av_a / h0 / h0 / h0;
float vel_dt = dt * 1.02268944e-6 * h0 / av_a; //1 km/s to comoving Mpc/Myr/h
struct tree_halo *halos = h->halos;
float max_dist = box_size / 2.0;
float mass_factor;
gen_ff_cache();
if (!tidal_tree) tidal_tree = fast3tree_init(0, NULL);
hpos = check_realloc(hpos, sizeof(struct halo_pos)*h->num_halos, "Allocating tidal halo positions");
nearest = fast3tree_results_init();
vel_dt /= 2.0;
for (i=0; i<h->num_halos; i++) {
halos[i].tidal_force = 0;
halos[i].tidal_id = -1;
halos[i].pid = halos[i].upid = -1;
halos[i].pid_vmax = halos[i].upid_vmax = 0;
if (halos[i].flags & MERGER_FLUCTUATION_FLAG) continue;
hpos[num_halo_pos].h = &(halos[i]);
for (j=0; j<3; j++) {
hpos[num_halo_pos].pos[j] = halos[i].pos[j] + vel_dt*halos[i].vel[j];
IF_PERIODIC {
if (hpos[num_halo_pos].pos[j] > box_size)
hpos[num_halo_pos].pos[j] -= box_size;
else if (hpos[num_halo_pos].pos[j] < 0)
hpos[num_halo_pos].pos[j] += box_size;
}
}
num_halo_pos++;
}
fast3tree_rebuild(tidal_tree, num_halo_pos, hpos);
IF_PERIODIC _fast3tree_set_minmax(tidal_tree, 0, box_size);
//Calculate accelerations
for (i=0; i<num_halo_pos; i++) {
h1 = hpos[i].h;
range = halo_tidal_range(h1->mvir/h0, av_a)*h0; //In comoving Mpc/h
rvir = h1->rvir / 1.0e3; //In comoving Mpc/h
if (range > rvir) nearest_range = range;
else nearest_range = rvir;
IF_PERIODIC {
if (nearest_range > max_dist) nearest_range = max_dist;
fast3tree_find_sphere_periodic(tidal_tree, nearest, hpos[i].pos, nearest_range);
} else {
fast3tree_find_sphere(tidal_tree, nearest, hpos[i].pos, nearest_range);
}
inv_rs = 1000.0/h1->rs; //In comoving h/Mpc
mass_factor = calculate_mass_factor(h1->mvir/h0, h1->rvir, h1->rs);
for (j=0; j<nearest->num_points; j++) {
//Calculate tidal forces
h2 = nearest->points[j]->h;
#ifndef NO_PERIODIC
#define DIST(a,b) a = hpos[i].pos[b] - nearest->points[j]->pos[b]; \
if (a > max_dist) a-=box_size; \
else if (a < -max_dist) a+=box_size;
#else
#define DIST(a,b) a = hpos[i].pos[b] - nearest->points[j]->pos[b];
#endif
DIST(dx,0);
DIST(dy,1);
DIST(dz,2);
#undef DIST
r = sqrtf(dx*dx + dy*dy + dz*dz); // in comoving Mpc/h
//Including acorr slightly improves velocity results
// as a function of redshift.
m = mass_factor*ff_cached(r*inv_rs*acorr); //In Msun
r3 = r * r * r * conv_const; //in (real Mpc)^3
acc = r3 ? Gc*m/r3 : 0; //In km/s / Myr / (real Mpc)
if (r < rvir && h1->vmax > h2->vmax) {
if (h2->pid < 0) {
h2->pid = h2->upid = h1->id;
h2->pid_vmax = h2->upid_vmax = h1->vmax;
}
else {
if (h2->pid_vmax > h1->vmax) {
h2->pid = h1->id;
h2->pid_vmax = h1->vmax;
}
if (h2->upid_vmax < h1->vmax) {
h2->upid = h1->id;
h2->upid_vmax = h1->vmax;
}
}
}
if ((acc > h2->tidal_force) && ((h2->mvir*MAJOR_MERGER) < h1->mvir)
&& (!(h2->phantom))) {
h2->tidal_force = acc;
h2->tidal_id = h1->id;
}
}
}
bool pose_dist_comp(geometry_msgs::Pose& p1, geometry_msgs::Pose& p2, geometry_msgs::Point& app_pt) {
return DIST(p1.position.x, p1.position.y, app_pt.x, app_pt.y) < DIST(p2.position.x, p2.position.y, app_pt.x, app_pt.y);
}
void bullet_update(struct moag *m, int id)
{
struct bullet *b = &m->bullets[id];
if (!b->active)
return;
if (b->type == LADDER)
{
b->active--;
b->obj.pos = VEC2_ADD(b->obj.pos, b->obj.vel);
b->x = (int)b->obj.pos.x;
b->y = (int)b->obj.pos.y;
if (get_land_at(m, b->x, b->y) == 1)
{
explode(m, b->x, b->y + LADDER_LENGTH - b->active, 1, E_SAFE_EXPLODE);
bullet_detonate(m, id);
}
return;
}
b->obj.pos = VEC2_ADD(b->obj.pos, b->obj.vel);
b->obj.vel = VEC2_ADD(b->obj.vel, VEC2(0, GRAVITY));
b->x = (int)b->obj.pos.x;
b->y = (int)b->obj.pos.y;
if (get_land_at(m, b->x, b->y))
{
bullet_detonate(m, id);
return;
}
if (b->active > 1)
{
b->active--;
return;
}
if (b->type == BOUNCER && b->active == 1)
b->active = -BOUNCER_BOUNCES;
if (b->type == TUNNELER && b->active == 1)
b->active = -TUNNELER_TUNNELINGS;
for (int i = 0; i < MAX_PLAYERS; i++)
{
if(DIST(m->players[i].tank.x, m->players[i].tank.y - 3,
b->x, b->y) < 8.5)
{
bullet_detonate(m, id);
return;
}
}
if (m->crate.active && DIST(m->crate.x, m->crate.y - 4, b->x, b->y) < 5.5)
{
if (m->crate.type == TRIPLER) {
float angle = -RAD2DEG(atan2(b->obj.vel.y, b->obj.vel.x));
float speed = VEC2_MAG(b->obj.vel);
fire_bullet_ang(m, b->type, b->x, b->y, angle - 20.0, speed);
fire_bullet_ang(m, b->type, b->x, b->y, angle + 20.0, speed);
} else if (m->crate.type == SHOTGUN) {
bullet_detonate(m, id);
float angle = -RAD2DEG(atan2(b->obj.vel.y, b->obj.vel.x));
float speed = VEC2_MAG(b->obj.vel);
int shots = SHOTGUN_PELLETS;
for (int i = 0; i < shots; i++)
fire_bullet_ang(m, m->crate.type, m->crate.x, m->crate.y - 4,
angle - (shots-1)*2 + i*4, speed*0.5);
} else {
bullet_detonate(m, id);
fire_bullet(m, m->crate.type, m->crate.x, m->crate.y - 4,
m->crate.type != BOUNCER ? 0 :
b->obj.vel.x < 0 ? -0.2 :
0.2, -0.2);
}
m->crate.active = false;
return;
}
if (b->type == MIRV && b->obj.vel.y > 0)
{
bullet_detonate(m, id);
return;
}
if (b->active)
broadcast_bullet_chunk(m, MOVE, id);
}
static void
rescale_layout_polarFocus(v_data * graph, int n,
double *x_coords, double *y_coords,
double x_focus, double y_focus, int interval, double distortion)
{
// Polar distortion - auxiliary function
int i;
double *densities = NULL, *smoothed_densities = NULL;
double *distances = N_NEW(n, double);
double *orig_distances = N_NEW(n, double);
int *ordering;
double ratio;
for (i = 0; i < n; i++)
{
distances[i] = DIST(x_coords[i], y_coords[i], x_focus, y_focus);
}
cpvec(orig_distances, 0, n - 1, distances);
ordering = N_NEW(n, int);
for (i = 0; i < n; i++)
{
ordering[i] = i;
}
quicksort_place(distances, ordering, 0, n - 1);
densities = compute_densities(graph, n, x_coords, y_coords);
smoothed_densities = smooth_vec(densities, ordering, n, interval, smoothed_densities);
// rescale distances
if (distortion < 1.01 && distortion > 0.99)
{
for (i = 1; i < n; i++)
{
distances[ordering[i]] = distances[ordering[i - 1]] + (orig_distances[ordering[i]] -
orig_distances[ordering
[i -
1]]) / smoothed_densities[ordering[i]];
}
} else
{
double factor;
// just to make milder behavior:
if (distortion >= 0)
{
factor = sqrt(distortion);
}
else
{
factor = -sqrt(-distortion);
}
for (i = 1; i < n; i++)
{
distances[ordering[i]] =
distances[ordering[i - 1]] + (orig_distances[ordering[i]] -
orig_distances[ordering
[i -
1]]) /
pow(smoothed_densities[ordering[i]], factor);
}
}
// compute new coordinate:
for (i = 0; i < n; i++)
{
if (orig_distances[i] == 0)
{
ratio = 0;
}
else
{
ratio = distances[i] / orig_distances[i];
}
x_coords[i] = x_focus + (x_coords[i] - x_focus) * ratio;
y_coords[i] = y_focus + (y_coords[i] - y_focus) * ratio;
}
free(densities);
free(smoothed_densities);
free(distances);
free(orig_distances);
free(ordering);
}
void
SimWingUpdate(tCar *car, int index, tSituation* s)
{
tWing *wing = &(car->wing[index]);
tdble vt2 = car->DynGC.vel.x;
tdble i_flow = 1.0;
// rear wing should not get any flow.
// compute angle of attack
// we don't add ay anymore since DynGC.vel.x,z are now in the correct frame of reference (see car.cpp)
tdble aoa = atan2(car->DynGC.vel.z, car->DynGC.vel.x); //+ car->DynGC.pos.ay;
// The flow to the rear wing can get cut off at large negative
// angles of attack. (so it won't produce lift because it will be
// completely shielded by the car's bottom)
// The value -0.4 should depend on the positioning of the wing.
// we also make this be like that.
if (index==1) {
i_flow = PartialFlowSmooth (-0.4, aoa);
}
// Flow to the wings gets cut off by other cars.
tdble airSpeed = car->DynGC.vel.x;
if (airSpeed > 10.0) {
tdble yaw = car->DynGC.pos.az;
tdble x = car->DynGC.pos.x + cos(yaw)*wing->staticPos.x;
tdble y = car->DynGC.pos.y + sin(yaw)*wing->staticPos.x;
tdble spdang = atan2(car->DynGCg.vel.y, car->DynGCg.vel.x);
int i;
for (i = 0; i < s->_ncars; i++) {
if (i == car->carElt->index) {
continue;
}
tdble tmpas = 1.00;
tCar* otherCar = &(SimCarTable[i]);
tdble otherYaw = otherCar->DynGC.pos.az;
tdble tmpsdpang = spdang - atan2(y - otherCar->DynGC.pos.y, x - otherCar->DynGC.pos.x);
NORM_PI_PI(tmpsdpang);
tdble dyaw = yaw - otherYaw;
NORM_PI_PI(dyaw);
if ((otherCar->DynGC.vel.x > 10.0) &&
(fabs(dyaw) < 0.1396)) {
if (fabs(tmpsdpang) > 2.9671) { /* 10 degrees */
/* behind another car - reduce overall airflow */
tdble factor = (fabs(tmpsdpang)-2.9671)/(M_PI-2.9671);
tmpas = 1.0 - factor*exp(- 2.0 * DIST(x, y, otherCar->DynGC.pos.x, otherCar->DynGC.pos.y) /
(otherCar->aero.Cd * otherCar->DynGC.vel.x));
i_flow = i_flow * tmpas;
}
}
}
}
//if (index==1) { -- thrown away so that we have different downforce
// reduction for front and rear parts.
if (1) {
// downforce due to body and ground effect.
tdble alpha = 0.0f;
tdble vt2b = vt2 * (alpha+(1-alpha)*i_flow);
vt2b = vt2b * vt2b;
tdble hm = 1.5 * (car->wheel[0].rideHeight + car->wheel[1].rideHeight + car->wheel[2].rideHeight + car->wheel[3].rideHeight);
hm = hm*hm;
hm = hm*hm;
hm = 1.0 + exp(-3.0*hm);
car->aero.lift[index] = - car->aero.Clift[index] * vt2b * hm;
//car->aero.lift[1] = - car->aero.Clift[1] * vt2b * hm;
//printf ("%f\n", car->aero.lift[0]+car->aero.lift[1]);
}
vt2=vt2*i_flow;
vt2=vt2*vt2;
aoa += wing->angle;
// the sinus of the angle of attack
tdble sinaoa = sin(aoa);
tdble cosaoa = cos(aoa);
if (car->DynGC.vel.x > 0.0f) {
switch (car->options->aeroflow_model) {
case SIMPLE:
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * sinaoa;
wing->forces.z = wing->Kz * vt2 * sinaoa;
break;
case PLANAR:
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * sinaoa * sinaoa * sinaoa;
wing->forces.z = wing->Kz * vt2 * sinaoa * sinaoa * cosaoa;
break;
case OPTIMAL:
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * (1.0f - cosaoa);
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * sinaoa;
break;
default:
fprintf (stderr, "Unimplemented option %d for aeroflow model\n", car->options->aeroflow_model);
}
} else {
wing->forces.x = wing->forces.z = 0.0f;
}
}
void Mutation::__mutateAddNode(Module *m, double probability, double max)
{
if(m->e_size() == 0) return;
if(Random::unit() >= probability) return;
VLOG(50) << ">>>>> add node";
LOG_MODULE;
VLOG(50) << " number of edges: " << m->e_size();
VLOG(50) << " will add one node";
m->setModified(true);
double probabilities[m->e_size()];
double sum = 0;
for(int i = 0; i < m->e_size(); i++)
{
double d = DIST(m->edge(i)->sourceNode()->position(),
m->edge(i)->destinationNode()->position());
sum += d;
probabilities[i] = d;
}
if(sum < 0.000001) return; // only self connections so far
for(int i = 0; i < m->e_size(); i++) probabilities[i] /= sum;
// if(VLOG_IS_ON(50))
// {
// cout << "probabilities:";
// for(int i = 0; i < m->e_size(); i++) cout << " " << probabilities[i];
// cout << endl;
// }
double p = Random::unit();
double s = 0.0;
int ei = -1;
for(int i = 0; i < m->e_size(); i++)
{
s += probabilities[i];
if(p <= s)
{
ei = i;
break;
}
}
VLOG(50) << " edge index into which a neuron will be inserted: " << ei;
Node *n = new Node(NULL);
Edge *e = m->edge(ei);
Node *src = e->sourceNode();
Node *dst = e->destinationNode();
P3D sp = src->position();
P3D dp = dst->position();
P3D np = (sp + dp) * 0.5;
stringstream oss;
bool found = true;
int newNodeIndex = -1;
while(found == true)
{
newNodeIndex++;
oss.str("");
oss << "hidden " << newNodeIndex;
found = m->nodeExists(oss.str());
}
VLOG(50) << "new node name: " << oss.str();
n->setType("hidden");
n->setPosition(np);
n->setBias(Random::rand(-max, max));
n->setLabel(oss.str());
n->setTransferfunction("tanh");
e->setSourceNode(n);
m->addNode(n);
Edge *ne = m->addEdge(src, n, 1.0);
LOG_MODULE;
VLOG(50) << " node added with label " << n->label();
VLOG(50) << " adding edge from: " << src->label() << " " << n->label();
VLOG(50) << " source neuron's position " << src->position();
VLOG(50) << " destination neuron's position " << dst->position();
VLOG(50) << " new neuron's position " << n->position();
VLOG(50) << " new synapse goes from " << e->sourceNode()->label() << " -> "
<< e->destinationNode()->label() << " with " << e->weight();
VLOG(50) << " new synapse goes from " << ne->sourceNode()->label() << " -> "
<< ne->destinationNode()->label() << " with " << ne->weight();
VLOG(50) << "<<<<< add node";
}
/* addEndpoint:
* Add node to graph representing spline end point p inside obstruction obs_id.
* For each side of obstruction, add edge from p to corresponding triangle.
* The node id of the new node in the graph is v_id.
* If p lies on the side of its node (sides != 0), we limit the triangles
* to those within 45 degrees of each side of the natural direction of p.
*/
static void addEndpoint(router_t * rtr, pointf p, node_t* v, int v_id, int sides)
{
int obs_id = ND_lim(v);
int starti = rtr->obs[obs_id];
int endi = rtr->obs[obs_id + 1];
pointf* pts = rtr->ps;
int i, t;
double d;
pointf vr, v0, v1;
switch (sides) {
case TOP :
vr = add_pointf (p, north);
v0 = add_pointf (p, northwest);
v1 = add_pointf (p, northeast);
break;
case TOP|RIGHT :
vr = add_pointf (p, northeast);
v0 = add_pointf (p, north);
v1 = add_pointf (p, east);
break;
case RIGHT :
vr = add_pointf (p, east);
v0 = add_pointf (p, northeast);
v1 = add_pointf (p, southeast);
break;
case BOTTOM|RIGHT :
vr = add_pointf (p, southeast);
v0 = add_pointf (p, east);
v1 = add_pointf (p, south);
break;
case BOTTOM :
vr = add_pointf (p, south);
v0 = add_pointf (p, southeast);
v1 = add_pointf (p, southwest);
break;
case BOTTOM|LEFT :
vr = add_pointf (p, southwest);
v0 = add_pointf (p, south);
v1 = add_pointf (p, west);
break;
case LEFT :
vr = add_pointf (p, west);
v0 = add_pointf (p, southwest);
v1 = add_pointf (p, northwest);
break;
case TOP|LEFT :
vr = add_pointf (p, northwest);
v0 = add_pointf (p, west);
v1 = add_pointf (p, north);
break;
case 0 :
break;
default :
assert (0);
break;
}
rtr->tg->nodes[v_id].ne = 0;
rtr->tg->nodes[v_id].ctr = p;
for (i = starti; i < endi; i++) {
ipair seg;
seg.i = i;
if (i < endi - 1)
seg.j = i + 1;
else
seg.j = starti;
t = findMap(rtr->trimap, seg.i, seg.j);
if (sides && !inCone (v0, p, v1, pts[seg.i]) && !inCone (v0, p, v1, pts[seg.j]) && !raySeg(p,vr,pts[seg.i],pts[seg.j]))
continue;
d = DIST(p, (rtr->tg->nodes + t)->ctr);
addTriEdge(rtr->tg, v_id, t, d, seg);
}
}
void Mutation::__mutateAddEdge(Module *m,
double probability,
double max,
double minDist)
{
VLOG(50) << ">>>>> add edge";
LOG_MODULE;
CfgMutationEdge *dee = Data::instance()->specification()->mutation()->edge();
double probabilities[m->n_size()][m->n_size()];
double d = 0.0;
for(int s_index = 0; s_index < m->n_size(); s_index++)
{
for(int d_index = 0; d_index < m->n_size(); d_index++)
{
probabilities[s_index][d_index] = 0.0;
}
}
for(int s_index = 0; s_index < m->n_size(); s_index++)
{
Node *src_node = m->node(s_index);
if(src_node->isInactive()) continue;
if(src_node->isSource())
{
for(int d_index = 0; d_index < m->n_size(); d_index++)
{
Node *dst_node = m->node(d_index);
if(dst_node->isInactive()) continue;
// if(src_node->type() == TAG_CONNECTOR && dst_node->type() == TAG_CONNECTOR)
// {
// continue;
// }
if(dst_node->isDestination())
{
if(dst_node->contains(src_node) == false)
{
// USE FIXED PROBABILITY FOR ALL EDGES
d = MAX(minDist, DIST(src_node->position(), dst_node->position()));
if(d < MIN_DIST) d = 0;
// probabilities[s_index][d_index] = exp(-d);
if(dee->mode() == EDGE_ADD_MODE_UNIFORM)
{
probabilities[s_index][d_index] = 1.0;
}
else
{
probabilities[s_index][d_index] = 1.0/d;
}
VLOG(50) << " edge from "
<< src_node->label() << " to "
<< dst_node->label() << " does not exist. setting distance to " << d;
// probabilities[s_index][d_index] = 1.0;
}
}
}
}
}
double pmax = 0.0;
if(dee->mode() == EDGE_ADD_MODE_DISTANCE)
{
for(int s_index = 0; s_index < m->n_size(); s_index++)
{
for(int d_index = 0; d_index < m->n_size(); d_index++)
{
if(pmax < probabilities[s_index][d_index]) pmax = probabilities[s_index][d_index];
}
}
}
if(pmax > 0.0)
{
for(int s_index = 0; s_index < m->n_size(); s_index++)
{
for(int d_index = 0; d_index < m->n_size(); d_index++)
{
// cout << probabilities[s_index][d_index] << " -> ";
probabilities[s_index][d_index] = probabilities[s_index][d_index] / pmax;
// cout << probabilities[s_index][d_index] << endl;
}
}
}
if(VLOG_IS_ON(50))
{
stringstream sst;
sst << " probabilities " << m->n_size() << "x" << m->n_size();
sst.precision(3);
sst.setf(ios::fixed,ios::floatfield);
sst << "[ ";
for(int s_index = 0; s_index < m->n_size(); s_index++)
{
sst << " [ " << probabilities[s_index][0];
for(int d_index = 0; d_index < m->n_size(); d_index++)
{
sst << ", " << probabilities[s_index][d_index];
}
sst << " ]";
}
sst << " ]";
VLOG(50) << sst.str();
}
// double p = Random::unit();
// VLOG(50) << " p = " << p;
for(int s_index = 0; s_index < m->n_size(); s_index++)
{
for(int d_index = 0; d_index < m->n_size(); d_index++)
{
double p = probabilities[s_index][d_index] * probability;
if(Random::unit() <= p)
{
Node *src = m->node(s_index);
Node *dst = m->node(d_index);
VLOG(50) << " adding edge from " << src->label() << " -> " << dst->label();
VLOG(50) << " before number of edges: " << m->e_size();
Edge *e = m->addEdge(src, dst, Random::rand(-max, max));
VLOG(50) << " adding edge from "
<< m->node(s_index)->label() << " to "
<< m->node(d_index)->label() << " with "
<< e->weight();
VLOG(50) << " after number of edges: " << m->e_size();
LOG_MODULE;
VLOG(50) << "<<<<< add edge";
m->setModified(true);
}
}
}
LOG_MODULE;
VLOG(50) << "<<<<< add edge";
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double *xgrad, *ygrad, *boundary, *dist;
double dxp, dxm, dyp, dym, xns, yns, nrm;
int m, n, i, j, nn;
/* Check number of outputs */
if (nlhs < 2)
mexErrMsgTxt("At least 2 output argument needed.");
else if (nlhs > 2)
mexErrMsgTxt("At most 2 output argument needed.");
/* Get inputs */
if (nrhs < 2)
mexErrMsgTxt("At least 2 input argument needed.");
else if (nrhs > 2)
mexErrMsgTxt("At most 2 input argument used.");
/* Get boundary */
if (!mxIsDouble(prhs[0]) || mxIsClass(prhs[0], "sparse"))
mexErrMsgTxt("Boundary field needs to be a full double precision matrix.");
boundary = mxGetPr(prhs[0]);
m = mxGetM(prhs[0]);
n = mxGetN(prhs[0]);
/* Get distance field */
if (!mxIsDouble(prhs[1]) || mxIsClass(prhs[1], "sparse") || mxGetM(prhs[1]) != m || mxGetN(prhs[1]) != n)
mexErrMsgTxt("Distance field needs to be a full double precision matrix with same dimension as the boundary.");
dist = mxGetPr(prhs[1]);
m = mxGetM(prhs[1]);
n = mxGetN(prhs[1]);
/* create and init output (gradient) matrices */
plhs[0] = mxCreateDoubleMatrix(m, n, mxREAL);
plhs[1] = mxCreateDoubleMatrix(m, n, mxREAL);
xgrad = mxGetPr(plhs[0]);
ygrad = mxGetPr(plhs[1]);
for (j = 0; j < n; ++j)
for (i = 0; i < m; ++i)
if (INTERIOR(i,j))
{
if (i > 0)
dxm = INTERIOR(i-1,j) ? DIST(i-1,j) : DIST(i,j);
else
dxm = DIST(i,j);
if (i < m-1)
dxp = INTERIOR(i+1,j) ? DIST(i+1,j) : DIST(i,j);
else
dxp = DIST(i,j);
if (j > 0)
dym = INTERIOR(i,j-1) ? DIST(i,j-1) : DIST(i,j);
else
dym = DIST(i,j);
if (j < n-1)
dyp = INTERIOR(i,j+1) ? DIST(i,j+1) : DIST(i,j);
else
dyp = DIST(i,j);
XGRAD(i, j) = (dxp - dxm) / 2.0;
YGRAD(i, j) = (dyp - dym) / 2.0;
nrm = sqrt(XGRAD(i, j)*XGRAD(i, j) + YGRAD(i, j)*YGRAD(i, j));
if (nrm > 1e-12)
{
XGRAD(i, j) /= nrm;
YGRAD(i, j) /= nrm;
}
}
else
{
XGRAD(i, j) = 0.0;
YGRAD(i, j) = 0.0;
}
for (j = 0; j < n; ++j)
for (i = 0; i < m; ++i)
if (!INTERIOR(i, j))
{
xns = 0.0;
yns = 0.0;
nn = 0;
if (i > 0 && INTERIOR(i-1,j))
{
xns += XGRAD(i-1,j);
yns += YGRAD(i-1,j);
++nn;
}
if (i < m-1 && INTERIOR(i+1,j))
{
xns += XGRAD(i+1,j);
yns += YGRAD(i+1,j);
++nn;
}
if (j > 0 && INTERIOR(i,j-1))
{
xns += XGRAD(i,j-1);
yns += YGRAD(i,j-1);
++nn;
}
if (j < n-1 && INTERIOR(i,j+1))
{
xns += XGRAD(i,j+1);
yns += YGRAD(i,j+1);
++nn;
}
if (nn > 0)
{
XGRAD(i, j) = xns / nn;
YGRAD(i, j) = yns / nn;
}
}
}
static void
RemoveCar(tCar *car, tSituation *s)
{
int i;
tCarElt *carElt;
tTrkLocPos trkPos;
int trkFlag;
tdble travelTime;
tdble dang;
static tdble PULL_Z_OFFSET = 3.0;
static tdble PULL_SPD = 0.5;
carElt = car->carElt;
if (carElt->_state & RM_CAR_STATE_PULLUP) {
carElt->_pos_Z += car->restPos.vel.z * SimDeltaTime;
carElt->_yaw += car->restPos.vel.az * SimDeltaTime;
carElt->_roll += car->restPos.vel.ax * SimDeltaTime;
carElt->_pitch += car->restPos.vel.ay * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
(float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch));
if (carElt->_pos_Z > (car->restPos.pos.z + PULL_Z_OFFSET)) {
carElt->_state &= ~RM_CAR_STATE_PULLUP;
carElt->_state |= RM_CAR_STATE_PULLSIDE;
// Moved pullside velocity computation down due to floating point error accumulation.
}
return;
}
if (carElt->_state & RM_CAR_STATE_PULLSIDE) {
// Recompute speed to avoid missing the parking point due to error accumulation (the pos might be
// in the 0-10000 range, depending on the track and vel*dt is around 0-0.001, so basically all
// but the most significant digits are lost under bad conditions, happens e.g on e-track-4).
// Should not lead to a division by zero because the pullside process stops if the car is within
// [0.5, 0.5]. Do not move it back.
travelTime = DIST(car->restPos.pos.x, car->restPos.pos.y, carElt->_pos_X, carElt->_pos_Y) / PULL_SPD;
car->restPos.vel.x = (car->restPos.pos.x - carElt->_pos_X) / travelTime;
car->restPos.vel.y = (car->restPos.pos.y - carElt->_pos_Y) / travelTime;
carElt->_pos_X += car->restPos.vel.x * SimDeltaTime;
carElt->_pos_Y += car->restPos.vel.y * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
(float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch));
if ((fabs(car->restPos.pos.x - carElt->_pos_X) < 0.5) && (fabs(car->restPos.pos.y - carElt->_pos_Y) < 0.5)) {
carElt->_state &= ~RM_CAR_STATE_PULLSIDE;
carElt->_state |= RM_CAR_STATE_PULLDN;
}
return;
}
if (carElt->_state & RM_CAR_STATE_PULLDN) {
carElt->_pos_Z -= car->restPos.vel.z * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
(float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch));
if (carElt->_pos_Z < car->restPos.pos.z) {
carElt->_state &= ~RM_CAR_STATE_PULLDN;
carElt->_state |= RM_CAR_STATE_OUT;
}
return;
}
if (carElt->_state & (RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT)) {
return;
}
if (carElt->_state & RM_CAR_STATE_PIT) {
if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) {
// Broken during pit stop.
carElt->_state &= ~RM_CAR_STATE_PIT;
carElt->_pit->pitCarIndex = TR_PIT_STATE_FREE;
} else {
return;
}
}
if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) {
carElt->_state |= RM_CAR_STATE_BROKEN;
} else {
carElt->_state |= RM_CAR_STATE_OUTOFGAS;
}
carElt->_gear = car->transmission.gearbox.gear = 0;
carElt->_enginerpm = car->engine.rads = 0;
if (!(carElt->_state & RM_CAR_STATE_DNF)) {
if (fabs(carElt->_speed_x) > 1.0) {
return;
}
}
carElt->_state |= RM_CAR_STATE_PULLUP;
// RM_CAR_STATE_NO_SIMU evaluates to > 0 from here, so we remove the car from the
// collision detection.
SimCollideRemoveCar(car, s->_ncars);
carElt->priv.collision = car->collision = 0;
for(i = 0; i < 4; i++) {
carElt->_skid[i] = 0;
carElt->_wheelSpinVel(i) = 0;
carElt->_brakeTemp(i) = 0;
}
carElt->pub.DynGC = car->DynGC;
carElt->_speed_x = 0;
// Compute the target zone for the wrecked car.
trkPos = car->trkPos;
if (trkPos.toRight > trkPos.seg->width / 2.0) {
while (trkPos.seg->lside != 0) {
trkPos.seg = trkPos.seg->lside;
}
trkPos.toLeft = -3.0;
trkFlag = TR_TOLEFT;
} else {
while (trkPos.seg->rside != 0) {
trkPos.seg = trkPos.seg->rside;
}
trkPos.toRight = -3.0;
trkFlag = TR_TORIGHT;
}
trkPos.type = TR_LPOS_SEGMENT;
RtTrackLocal2Global(&trkPos, &(car->restPos.pos.x), &(car->restPos.pos.y), trkFlag);
car->restPos.pos.z = RtTrackHeightL(&trkPos) + carElt->_statGC_z;
car->restPos.pos.az = RtTrackSideTgAngleL(&trkPos);
car->restPos.pos.ax = 0;
car->restPos.pos.ay = 0;
car->restPos.vel.z = PULL_SPD;
travelTime = (car->restPos.pos.z + PULL_Z_OFFSET - carElt->_pos_Z) / car->restPos.vel.z;
dang = car->restPos.pos.az - carElt->_yaw;
FLOAT_NORM_PI_PI(dang);
car->restPos.vel.az = dang / travelTime;
dang = car->restPos.pos.ax - carElt->_roll;
FLOAT_NORM_PI_PI(dang);
car->restPos.vel.ax = dang / travelTime;
dang = car->restPos.pos.ay - carElt->_pitch;
FLOAT_NORM_PI_PI(dang);
car->restPos.vel.ay = dang / travelTime;
}
void
SimAeroUpdate(tCar *car, tSituation *s)
{
tdble hm;
int i;
tCar *otherCar;
tdble x, y;
tdble yaw, otherYaw, airSpeed, tmpas, spdang, tmpsdpang, dyaw;
tdble dragK = 1.0;
x = car->DynGCg.pos.x;
y = car->DynGCg.pos.y;
yaw = car->DynGCg.pos.az;
airSpeed = car->DynGC.vel.x;
spdang = atan2(car->DynGCg.vel.y, car->DynGCg.vel.x);
if (airSpeed > 10.0) {
for (i = 0; i < s->_ncars; i++) {
if (i == car->carElt->index) {
continue;
}
otherCar = &(SimCarTable[i]);
otherYaw = otherCar->DynGCg.pos.az;
tmpsdpang = spdang - atan2(y - otherCar->DynGCg.pos.y, x - otherCar->DynGCg.pos.x);
FLOAT_NORM_PI_PI(tmpsdpang);
dyaw = yaw - otherYaw;
FLOAT_NORM_PI_PI(dyaw);
if ((otherCar->DynGC.vel.x > 10.0) &&
(fabs(dyaw) < 0.1396)) {
if (fabs(tmpsdpang) > 2.9671) { /* 10 degrees */
/* behind another car */
tmpas = (tdble) (1.0 - exp(- 2.0 * DIST(x, y, otherCar->DynGCg.pos.x, otherCar->DynGCg.pos.y) /
(otherCar->aero.Cd * otherCar->DynGC.vel.x)));
if (tmpas < dragK) {
dragK = tmpas;
}
} else if (fabs(tmpsdpang) < 0.1396) { /* 8 degrees */
/* before another car [not sure how much the drag should be reduced in this case. In no case it should be lowered more than 50% I think. - Christos] */
tmpas = (tdble) (1.0 - 0.5f * exp(- 8.0 * DIST(x, y, otherCar->DynGCg.pos.x, otherCar->DynGCg.pos.y) / (car->aero.Cd * car->DynGC.vel.x)));
if (tmpas < dragK) {
dragK = tmpas;
}
}
}
}
}
car->airSpeed2 = airSpeed * airSpeed;
tdble v2 = car->airSpeed2;
// simulate ground effect drop off caused by non-frontal airflow (diffusor stops working etc.)
// Never used : remove ?
//tdble speed = sqrt(car->DynGC.vel.x*car->DynGC.vel.x + car->DynGC.vel.y*car->DynGC.vel.y);
//tdble cosa = 1.0f;
car->aero.drag = (tdble) (-SIGN(car->DynGC.vel.x) * car->aero.SCx2 * v2 * (1.0f + (tdble)car->dammage / 10000.0f) * dragK * dragK);
hm = 1.5f * (car->wheel[0].rideHeight + car->wheel[1].rideHeight + car->wheel[2].rideHeight + car->wheel[3].rideHeight);
hm = hm*hm;
hm = hm*hm;
hm = 2 * exp(-3.0f*hm);
car->aero.lift[0] = - car->aero.Clift[0] * v2 * hm;
car->aero.lift[1] = - car->aero.Clift[1] * v2 * hm;
}
void
SimAeroUpdate(tCar *car, tSituation *s)
{
//tdble hm;
int i;
tdble airSpeed;
tdble dragK = 1.0;
airSpeed = car->DynGC.vel.x;
if (airSpeed > 10.0) {
tdble x = car->DynGC.pos.x;
tdble y = car->DynGC.pos.y;
// tdble x = car->DynGC.pos.x + cos(yaw)*wing->staticPos.x;
// tdble y = car->DynGC.pos.y + sin(yaw)*wing->staticPos.x;
tdble yaw = car->DynGC.pos.az;
tdble spdang = atan2(car->DynGCg.vel.y, car->DynGCg.vel.x);
for (i = 0; i < s->_ncars; i++) {
if (i == car->carElt->index) {
continue;
}
tdble tmpas = 1.00;
tCar* otherCar = &(SimCarTable[i]);
tdble otherYaw = otherCar->DynGC.pos.az;
tdble tmpsdpang = spdang - atan2(y - otherCar->DynGC.pos.y, x - otherCar->DynGC.pos.x);
NORM_PI_PI(tmpsdpang);
tdble dyaw = yaw - otherYaw;
NORM_PI_PI(dyaw);
if ((otherCar->DynGC.vel.x > 10.0) &&
(fabs(dyaw) < 0.1396)) {
if (fabs(tmpsdpang) > 2.9671) { /* 10 degrees */
/* behind another car - reduce overall airflow */
tdble factor = (fabs(tmpsdpang)-2.9671)/(M_PI-2.9671);
tmpas = 1.0 - factor * exp(- 2.0 * DIST(x, y, otherCar->DynGC.pos.x, otherCar->DynGC.pos.y)/(otherCar->aero.Cd * otherCar->DynGC.vel.x));
airSpeed = airSpeed * tmpas;
} else if (fabs(tmpsdpang) < 0.1396f) { /* 8 degrees */
tdble factor = 0.5f * (0.1396f-fabs(tmpsdpang))/(0.1396f);
/* before another car - breaks down rear eddies, reduces only drag*/
tmpas = 1.0f - factor * exp(- 8.0 * DIST(x, y, otherCar->DynGC.pos.x, otherCar->DynGC.pos.y) / (car->aero.Cd * car->DynGC.vel.x));
dragK = dragK * tmpas;
}
}
}
}
car->airSpeed2 = airSpeed * airSpeed;
tdble v2 = car->airSpeed2;
tdble dmg_coef = ((tdble)car->dammage / 10000.0);
car->aero.drag = -SIGN(car->DynGC.vel.x) * car->aero.SCx2 * v2 * (1.0 + dmg_coef) * dragK * dragK;
// Since we have the forces ready, we just multiply.
// Should insert constants here.
// Also, no torque is produced since the effect can be
// quite dramatic. Interesting idea to make all drags produce
// torque when the car is damaged.
car->aero.Mx = car->aero.drag * dmg_coef * car->aero.rot_front[0];
car->aero.My = car->aero.drag * dmg_coef * car->aero.rot_front[1];
car->aero.Mz = car->aero.drag * dmg_coef * car->aero.rot_front[2];
v2 = car->DynGC.vel.y;
car->aero.lateral_drag = -SIGN(v2)*v2*v2*0.7;
car->aero.Mx += car->aero.lateral_drag * dmg_coef * car->aero.rot_lateral[0];
car->aero.My += car->aero.lateral_drag * dmg_coef * car->aero.rot_lateral[1];
car->aero.Mz += car->aero.lateral_drag * dmg_coef * car->aero.rot_lateral[2];
v2 = car->DynGC.vel.z;
car->aero.vertical_drag = -SIGN(v2)*v2*v2*1.5;
car->aero.Mx += car->aero.vertical_drag * dmg_coef * car->aero.rot_vertical[0];
car->aero.My += car->aero.vertical_drag * dmg_coef * car->aero.rot_vertical[1];
car->aero.Mz += car->aero.vertical_drag * dmg_coef * car->aero.rot_vertical[2];
}
static uint32_t
calc_edit_distance(grn_ctx *ctx, char *sx, char *ex, char *sy, char *ey, int flags)
{
int d = 0;
uint32_t cx, lx, cy, ly, *dists;
char *px, *py;
for (px = sx, lx = 0; px < ex && (cx = grn_charlen(ctx, px, ex)); px += cx, lx++);
for (py = sy, ly = 0; py < ey && (cy = grn_charlen(ctx, py, ey)); py += cy, ly++);
if ((dists = GRN_PLUGIN_MALLOC(ctx, (lx + 1) * (ly + 1) * sizeof(uint32_t)))) {
uint32_t x, y;
for (x = 0; x <= lx; x++) { DIST(x, 0) = x; }
for (y = 0; y <= ly; y++) { DIST(0, y) = y; }
for (x = 1, px = sx; x <= lx; x++, px += cx) {
cx = grn_charlen(ctx, px, ex);
for (y = 1, py = sy; y <= ly; y++, py += cy) {
cy = grn_charlen(ctx, py, ey);
if (cx == cy && !memcmp(px, py, cx)) {
DIST(x, y) = DIST(x - 1, y - 1);
} else {
uint32_t a = DIST(x - 1, y) + 1;
uint32_t b = DIST(x, y - 1) + 1;
uint32_t c = DIST(x - 1, y - 1) + 1;
DIST(x, y) = ((a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c));
if (flags & GRN_TABLE_FUZZY_SEARCH_WITH_TRANSPOSITION &&
x > 1 && y > 1 && cx == cy &&
memcmp(px, py - cy, cx) == 0 &&
memcmp(px - cx, py, cx) == 0) {
uint32_t t = DIST(x - 2, y - 2) + 1;
DIST(x, y) = ((DIST(x, y) < t) ? DIST(x, y) : t);
}
}
}
}
d = DIST(lx, ly);
GRN_PLUGIN_FREE(ctx, dists);
}
return d;
}
bool pose_dist_thresh(geometry_msgs::Pose pose, geometry_msgs::Point pt, double thresh) {
ROS_INFO("dist: %f, thresh: %f, val %d", DIST(pose.position.x, pose.position.y, pt.x, pt.y), thresh, DIST(pose.position.x, pose.position.y, pt.x, pt.y) < thresh);
return DIST(pose.position.x, pose.position.y, pt.x, pt.y) < thresh;
}
