void SlideNavmesh::update(const float dt)
{
if (!m_update && !m_step)
return;
m_step = false;
const float maxSpeed = 1.0f;
NavmeshAgent* agent = &m_scene.agents[0];
// Find next corner to steer to.
// Smooth corner finding does a little bit of magic to calculate spline
// like curve (or first tangent) based on current position and movement direction
// next couple of corners.
float corner[2],dir[2];
int last = 1;
vsub(dir, agent->pos, agent->oldpos); // This delta handles wall-hugging better than using current velocity.
vnorm(dir);
vcpy(corner, agent->pos);
if (m_moveMode == AGENTMOVE_SMOOTH || m_moveMode == AGENTMOVE_DRUNK)
last = agentFindNextCornerSmooth(agent, dir, m_scene.nav, corner);
else
last = agentFindNextCorner(agent, m_scene.nav, corner);
if (last && vdist(agent->pos, corner) < 0.02f)
{
// Reached goal
vcpy(agent->oldpos, agent->pos);
vset(agent->dvel, 0,0);
vcpy(agent->vel, agent->dvel);
return;
}
vsub(agent->dvel, corner, agent->pos);
// Apply style
if (m_moveMode == AGENTMOVE_DRUNK)
{
agent->t += dt*4;
float amp = cosf(agent->t)*0.25f;
float nx = -agent->dvel[1];
float ny = agent->dvel[0];
agent->dvel[0] += nx * amp;
agent->dvel[1] += ny * amp;
}
// Limit desired velocity to max speed.
const float distToTarget = vdist(agent->pos,agent->target);
const float clampedSpeed = maxSpeed * min(1.0f, distToTarget/agent->rad);
vsetlen(agent->dvel, clampedSpeed);
vcpy(agent->vel, agent->dvel);
// Move agent
vscale(agent->delta, agent->vel, dt);
float npos[2];
vadd(npos, agent->pos, agent->delta);
agentMoveAndAdjustCorridor(&m_scene.agents[0], npos, m_scene.nav);
}
void selectitem()
{
if (selected_items.size() < ents.length())
selected_items.resize(ents.length());
if (!select_multiple) fill(selected_items.begin(), selected_items.end(), false);
vec selv = { worldpos.x, worldpos.y, worldpos.z };
loopv(ents)
{
entity &e = ents[i];
if(e.type < I_SHELLS || e.type > I_QUAD) continue;
if (!e.spawned) continue;
if(OUTBORD(e.x, e.y)) continue;
vec v = { e.x, e.y, S(e.x, e.y)->floor };
vdist(dist, t, selv, v);
if(dist<2.0)
{
selset = false;
selected_items[i] = true;
}
}
}
void createrays(vec &from, vec &to) // create random spread of rays for the shotgun
{
vdist(dist, dvec, from, to);
float f = dist*SGSPREAD/1000;
loopi(SGRAYS)
{
#define RNDD (rnd(101)-50)*f
vec r = { RNDD, RNDD, RNDD };
sg[i] = to;
vadd(sg[i], r);
};
};
static void on_highlight_char(fz_context *ctx, void *arg, fz_stext_line *line, fz_stext_char *ch)
{
struct highlight *hits = arg;
float vfuzz = ch->size * hits->vfuzz;
float hfuzz = ch->size * hits->hfuzz;
if (hits->len > 0)
{
fz_quad *end = &hits->box[hits->len-1];
if (hdist(&line->dir, &end->lr, &ch->quad.ll) < hfuzz
&& vdist(&line->dir, &end->lr, &ch->quad.ll) < vfuzz
&& hdist(&line->dir, &end->ur, &ch->quad.ul) < hfuzz
&& vdist(&line->dir, &end->ur, &ch->quad.ul) < vfuzz)
{
end->ur = ch->quad.ur;
end->lr = ch->quad.lr;
return;
}
}
if (hits->len < hits->cap)
hits->box[hits->len++] = ch->quad;
}
void geRK4ApplyGravity(ge_RK4State* s1, ge_RK4State* s2){
const double G = 6.67234E-11;
double d = vdist(s1->position, s2->position);
if(d <= 1.0 || d < s1->radius_min || d < s2->radius_min){
return;
}
double P = G * ((s1->mass * s2->mass) / (d * d));
ge_Vector3d force, f1, f2;
force.x = s2->position.x - s1->position.x;
force.y = s2->position.y - s1->position.y;
force.z = s2->position.z - s1->position.z;
force = vnorm(force);
force = vmulk(force, P);
f1.x = force.x;
f1.y = force.y;
f1.z = force.z;
f2.x = -force.x;
f2.y = -force.y;
f2.z = -force.z;
geRK4ApplyForce(s1, f1);
geRK4ApplyForce(s2, f2);
}
void demoplaybackstep()
{
while(demoplayback && lastmillis>=playbacktime)
{
int len = gzgeti();
if(len<1 || len>MAXTRANS)
{
conoutf("error: huge packet during demo play (%d)", len);
stopreset();
return;
};
uchar buf[MAXTRANS];
gzread(f, buf, len);
localservertoclient(buf, len); // update game state
dynent *target = players[democlientnum];
assert(target);
int extras;
if(extras = gzget()) // read additional client side state not present in normal network stream
{
target->gunselect = gzget();
target->lastattackgun = gzget();
target->lastaction = scaletime(gzgeti());
target->gunwait = gzgeti();
target->health = gzgeti();
target->armour = gzgeti();
target->armourtype = gzget();
loopi(NUMGUNS) target->ammo[i] = gzget();
target->state = gzget();
target->lastmove = playbacktime;
if(bdamage = gzgeti()) damageblend(bdamage);
if(ddamage = gzgeti()) { gzgetv(dorig); particle_splash(3, ddamage, 1000, dorig); };
// FIXME: set more client state here
};
// insert latest copy of player into history
if(extras && (playerhistory.empty() || playerhistory.last()->lastupdate!=playbacktime))
{
dynent *d = newdynent();
*d = *target;
d->lastupdate = playbacktime;
playerhistory.add(d);
if(playerhistory.length()>20)
{
zapdynent(playerhistory[0]);
playerhistory.remove(0);
};
};
readdemotime();
};
if(demoplayback)
{
int itime = lastmillis-demodelaymsec;
loopvrev(playerhistory) if(playerhistory[i]->lastupdate<itime) // find 2 positions in history that surround interpolation time point
{
dynent *a = playerhistory[i];
dynent *b = a;
if(i+1<playerhistory.length()) b = playerhistory[i+1];
*player1 = *b;
if(a!=b) // interpolate pos & angles
{
dynent *c = b;
if(i+2<playerhistory.length()) c = playerhistory[i+2];
dynent *z = a;
if(i-1>=0) z = playerhistory[i-1];
//if(a==z || b==c) printf("* %d\n", lastmillis);
float bf = (itime-a->lastupdate)/(float)(b->lastupdate-a->lastupdate);
fixwrap(a, player1);
fixwrap(c, player1);
fixwrap(z, player1);
vdist(dist, v, z->o, c->o);
if(dist<16) // if teleport or spawn, dont't interpolate
{
catmulrom(z->o, a->o, b->o, c->o, bf, player1->o);
catmulrom(*(vec *)&z->yaw, *(vec *)&a->yaw, *(vec *)&b->yaw, *(vec *)&c->yaw, bf, *(vec *)&player1->yaw);
};
fixplayer1range();
};
break;
};
//if(player1->state!=CS_DEAD) showscores(false);
};
};
void vdist_from_r(int *vectors_length, double vec_data1[], double vec_data2[], int genotypes[])
{
vdist(*vectors_length, vec_data1, vec_data2, genotypes);
}
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
KX_Obstacles& obstacles, float levelHeight, const float vmax,
const float* spos, const float cs, const int nspos, float* res,
float maxToi, float velWeight, float curVelWeight, float sideWeight,
float toiWeight)
{
vset(res, 0,0);
const float ivmax = 1.0f / vmax;
float adir[2], adist;
vcpy(adir, activeObst->pvel);
if (vlen(adir) > 0.01f)
vnorm(adir);
else
vset(adir,0,0);
float activeObstPos[2];
vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y());
adist = vdot(adir, activeObstPos);
float minPenalty = FLT_MAX;
for (int n = 0; n < nspos; ++n)
{
float vcand[2];
vcpy(vcand, &spos[n*2]);
// Find min time of impact and exit amongst all obstacles.
float tmin = maxToi;
float side = 0;
int nside = 0;
for (int i = 0; i < obstacles.size(); ++i)
{
KX_Obstacle* ob = obstacles[i];
bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
if (!res)
continue;
float htmin, htmax;
if (ob->m_shape==KX_OBSTACLE_CIRCLE)
{
float vab[2];
// Moving, use RVO
vscale(vab, vcand, 2);
vsub(vab, vab, activeObst->vel);
vsub(vab, vab, ob->vel);
// Side
// NOTE: dp, and dv are constant over the whole calculation,
// they can be precomputed per object.
const float* pa = activeObstPos;
float pb[2];
vset(pb, ob->m_pos.x(), ob->m_pos.y());
const float orig[2] = {0,0};
float dp[2],dv[2],np[2];
vsub(dp,pb,pa);
vnorm(dp);
vsub(dv,ob->dvel, activeObst->dvel);
const float a = triarea(orig, dp,dv);
if (a < 0.01f)
{
np[0] = -dp[1];
np[1] = dp[0];
}
else
{
np[0] = dp[1];
np[1] = -dp[0];
}
side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
nside++;
if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad,
htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
}
else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{
MT_Point3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2;
//apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{
KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
p1 = navmeshobj->TransformToWorldCoords(p1);
p2 = navmeshobj->TransformToWorldCoords(p2);
}
float p[2], q[2];
vset(p, p1.x(), p1.y());
vset(q, p2.x(), p2.y());
// NOTE: the segments are assumed to come from a navmesh which is shrunken by
// the agent radius, hence the use of really small radius.
// This can be handle more efficiently by using seg-seg test instead.
// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
const float r = 0.01f; // agent->rad
if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
{
float sdir[2], snorm[2];
vsub(sdir, q, p);
snorm[0] = sdir[1];
snorm[1] = -sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (vdot(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
htmax = 10.0f;
}
else
{
if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
const float spen = sideWeight * side;
const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
const float penalty = vpen + vcpen + spen + tpen;
if (penalty < minPenalty)
{
minPenalty = penalty;
vcpy(res, vcand);
}
}
}
//---------------------------------------------------------------------------------------
void z_ed3View::OnMouseMove(UINT nFlags, CPoint point)
{
V3 scrPt;
CPoint pt = _mm._pt - point;
if(_mm._rdown || _mm._ldown)
{
_mm._pt = point;
//SetCursorPos(_mm._pt.x, _mm._pt.y);
if(AKEY('L'))
{
DrawInHrc(_hdc);
CView::OnMouseMove(nFlags, point);
REAL cf = _fov + pt.y;
if(cf < 35) cf=35;
if(cf>140) cf=140;
_fov = cf ;
DOC()->_cam.SetFov(_fov);
SBT(3,MKSTR("FOV :%03.0f", _fov));
return;
}
/*
if(!AKEY(VK_LBUTTON))
_mm._ldown=0;
if(!AKEY(VK_RBUTTON))
_mm._rdown=0;
*/
}
DOC()->Make3dCursorPos(this, point, _rt, scrPt);
if(_mm._ldown)
{
if(abs(pt.x) > 0||
abs(pt.y) > 0)
{
if(_mm._rdown)
{
if(AKEY(VK_SPACE))
{
DOC()->_cam.MoveUp(pt.y*64.f);
}
else
{
DOC()->_cam.MoveUp(pt.y*2.f);
}
if(AKEY(VK_SPACE))
{
DOC()->_cam.MoveSide(-pt.x*64.f);
}
else
{
DOC()->_cam.MoveSide(-pt.x*2.f);
}
}
else
{
if(AKEY(VK_SHIFT))
{
V3 cr ;
int nC = 0;
Brush** ppb = DOC()->GetSelBrushes(nC);
if(nC){
cr = (*ppb)->_box.GetCenter();
}
REAL dst = -vdist(DOC()->_cam._pos , cr);
DOC()->_cam._pos = cr;
DOC()->_cam.Rotate(-pt.x/128.f,-pt.y/128.f,0);
DOC()->_cam._pos = cr + (DOC()->_cam._fwd)*dst;
}
else
{
DOC()->_cam.Rotate(-pt.x/128.f,-pt.y/128.f,0);
}
}
_rotating = TRUE;
}
SBT(2,MKSTR("CAM: %04.2f %04.2f z:%04.2f", DOC()->_cam._pos.x/GMeters,
DOC()->_cam._pos.y/GMeters,
DOC()->_cam._pos.z/GMeters));
if(DOC()->_compviewmode != C_NA)
{
DOC()->Invalidate(); //allow to see the full BSP
}
else
DrawInHrc(_hdc);
}
else if(_mm._rdown )
{
if(AKEY(VK_SPACE) )
DOC()->_cam.MoveFwd(pt.y*64.f);
else
DOC()->_cam.MoveFwd(pt.y*4.f);
SBT(2,MKSTR("CAM: %04.2f %04.2f z:%04.2f",DOC()->_cam._pos.x/GMeters,
DOC()->_cam._pos.y/GMeters,
DOC()->_cam._pos.z/GMeters));
_rotating = TRUE;
if(DOC()->_compviewmode != C_NA)
{
DOC()->Invalidate();
}
else
DrawInHrc(_hdc);
}
if(_mm._rdown || _mm._ldown)
Rollover(point);
CView::OnMouseMove(nFlags, point);
}
