int CrowdToolState::hitTestAgents(const float* s, const float* p)
{
if (!m_sample) return -1;
dtCrowd* crowd = m_sample->getCrowd();
int isel = -1;
float tsel = FLT_MAX;
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
float bmin[3], bmax[3];
getAgentBounds(ag, bmin, bmax);
float tmin, tmax;
if (isectSegAABB(s, p, bmin,bmax, tmin, tmax))
{
if (tmin > 0 && tmin < tsel)
{
isel = i;
tsel = tmin;
}
}
}
return isel;
}
int NavMesh::HitTestAgent(const float* s, const float* p)
{
int isel = -1;
float tsel = FLT_MAX;
for (int i = 0; i < m_crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = m_crowd->getAgent(i);
if (!ag->active) continue;
float bmin[3], bmax[3];
// get agent bounds
const float* p = ag->npos;
const float r = ag->params.radius;
const float h = ag->params.height;
bmin[0] = p[0] - r;
bmin[1] = p[1];
bmin[2] = p[2] - r;
bmax[0] = p[0] + r;
bmax[1] = p[1] + h;
bmax[2] = p[2] + r;
float tmin, tmax;
if (isectSegAABB(s, p, bmin,bmax, tmin, tmax))
{
if (tmin > 0 && tmin < tsel)
{
isel = i;
tsel = tmin;
}
}
}
return isel;
}
int NavMesh::HitTestObstacle(const float* sp, const float* sq)
{
if (!m_tileCache)
return 0;
float tmin = FLT_MAX;
const dtTileCacheObstacle* obmin = 0;
for (int i = 0; i < m_tileCache->getObstacleCount(); ++i)
{
const dtTileCacheObstacle* ob = m_tileCache->getObstacle(i);
if (ob->state == DT_OBSTACLE_EMPTY)
continue;
float bmin[3], bmax[3], t0,t1;
m_tileCache->getObstacleBounds(ob, bmin,bmax);
if (isectSegAABB(sp,sq, bmin,bmax, t0,t1))
{
if (t0 < tmin)
{
tmin = t0;
obmin = ob;
}
}
}
return m_tileCache->getObstacleRef(obmin);
}
bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
{
float dir[3];
rcVsub(dir, dst, src);
// Prune hit ray.
float btmin, btmax;
if (!isectSegAABB(src, dst, &m_meshBMin[0], &m_meshBMax[0], btmin, btmax))
return false;
float p[2], q[2];
p[0] = src[0] + (dst[0]-src[0])*btmin;
p[1] = src[2] + (dst[2]-src[2])*btmin;
q[0] = src[0] + (dst[0]-src[0])*btmax;
q[1] = src[2] + (dst[2]-src[2])*btmax;
int cid[512];
const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh.get(), p, q, cid, 512);
if (!ncid)
return false;
tmin = 1.0f;
bool hit = false;
const float* verts = m_loader->getVerts();
for (int i = 0; i < ncid; ++i)
{
const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]];
const int* tris = &m_chunkyMesh->tris[node.i*3];
const int ntris = node.n;
for (int j = 0; j < ntris*3; j += 3)
{
float t = 1;
if (intersectSegmentTriangle(
src, dst,
&verts[tris[j]*3],
&verts[tris[j+1]*3],
&verts[tris[j+2]*3], t))
{
if (t < tmin)
tmin = t;
hit = true;
}
}
}
return hit;
}
dtObstacleRef OgreDetourTileCache::hitTestObstacle(const dtTileCache* tc, const float* sp, const float* sq)
{
float tmin = FLT_MAX;
const dtTileCacheObstacle* obmin = 0;
for (int i = 0; i < tc->getObstacleCount(); ++i)
{
const dtTileCacheObstacle* ob = tc->getObstacle(i);
if (ob->state == DT_OBSTACLE_EMPTY)
continue;
float bmin[3], bmax[3], t0,t1;
tc->getObstacleBounds(ob, bmin,bmax);
if (isectSegAABB(sp,sq, bmin,bmax, t0,t1))
{
if (t0 < tmin)
{
tmin = t0;
obmin = ob;
}
}
}
return tc->getObstacleRef(obmin);
}
void CrowdTool::handleClick(const float* s, const float* p, bool shift)
{
if (!m_sample) return;
InputGeom* geom = m_sample->getInputGeom();
if (!geom) return;
if (m_mode == TOOLMODE_CREATE)
{
if (shift)
{
// Delete
int isel = -1;
float tsel = FLT_MAX;
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
float bmin[3], bmax[3];
getAgentBounds(ag, bmin, bmax);
float tmin, tmax;
if (isectSegAABB(s, p, bmin,bmax, tmin, tmax))
{
if (tmin > 0 && tmin < tsel)
{
isel = i;
tsel = tmin;
}
}
}
if (isel != -1)
{
m_crowd.removeAgent(isel);
}
}
else
{
// Add
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
int idx = m_crowd.addAgent(p, m_sample->getAgentRadius(), m_sample->getAgentHeight(), navquery);
if (idx != -1 && m_targetRef)
m_crowd.requestMoveTarget(idx, m_targetRef, m_targetPos);
}
}
else if (m_mode == TOOLMODE_MOVE_TARGET)
{
// Find nearest point on navmesh and set move request to that location.
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
const dtQueryFilter* filter = m_crowd.getFilter();
const float* ext = m_crowd.getQueryExtents();
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
if (shift)
{
// Adjust target using tiny local search.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
m_crowd.adjustMoveTarget(i, m_targetRef, m_targetPos);
}
}
else
{
// Move target using paht finder
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
m_crowd.requestMoveTarget(i, m_targetRef, m_targetPos);
}
}
}
}
bool Visualization::pick(float* position, int* agentIdx)
{
bool picked = false;
if (m_scene)
{
float rays[3];
float raye[3];
float hitt;
// Compute the ray
GLdouble x, y, z;
gluUnProject(m_mousePosition[0], m_mousePosition[1], 0.f, m_modelView, m_projection, m_viewport, &x, &y, &z);
rays[0] = (float)x; rays[1] = (float)y; rays[2] = (float)z;
gluUnProject(m_mousePosition[0], m_mousePosition[1], 1.f, m_modelView, m_projection, m_viewport, &x, &y, &z);
raye[0] = (float)x; raye[1] = (float)y; raye[2] = (float)z;
// ray cast
if (m_scene->raycastMesh(rays, raye, hitt))
{
position[0] = rays[0] + (raye[0] - rays[0])*hitt;
position[1] = rays[1] + (raye[1] - rays[1])*hitt;
position[2] = rays[2] + (raye[2] - rays[2])*hitt;
picked = true;
if (m_crowd)
{
*agentIdx = -1;
float tsel = FLT_MAX;
float bmin[3], bmax[3];
for (int i = 0, size(m_crowd->getAgentCount()); i < size; ++i)
{
const dtCrowdAgent* ag = m_crowd->getAgent(i);
if (ag->active)
{
bmin[0] = ag->position[0] - ag->radius;
bmin[1] = ag->position[1];
bmin[2] = ag->position[2] - ag->radius;
bmax[0] = ag->position[0] + ag->radius;
bmax[1] = ag->position[1] + ag->height;
bmax[2] = ag->position[2] + ag->radius;
float tmin, tmax;
if (isectSegAABB(rays, raye, bmin, bmax, tmin, tmax))
{
if (tmin > 0 && tmin < tsel)
{
*agentIdx = i;
tsel = tmin;
dtVcopy(position, ag->position);
}
}
}
}
}
}
}
return picked;
}
