9
3

CrowdTool.cpp 30 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107
  1. //
  2. // Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
  3. //
  4. // This software is provided 'as-is', without any express or implied
  5. // warranty. In no event will the authors be held liable for any damages
  6. // arising from the use of this software.
  7. // Permission is granted to anyone to use this software for any purpose,
  8. // including commercial applications, and to alter it and redistribute it
  9. // freely, subject to the following restrictions:
  10. // 1. The origin of this software must not be misrepresented; you must not
  11. // claim that you wrote the original software. If you use this software
  12. // in a product, an acknowledgment in the product documentation would be
  13. // appreciated but is not required.
  14. // 2. Altered source versions must be plainly marked as such, and must not be
  15. // misrepresented as being the original software.
  16. // 3. This notice may not be removed or altered from any source distribution.
  17. //
  18. #define _USE_MATH_DEFINES
  19. #include <math.h>
  20. #include <stdio.h>
  21. #include <string.h>
  22. #include <float.h>
  23. #include "SDL.h"
  24. #include "SDL_opengl.h"
  25. #ifdef __APPLE__
  26. # include <OpenGL/glu.h>
  27. #else
  28. # include <GL/glu.h>
  29. #endif
  30. #include "imgui.h"
  31. #include "CrowdTool.h"
  32. #include "InputGeom.h"
  33. #include "Sample.h"
  34. #include "DetourCrowd.h"
  35. #include "DetourDebugDraw.h"
  36. #include "DetourObstacleAvoidance.h"
  37. #include "DetourCommon.h"
  38. #include "DetourNode.h"
  39. #include "SampleInterfaces.h"
  40. #ifdef WIN32
  41. # define snprintf _snprintf
  42. #endif
  43. static bool isectSegAABB(const float* sp, const float* sq,
  44. const float* amin, const float* amax,
  45. float& tmin, float& tmax)
  46. {
  47. static const float EPS = 1e-6f;
  48. float d[3];
  49. dtVsub(d, sq, sp);
  50. tmin = 0; // set to -FLT_MAX to get first hit on line
  51. tmax = FLT_MAX; // set to max distance ray can travel (for segment)
  52. // For all three slabs
  53. for (int i = 0; i < 3; i++)
  54. {
  55. if (fabsf(d[i]) < EPS)
  56. {
  57. // Ray is parallel to slab. No hit if origin not within slab
  58. if (sp[i] < amin[i] || sp[i] > amax[i])
  59. return false;
  60. }
  61. else
  62. {
  63. // Compute intersection t value of ray with near and far plane of slab
  64. const float ood = 1.0f / d[i];
  65. float t1 = (amin[i] - sp[i]) * ood;
  66. float t2 = (amax[i] - sp[i]) * ood;
  67. // Make t1 be intersection with near plane, t2 with far plane
  68. if (t1 > t2) dtSwap(t1, t2);
  69. // Compute the intersection of slab intersections intervals
  70. if (t1 > tmin) tmin = t1;
  71. if (t2 < tmax) tmax = t2;
  72. // Exit with no collision as soon as slab intersection becomes empty
  73. if (tmin > tmax) return false;
  74. }
  75. }
  76. return true;
  77. }
  78. static void getAgentBounds(const dtCrowdAgent* ag, float* bmin, float* bmax)
  79. {
  80. const float* p = ag->npos;
  81. const float r = ag->params.radius;
  82. const float h = ag->params.height;
  83. bmin[0] = p[0] - r;
  84. bmin[1] = p[1];
  85. bmin[2] = p[2] - r;
  86. bmax[0] = p[0] + r;
  87. bmax[1] = p[1] + h;
  88. bmax[2] = p[2] + r;
  89. }
  90. CrowdToolState::CrowdToolState() :
  91. m_sample(0),
  92. m_nav(0),
  93. m_crowd(0),
  94. m_targetRef(0),
  95. m_run(true)
  96. {
  97. m_toolParams.m_expandSelectedDebugDraw = true;
  98. m_toolParams.m_showCorners = false;
  99. m_toolParams.m_showCollisionSegments = false;
  100. m_toolParams.m_showPath = false;
  101. m_toolParams.m_showVO = false;
  102. m_toolParams.m_showOpt = false;
  103. m_toolParams.m_showNeis = false;
  104. m_toolParams.m_expandDebugDraw = false;
  105. m_toolParams.m_showLabels = false;
  106. m_toolParams.m_showGrid = false;
  107. m_toolParams.m_showNodes = false;
  108. m_toolParams.m_showPerfGraph = false;
  109. m_toolParams.m_showDetailAll = false;
  110. m_toolParams.m_expandOptions = true;
  111. m_toolParams.m_anticipateTurns = true;
  112. m_toolParams.m_optimizeVis = true;
  113. m_toolParams.m_optimizeTopo = true;
  114. m_toolParams.m_obstacleAvoidance = true;
  115. m_toolParams.m_obstacleAvoidanceType = 3.0f;
  116. m_toolParams.m_separation = false;
  117. m_toolParams.m_separationWeight = 2.0f;
  118. memset(m_trails, 0, sizeof(m_trails));
  119. m_vod = dtAllocObstacleAvoidanceDebugData();
  120. m_vod->init(2048);
  121. memset(&m_agentDebug, 0, sizeof(m_agentDebug));
  122. m_agentDebug.idx = -1;
  123. m_agentDebug.vod = m_vod;
  124. }
  125. CrowdToolState::~CrowdToolState()
  126. {
  127. dtFreeObstacleAvoidanceDebugData(m_vod);
  128. }
  129. void CrowdToolState::init(class Sample* sample)
  130. {
  131. if (m_sample != sample)
  132. {
  133. m_sample = sample;
  134. }
  135. dtNavMesh* nav = m_sample->getNavMesh();
  136. dtCrowd* crowd = m_sample->getCrowd();
  137. if (nav && crowd && (m_nav != nav || m_crowd != crowd))
  138. {
  139. m_nav = nav;
  140. m_crowd = crowd;
  141. crowd->init(MAX_AGENTS, m_sample->getAgentRadius(), nav);
  142. // Make polygons with 'disabled' flag invalid.
  143. crowd->getEditableFilter(0)->setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);
  144. // Setup local avoidance params to different qualities.
  145. dtObstacleAvoidanceParams params;
  146. // Use mostly default settings, copy from dtCrowd.
  147. memcpy(&params, crowd->getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
  148. // Low (11)
  149. params.velBias = 0.5f;
  150. params.adaptiveDivs = 5;
  151. params.adaptiveRings = 2;
  152. params.adaptiveDepth = 1;
  153. crowd->setObstacleAvoidanceParams(0, &params);
  154. // Medium (22)
  155. params.velBias = 0.5f;
  156. params.adaptiveDivs = 5;
  157. params.adaptiveRings = 2;
  158. params.adaptiveDepth = 2;
  159. crowd->setObstacleAvoidanceParams(1, &params);
  160. // Good (45)
  161. params.velBias = 0.5f;
  162. params.adaptiveDivs = 7;
  163. params.adaptiveRings = 2;
  164. params.adaptiveDepth = 3;
  165. crowd->setObstacleAvoidanceParams(2, &params);
  166. // High (66)
  167. params.velBias = 0.5f;
  168. params.adaptiveDivs = 7;
  169. params.adaptiveRings = 3;
  170. params.adaptiveDepth = 3;
  171. crowd->setObstacleAvoidanceParams(3, &params);
  172. }
  173. }
  174. void CrowdToolState::reset()
  175. {
  176. }
  177. void CrowdToolState::handleRender()
  178. {
  179. duDebugDraw& dd = m_sample->getDebugDraw();
  180. const float rad = m_sample->getAgentRadius();
  181. dtNavMesh* nav = m_sample->getNavMesh();
  182. dtCrowd* crowd = m_sample->getCrowd();
  183. if (!nav || !crowd)
  184. return;
  185. if (m_toolParams.m_showNodes && crowd->getPathQueue())
  186. {
  187. const dtNavMeshQuery* navquery = crowd->getPathQueue()->getNavQuery();
  188. if (navquery)
  189. duDebugDrawNavMeshNodes(&dd, *navquery);
  190. }
  191. dd.depthMask(false);
  192. // Draw paths
  193. if (m_toolParams.m_showPath)
  194. {
  195. for (int i = 0; i < crowd->getAgentCount(); i++)
  196. {
  197. if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
  198. continue;
  199. const dtCrowdAgent* ag =crowd->getAgent(i);
  200. if (!ag->active)
  201. continue;
  202. const dtPolyRef* path = ag->corridor.getPath();
  203. const int npath = ag->corridor.getPathCount();
  204. for (int j = 0; j < npath; ++j)
  205. duDebugDrawNavMeshPoly(&dd, *nav, path[j], duRGBA(255,255,255,24));
  206. }
  207. }
  208. if (m_targetRef)
  209. duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1]+0.1f,m_targetPos[2], rad, duRGBA(255,255,255,192), 2.0f);
  210. // Occupancy grid.
  211. if (m_toolParams.m_showGrid)
  212. {
  213. float gridy = -FLT_MAX;
  214. for (int i = 0; i < crowd->getAgentCount(); ++i)
  215. {
  216. const dtCrowdAgent* ag = crowd->getAgent(i);
  217. if (!ag->active) continue;
  218. const float* pos = ag->corridor.getPos();
  219. gridy = dtMax(gridy, pos[1]);
  220. }
  221. gridy += 1.0f;
  222. dd.begin(DU_DRAW_QUADS);
  223. const dtProximityGrid* grid = crowd->getGrid();
  224. const int* bounds = grid->getBounds();
  225. const float cs = grid->getCellSize();
  226. for (int y = bounds[1]; y <= bounds[3]; ++y)
  227. {
  228. for (int x = bounds[0]; x <= bounds[2]; ++x)
  229. {
  230. const int count = grid->getItemCountAt(x,y);
  231. if (!count) continue;
  232. unsigned int col = duRGBA(128,0,0,dtMin(count*40,255));
  233. dd.vertex(x*cs, gridy, y*cs, col);
  234. dd.vertex(x*cs, gridy, y*cs+cs, col);
  235. dd.vertex(x*cs+cs, gridy, y*cs+cs, col);
  236. dd.vertex(x*cs+cs, gridy, y*cs, col);
  237. }
  238. }
  239. dd.end();
  240. }
  241. // Trail
  242. for (int i = 0; i < crowd->getAgentCount(); ++i)
  243. {
  244. const dtCrowdAgent* ag = crowd->getAgent(i);
  245. if (!ag->active) continue;
  246. const AgentTrail* trail = &m_trails[i];
  247. const float* pos = ag->npos;
  248. dd.begin(DU_DRAW_LINES,3.0f);
  249. float prev[3], preva = 1;
  250. dtVcopy(prev, pos);
  251. for (int j = 0; j < AGENT_MAX_TRAIL-1; ++j)
  252. {
  253. const int idx = (trail->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL;
  254. const float* v = &trail->trail[idx*3];
  255. float a = 1 - j/(float)AGENT_MAX_TRAIL;
  256. dd.vertex(prev[0],prev[1]+0.1f,prev[2], duRGBA(0,0,0,(int)(128*preva)));
  257. dd.vertex(v[0],v[1]+0.1f,v[2], duRGBA(0,0,0,(int)(128*a)));
  258. preva = a;
  259. dtVcopy(prev, v);
  260. }
  261. dd.end();
  262. }
  263. // Corners & co
  264. for (int i = 0; i < crowd->getAgentCount(); i++)
  265. {
  266. if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
  267. continue;
  268. const dtCrowdAgent* ag =crowd->getAgent(i);
  269. if (!ag->active)
  270. continue;
  271. const float radius = ag->params.radius;
  272. const float* pos = ag->npos;
  273. if (m_toolParams.m_showCorners)
  274. {
  275. if (ag->ncorners)
  276. {
  277. dd.begin(DU_DRAW_LINES, 2.0f);
  278. for (int j = 0; j < ag->ncorners; ++j)
  279. {
  280. const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
  281. const float* vb = &ag->cornerVerts[j*3];
  282. dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192));
  283. dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192));
  284. }
  285. if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
  286. {
  287. const float* v = &ag->cornerVerts[(ag->ncorners-1)*3];
  288. dd.vertex(v[0],v[1],v[2], duRGBA(192,0,0,192));
  289. dd.vertex(v[0],v[1]+radius*2,v[2], duRGBA(192,0,0,192));
  290. }
  291. dd.end();
  292. if (m_toolParams.m_anticipateTurns)
  293. {
  294. /* float dvel[3], pos[3];
  295. calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel);
  296. pos[0] = ag->pos[0] + dvel[0];
  297. pos[1] = ag->pos[1] + dvel[1];
  298. pos[2] = ag->pos[2] + dvel[2];
  299. const float off = ag->radius+0.1f;
  300. const float* tgt = &ag->cornerVerts[0];
  301. const float y = ag->pos[1]+off;
  302. dd.begin(DU_DRAW_LINES, 2.0f);
  303. dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192));
  304. dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
  305. dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
  306. dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
  307. dd.end();*/
  308. }
  309. }
  310. }
  311. if (m_toolParams.m_showCollisionSegments)
  312. {
  313. const float* center = ag->boundary.getCenter();
  314. duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f);
  315. duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange,
  316. duRGBA(192,0,128,128), 2.0f);
  317. dd.begin(DU_DRAW_LINES, 3.0f);
  318. for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
  319. {
  320. const float* s = ag->boundary.getSegment(j);
  321. unsigned int col = duRGBA(192,0,128,192);
  322. if (dtTriArea2D(pos, s, s+3) < 0.0f)
  323. col = duDarkenCol(col);
  324. duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
  325. }
  326. dd.end();
  327. }
  328. if (m_toolParams.m_showNeis)
  329. {
  330. duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
  331. duRGBA(0,192,128,128), 2.0f);
  332. dd.begin(DU_DRAW_LINES, 2.0f);
  333. for (int j = 0; j < ag->nneis; ++j)
  334. {
  335. // Get 'n'th active agent.
  336. // TODO: fix this properly.
  337. const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
  338. if (nei)
  339. {
  340. dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
  341. dd.vertex(nei->npos[0],nei->npos[1]+radius,nei->npos[2], duRGBA(0,192,128,128));
  342. }
  343. }
  344. dd.end();
  345. }
  346. if (m_toolParams.m_showOpt)
  347. {
  348. dd.begin(DU_DRAW_LINES, 2.0f);
  349. dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192));
  350. dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192));
  351. dd.end();
  352. }
  353. }
  354. // Agent cylinders.
  355. for (int i = 0; i < crowd->getAgentCount(); ++i)
  356. {
  357. const dtCrowdAgent* ag = crowd->getAgent(i);
  358. if (!ag->active) continue;
  359. const float radius = ag->params.radius;
  360. const float* pos = ag->npos;
  361. unsigned int col = duRGBA(0,0,0,32);
  362. if (m_agentDebug.idx == i)
  363. col = duRGBA(255,0,0,128);
  364. duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, col, 2.0f);
  365. }
  366. for (int i = 0; i < crowd->getAgentCount(); ++i)
  367. {
  368. const dtCrowdAgent* ag = crowd->getAgent(i);
  369. if (!ag->active) continue;
  370. const float height = ag->params.height;
  371. const float radius = ag->params.radius;
  372. const float* pos = ag->npos;
  373. unsigned int col = duRGBA(220,220,220,128);
  374. if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
  375. col = duLerpCol(col, duRGBA(128,0,255,128), 32);
  376. else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
  377. col = duLerpCol(col, duRGBA(128,0,255,128), 128);
  378. else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
  379. col = duRGBA(255,32,16,128);
  380. else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  381. col = duLerpCol(col, duRGBA(64,255,0,128), 128);
  382. duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
  383. pos[0]+radius, pos[1]+height, pos[2]+radius, col);
  384. }
  385. if (m_toolParams.m_showVO)
  386. {
  387. for (int i = 0; i < crowd->getAgentCount(); i++)
  388. {
  389. if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
  390. continue;
  391. const dtCrowdAgent* ag =crowd->getAgent(i);
  392. if (!ag->active)
  393. continue;
  394. // Draw detail about agent sela
  395. const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
  396. const float dx = ag->npos[0];
  397. const float dy = ag->npos[1]+ag->params.height;
  398. const float dz = ag->npos[2];
  399. duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
  400. dd.begin(DU_DRAW_QUADS);
  401. for (int j = 0; j < vod->getSampleCount(); ++j)
  402. {
  403. const float* p = vod->getSampleVelocity(j);
  404. const float sr = vod->getSampleSize(j);
  405. const float pen = vod->getSamplePenalty(j);
  406. const float pen2 = vod->getSamplePreferredSidePenalty(j);
  407. unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255));
  408. col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128));
  409. dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
  410. dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
  411. dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
  412. dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
  413. }
  414. dd.end();
  415. }
  416. }
  417. // Velocity stuff.
  418. for (int i = 0; i < crowd->getAgentCount(); ++i)
  419. {
  420. const dtCrowdAgent* ag = crowd->getAgent(i);
  421. if (!ag->active) continue;
  422. const float radius = ag->params.radius;
  423. const float height = ag->params.height;
  424. const float* pos = ag->npos;
  425. const float* vel = ag->vel;
  426. const float* dvel = ag->dvel;
  427. unsigned int col = duRGBA(220,220,220,192);
  428. if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
  429. col = duLerpCol(col, duRGBA(128,0,255,192), 32);
  430. else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
  431. col = duLerpCol(col, duRGBA(128,0,255,192), 128);
  432. else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
  433. col = duRGBA(255,32,16,192);
  434. else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  435. col = duLerpCol(col, duRGBA(64,255,0,192), 128);
  436. duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
  437. duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
  438. pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+dvel[2],
  439. 0.0f, 0.4f, duRGBA(0,192,255,192), (m_agentDebug.idx == i) ? 2.0f : 1.0f);
  440. duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
  441. pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
  442. 0.0f, 0.4f, duRGBA(0,0,0,160), 2.0f);
  443. }
  444. dd.depthMask(true);
  445. }
  446. void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
  447. {
  448. GLdouble x, y, z;
  449. // Draw start and end point labels
  450. if (m_targetRef && gluProject((GLdouble)m_targetPos[0], (GLdouble)m_targetPos[1], (GLdouble)m_targetPos[2],
  451. model, proj, view, &x, &y, &z))
  452. {
  453. imguiDrawText((int)x, (int)(y+25), IMGUI_ALIGN_CENTER, "TARGET", imguiRGBA(0,0,0,220));
  454. }
  455. char label[32];
  456. if (m_toolParams.m_showNodes)
  457. {
  458. dtCrowd* crowd = m_sample->getCrowd();
  459. if (crowd && crowd->getPathQueue())
  460. {
  461. const dtNavMeshQuery* navquery = crowd->getPathQueue()->getNavQuery();
  462. const dtNodePool* pool = navquery->getNodePool();
  463. if (pool)
  464. {
  465. const float off = 0.5f;
  466. for (int i = 0; i < pool->getHashSize(); ++i)
  467. {
  468. for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j))
  469. {
  470. const dtNode* node = pool->getNodeAtIdx(j+1);
  471. if (!node) continue;
  472. if (gluProject((GLdouble)node->pos[0],(GLdouble)node->pos[1]+off,(GLdouble)node->pos[2],
  473. model, proj, view, &x, &y, &z))
  474. {
  475. const float heuristic = node->total;// - node->cost;
  476. snprintf(label, 32, "%.2f", heuristic);
  477. imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(0,0,0,220));
  478. }
  479. }
  480. }
  481. }
  482. }
  483. }
  484. if (m_toolParams.m_showLabels)
  485. {
  486. dtCrowd* crowd = m_sample->getCrowd();
  487. if (crowd)
  488. {
  489. for (int i = 0; i < crowd->getAgentCount(); ++i)
  490. {
  491. const dtCrowdAgent* ag = crowd->getAgent(i);
  492. if (!ag->active) continue;
  493. const float* pos = ag->npos;
  494. const float h = ag->params.height;
  495. if (gluProject((GLdouble)pos[0], (GLdouble)pos[1]+h, (GLdouble)pos[2],
  496. model, proj, view, &x, &y, &z))
  497. {
  498. snprintf(label, 32, "%d", i);
  499. imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(0,0,0,220));
  500. }
  501. }
  502. }
  503. }
  504. if (m_agentDebug.idx != -1)
  505. {
  506. dtCrowd* crowd = m_sample->getCrowd();
  507. if (crowd)
  508. {
  509. for (int i = 0; i < crowd->getAgentCount(); i++)
  510. {
  511. if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
  512. continue;
  513. const dtCrowdAgent* ag =crowd->getAgent(i);
  514. if (!ag->active)
  515. continue;
  516. const float radius = ag->params.radius;
  517. if (m_toolParams.m_showNeis)
  518. {
  519. for (int j = 0; j < ag->nneis; ++j)
  520. {
  521. const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
  522. if (!nei->active) continue;
  523. if (gluProject((GLdouble)nei->npos[0], (GLdouble)nei->npos[1]+radius, (GLdouble)nei->npos[2],
  524. model, proj, view, &x, &y, &z))
  525. {
  526. snprintf(label, 32, "%.3f", ag->neis[j].dist);
  527. imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(255,255,255,220));
  528. }
  529. }
  530. }
  531. }
  532. }
  533. }
  534. if (m_toolParams.m_showPerfGraph)
  535. {
  536. GraphParams gp;
  537. gp.setRect(300, 10, 500, 200, 8);
  538. gp.setValueRange(0.0f, 2.0f, 4, "ms");
  539. drawGraphBackground(&gp);
  540. drawGraph(&gp, &m_crowdTotalTime, 1, "Total", duRGBA(255,128,0,255));
  541. gp.setRect(300, 10, 500, 50, 8);
  542. gp.setValueRange(0.0f, 2000.0f, 1, "");
  543. drawGraph(&gp, &m_crowdSampleCount, 0, "Sample Count", duRGBA(96,96,96,128));
  544. }
  545. }
  546. void CrowdToolState::handleUpdate(const float dt)
  547. {
  548. if (m_run)
  549. updateTick(dt);
  550. }
  551. void CrowdToolState::addAgent(const float* p)
  552. {
  553. if (!m_sample) return;
  554. dtCrowd* crowd = m_sample->getCrowd();
  555. dtCrowdAgentParams ap;
  556. memset(&ap, 0, sizeof(ap));
  557. ap.radius = m_sample->getAgentRadius();
  558. ap.height = m_sample->getAgentHeight();
  559. ap.maxAcceleration = 8.0f;
  560. ap.maxSpeed = 3.5f;
  561. ap.collisionQueryRange = ap.radius * 12.0f;
  562. ap.pathOptimizationRange = ap.radius * 30.0f;
  563. ap.updateFlags = 0;
  564. if (m_toolParams.m_anticipateTurns)
  565. ap.updateFlags |= DT_CROWD_ANTICIPATE_TURNS;
  566. if (m_toolParams.m_optimizeVis)
  567. ap.updateFlags |= DT_CROWD_OPTIMIZE_VIS;
  568. if (m_toolParams.m_optimizeTopo)
  569. ap.updateFlags |= DT_CROWD_OPTIMIZE_TOPO;
  570. if (m_toolParams.m_obstacleAvoidance)
  571. ap.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
  572. if (m_toolParams.m_separation)
  573. ap.updateFlags |= DT_CROWD_SEPARATION;
  574. ap.obstacleAvoidanceType = (unsigned char)m_toolParams.m_obstacleAvoidanceType;
  575. ap.separationWeight = m_toolParams.m_separationWeight;
  576. int idx = crowd->addAgent(p, &ap);
  577. if (idx != -1)
  578. {
  579. if (m_targetRef)
  580. crowd->requestMoveTarget(idx, m_targetRef, m_targetPos);
  581. // Init trail
  582. AgentTrail* trail = &m_trails[idx];
  583. for (int i = 0; i < AGENT_MAX_TRAIL; ++i)
  584. dtVcopy(&trail->trail[i*3], p);
  585. trail->htrail = 0;
  586. }
  587. }
  588. void CrowdToolState::removeAgent(const int idx)
  589. {
  590. if (!m_sample) return;
  591. dtCrowd* crowd = m_sample->getCrowd();
  592. crowd->removeAgent(idx);
  593. if (idx == m_agentDebug.idx)
  594. m_agentDebug.idx = -1;
  595. }
  596. void CrowdToolState::hilightAgent(const int idx)
  597. {
  598. m_agentDebug.idx = idx;
  599. }
  600. static void calcVel(float* vel, const float* pos, const float* tgt, const float speed)
  601. {
  602. dtVsub(vel, tgt, pos);
  603. vel[1] = 0.0;
  604. dtVnormalize(vel);
  605. dtVscale(vel, vel, speed);
  606. }
  607. void CrowdToolState::setMoveTarget(const float* p, bool adjust)
  608. {
  609. if (!m_sample) return;
  610. // Find nearest point on navmesh and set move request to that location.
  611. dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
  612. dtCrowd* crowd = m_sample->getCrowd();
  613. const dtQueryFilter* filter = crowd->getFilter(0);
  614. const float* halfExtents = crowd->getQueryExtents();
  615. if (adjust)
  616. {
  617. float vel[3];
  618. // Request velocity
  619. if (m_agentDebug.idx != -1)
  620. {
  621. const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
  622. if (ag && ag->active)
  623. {
  624. calcVel(vel, ag->npos, p, ag->params.maxSpeed);
  625. crowd->requestMoveVelocity(m_agentDebug.idx, vel);
  626. }
  627. }
  628. else
  629. {
  630. for (int i = 0; i < crowd->getAgentCount(); ++i)
  631. {
  632. const dtCrowdAgent* ag = crowd->getAgent(i);
  633. if (!ag->active) continue;
  634. calcVel(vel, ag->npos, p, ag->params.maxSpeed);
  635. crowd->requestMoveVelocity(i, vel);
  636. }
  637. }
  638. }
  639. else
  640. {
  641. navquery->findNearestPoly(p, halfExtents, filter, &m_targetRef, m_targetPos);
  642. if (m_agentDebug.idx != -1)
  643. {
  644. const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
  645. if (ag && ag->active)
  646. crowd->requestMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos);
  647. }
  648. else
  649. {
  650. for (int i = 0; i < crowd->getAgentCount(); ++i)
  651. {
  652. const dtCrowdAgent* ag = crowd->getAgent(i);
  653. if (!ag->active) continue;
  654. crowd->requestMoveTarget(i, m_targetRef, m_targetPos);
  655. }
  656. }
  657. }
  658. }
  659. int CrowdToolState::hitTestAgents(const float* s, const float* p)
  660. {
  661. if (!m_sample) return -1;
  662. dtCrowd* crowd = m_sample->getCrowd();
  663. int isel = -1;
  664. float tsel = FLT_MAX;
  665. for (int i = 0; i < crowd->getAgentCount(); ++i)
  666. {
  667. const dtCrowdAgent* ag = crowd->getAgent(i);
  668. if (!ag->active) continue;
  669. float bmin[3], bmax[3];
  670. getAgentBounds(ag, bmin, bmax);
  671. float tmin, tmax;
  672. if (isectSegAABB(s, p, bmin,bmax, tmin, tmax))
  673. {
  674. if (tmin > 0 && tmin < tsel)
  675. {
  676. isel = i;
  677. tsel = tmin;
  678. }
  679. }
  680. }
  681. return isel;
  682. }
  683. void CrowdToolState::updateAgentParams()
  684. {
  685. if (!m_sample) return;
  686. dtCrowd* crowd = m_sample->getCrowd();
  687. if (!crowd) return;
  688. unsigned char updateFlags = 0;
  689. unsigned char obstacleAvoidanceType = 0;
  690. if (m_toolParams.m_anticipateTurns)
  691. updateFlags |= DT_CROWD_ANTICIPATE_TURNS;
  692. if (m_toolParams.m_optimizeVis)
  693. updateFlags |= DT_CROWD_OPTIMIZE_VIS;
  694. if (m_toolParams.m_optimizeTopo)
  695. updateFlags |= DT_CROWD_OPTIMIZE_TOPO;
  696. if (m_toolParams.m_obstacleAvoidance)
  697. updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
  698. if (m_toolParams.m_obstacleAvoidance)
  699. updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
  700. if (m_toolParams.m_separation)
  701. updateFlags |= DT_CROWD_SEPARATION;
  702. obstacleAvoidanceType = (unsigned char)m_toolParams.m_obstacleAvoidanceType;
  703. dtCrowdAgentParams params;
  704. for (int i = 0; i < crowd->getAgentCount(); ++i)
  705. {
  706. const dtCrowdAgent* ag = crowd->getAgent(i);
  707. if (!ag->active) continue;
  708. memcpy(&params, &ag->params, sizeof(dtCrowdAgentParams));
  709. params.updateFlags = updateFlags;
  710. params.obstacleAvoidanceType = obstacleAvoidanceType;
  711. params.separationWeight = m_toolParams.m_separationWeight;
  712. crowd->updateAgentParameters(i, &params);
  713. }
  714. }
  715. void CrowdToolState::updateTick(const float dt)
  716. {
  717. if (!m_sample) return;
  718. dtNavMesh* nav = m_sample->getNavMesh();
  719. dtCrowd* crowd = m_sample->getCrowd();
  720. if (!nav || !crowd) return;
  721. TimeVal startTime = getPerfTime();
  722. crowd->update(dt, &m_agentDebug);
  723. TimeVal endTime = getPerfTime();
  724. // Update agent trails
  725. for (int i = 0; i < crowd->getAgentCount(); ++i)
  726. {
  727. const dtCrowdAgent* ag = crowd->getAgent(i);
  728. AgentTrail* trail = &m_trails[i];
  729. if (!ag->active)
  730. continue;
  731. // Update agent movement trail.
  732. trail->htrail = (trail->htrail + 1) % AGENT_MAX_TRAIL;
  733. dtVcopy(&trail->trail[trail->htrail*3], ag->npos);
  734. }
  735. m_agentDebug.vod->normalizeSamples();
  736. m_crowdSampleCount.addSample((float)crowd->getVelocitySampleCount());
  737. m_crowdTotalTime.addSample(getPerfTimeUsec(endTime - startTime) / 1000.0f);
  738. }
  739. CrowdTool::CrowdTool() :
  740. m_sample(0),
  741. m_state(0),
  742. m_mode(TOOLMODE_CREATE)
  743. {
  744. }
  745. void CrowdTool::init(Sample* sample)
  746. {
  747. if (m_sample != sample)
  748. {
  749. m_sample = sample;
  750. }
  751. if (!sample)
  752. return;
  753. m_state = (CrowdToolState*)sample->getToolState(type());
  754. if (!m_state)
  755. {
  756. m_state = new CrowdToolState();
  757. sample->setToolState(type(), m_state);
  758. }
  759. m_state->init(sample);
  760. }
  761. void CrowdTool::reset()
  762. {
  763. }
  764. void CrowdTool::handleMenu()
  765. {
  766. if (!m_state)
  767. return;
  768. CrowdToolParams* params = m_state->getToolParams();
  769. if (imguiCheck("Create Agents", m_mode == TOOLMODE_CREATE))
  770. m_mode = TOOLMODE_CREATE;
  771. if (imguiCheck("Move Target", m_mode == TOOLMODE_MOVE_TARGET))
  772. m_mode = TOOLMODE_MOVE_TARGET;
  773. if (imguiCheck("Select Agent", m_mode == TOOLMODE_SELECT))
  774. m_mode = TOOLMODE_SELECT;
  775. if (imguiCheck("Toggle Polys", m_mode == TOOLMODE_TOGGLE_POLYS))
  776. m_mode = TOOLMODE_TOGGLE_POLYS;
  777. imguiSeparatorLine();
  778. if (imguiCollapse("Options", 0, params->m_expandOptions))
  779. params->m_expandOptions = !params->m_expandOptions;
  780. if (params->m_expandOptions)
  781. {
  782. imguiIndent();
  783. if (imguiCheck("Optimize Visibility", params->m_optimizeVis))
  784. {
  785. params->m_optimizeVis = !params->m_optimizeVis;
  786. m_state->updateAgentParams();
  787. }
  788. if (imguiCheck("Optimize Topology", params->m_optimizeTopo))
  789. {
  790. params->m_optimizeTopo = !params->m_optimizeTopo;
  791. m_state->updateAgentParams();
  792. }
  793. if (imguiCheck("Anticipate Turns", params->m_anticipateTurns))
  794. {
  795. params->m_anticipateTurns = !params->m_anticipateTurns;
  796. m_state->updateAgentParams();
  797. }
  798. if (imguiCheck("Obstacle Avoidance", params->m_obstacleAvoidance))
  799. {
  800. params->m_obstacleAvoidance = !params->m_obstacleAvoidance;
  801. m_state->updateAgentParams();
  802. }
  803. if (imguiSlider("Avoidance Quality", &params->m_obstacleAvoidanceType, 0.0f, 3.0f, 1.0f))
  804. {
  805. m_state->updateAgentParams();
  806. }
  807. if (imguiCheck("Separation", params->m_separation))
  808. {
  809. params->m_separation = !params->m_separation;
  810. m_state->updateAgentParams();
  811. }
  812. if (imguiSlider("Separation Weight", &params->m_separationWeight, 0.0f, 20.0f, 0.01f))
  813. {
  814. m_state->updateAgentParams();
  815. }
  816. imguiUnindent();
  817. }
  818. if (imguiCollapse("Selected Debug Draw", 0, params->m_expandSelectedDebugDraw))
  819. params->m_expandSelectedDebugDraw = !params->m_expandSelectedDebugDraw;
  820. if (params->m_expandSelectedDebugDraw)
  821. {
  822. imguiIndent();
  823. if (imguiCheck("Show Corners", params->m_showCorners))
  824. params->m_showCorners = !params->m_showCorners;
  825. if (imguiCheck("Show Collision Segs", params->m_showCollisionSegments))
  826. params->m_showCollisionSegments = !params->m_showCollisionSegments;
  827. if (imguiCheck("Show Path", params->m_showPath))
  828. params->m_showPath = !params->m_showPath;
  829. if (imguiCheck("Show VO", params->m_showVO))
  830. params->m_showVO = !params->m_showVO;
  831. if (imguiCheck("Show Path Optimization", params->m_showOpt))
  832. params->m_showOpt = !params->m_showOpt;
  833. if (imguiCheck("Show Neighbours", params->m_showNeis))
  834. params->m_showNeis = !params->m_showNeis;
  835. imguiUnindent();
  836. }
  837. if (imguiCollapse("Debug Draw", 0, params->m_expandDebugDraw))
  838. params->m_expandDebugDraw = !params->m_expandDebugDraw;
  839. if (params->m_expandDebugDraw)
  840. {
  841. imguiIndent();
  842. if (imguiCheck("Show Labels", params->m_showLabels))
  843. params->m_showLabels = !params->m_showLabels;
  844. if (imguiCheck("Show Prox Grid", params->m_showGrid))
  845. params->m_showGrid = !params->m_showGrid;
  846. if (imguiCheck("Show Nodes", params->m_showNodes))
  847. params->m_showNodes = !params->m_showNodes;
  848. if (imguiCheck("Show Perf Graph", params->m_showPerfGraph))
  849. params->m_showPerfGraph = !params->m_showPerfGraph;
  850. if (imguiCheck("Show Detail All", params->m_showDetailAll))
  851. params->m_showDetailAll = !params->m_showDetailAll;
  852. imguiUnindent();
  853. }
  854. }
  855. void CrowdTool::handleClick(const float* s, const float* p, bool shift)
  856. {
  857. if (!m_sample) return;
  858. if (!m_state) return;
  859. InputGeom* geom = m_sample->getInputGeom();
  860. if (!geom) return;
  861. dtCrowd* crowd = m_sample->getCrowd();
  862. if (!crowd) return;
  863. if (m_mode == TOOLMODE_CREATE)
  864. {
  865. if (shift)
  866. {
  867. // Delete
  868. int ahit = m_state->hitTestAgents(s,p);
  869. if (ahit != -1)
  870. m_state->removeAgent(ahit);
  871. }
  872. else
  873. {
  874. // Add
  875. m_state->addAgent(p);
  876. }
  877. }
  878. else if (m_mode == TOOLMODE_MOVE_TARGET)
  879. {
  880. m_state->setMoveTarget(p, shift);
  881. }
  882. else if (m_mode == TOOLMODE_SELECT)
  883. {
  884. // Highlight
  885. int ahit = m_state->hitTestAgents(s,p);
  886. m_state->hilightAgent(ahit);
  887. }
  888. else if (m_mode == TOOLMODE_TOGGLE_POLYS)
  889. {
  890. dtNavMesh* nav = m_sample->getNavMesh();
  891. dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
  892. if (nav && navquery)
  893. {
  894. dtQueryFilter filter;
  895. const float* halfExtents = crowd->getQueryExtents();
  896. float tgt[3];
  897. dtPolyRef ref;
  898. navquery->findNearestPoly(p, halfExtents, &filter, &ref, tgt);
  899. if (ref)
  900. {
  901. unsigned short flags = 0;
  902. if (dtStatusSucceed(nav->getPolyFlags(ref, &flags)))
  903. {
  904. flags ^= SAMPLE_POLYFLAGS_DISABLED;
  905. nav->setPolyFlags(ref, flags);
  906. }
  907. }
  908. }
  909. }
  910. }
  911. void CrowdTool::handleStep()
  912. {
  913. if (!m_state) return;
  914. const float dt = 1.0f/20.0f;
  915. m_state->updateTick(dt);
  916. m_state->setRunning(false);
  917. }
  918. void CrowdTool::handleToggle()
  919. {
  920. if (!m_state) return;
  921. m_state->setRunning(!m_state->isRunning());
  922. }
  923. void CrowdTool::handleUpdate(const float dt)
  924. {
  925. rcIgnoreUnused(dt);
  926. }
  927. void CrowdTool::handleRender()
  928. {
  929. }
  930. void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
  931. {
  932. rcIgnoreUnused(model);
  933. rcIgnoreUnused(proj);
  934. // Tool help
  935. const int h = view[3];
  936. int ty = h-40;
  937. if (m_mode == TOOLMODE_CREATE)
  938. {
  939. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: add agent. Shift+LMB: remove agent.", imguiRGBA(255,255,255,192));
  940. }
  941. else if (m_mode == TOOLMODE_MOVE_TARGET)
  942. {
  943. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: set move target. Shift+LMB: adjust set velocity.", imguiRGBA(255,255,255,192));
  944. ty -= 20;
  945. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "Setting velocity will move the agents without pathfinder.", imguiRGBA(255,255,255,192));
  946. }
  947. else if (m_mode == TOOLMODE_SELECT)
  948. {
  949. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: select agent.", imguiRGBA(255,255,255,192));
  950. }
  951. ty -= 20;
  952. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "SPACE: Run/Pause simulation. 1: Step simulation.", imguiRGBA(255,255,255,192));
  953. ty -= 20;
  954. if (m_state && m_state->isRunning())
  955. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "- RUNNING -", imguiRGBA(255,32,16,255));
  956. else
  957. imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "- PAUSED -", imguiRGBA(255,255,255,128));
  958. }