← Back to branch summary

~rebel/horde3d/trunk

« back to all changes in this revision

Viewing changes to trunk/Tools/Dependencies/RecastNavigation/DetourCrowd/Source/DetourCrowd.cpp

  • Committer: felix
  • Date: 2015-07-07 12:57:07 UTC
  • Revision ID: svn-v4:5ce291ac-9df0-446f-9e4f-d57731c4dda7::1699
- Updated RecastNavigation to latest version and fixed multiple issues.
- Adapted GameDetourComponent, GameDetourCrowdComponent, DetourCrowdDemo and AAA accordingly.

Show diffs side-by-side

added added

removed removed

Lines of Context:
trunk/Tools/Dependencies/RecastNavigation/DetourCrowd/Source/DetourCrowd.cpp
17
17
//
18
18
 
19
19
#define _USE_MATH_DEFINES
20
 
#include <math.h>
21
20
#include <string.h>
22
21
#include <float.h>
23
22
#include <stdlib.h>
27
26
#include "DetourNavMeshQuery.h"
28
27
#include "DetourObstacleAvoidance.h"
29
28
#include "DetourCommon.h"
 
29
#include "DetourMath.h"
30
30
#include "DetourAssert.h"
31
31
#include "DetourAlloc.h"
32
32
 
206
206
                // Check for overlap.
207
207
                float diff[3];
208
208
                dtVsub(diff, pos, ag->npos);
209
 
                if (fabsf(diff[1]) >= (height+ag->params.height)/2.0f)
 
209
                if (dtMathFabsf(diff[1]) >= (height+ag->params.height)/2.0f)
210
210
                        continue;
211
211
                diff[1] = 0;
212
212
                const float distSqr = dtVlenSqr(diff);
441
441
        for (int i = 0; i < m_maxAgents; ++i)
442
442
        {
443
443
                new(&m_agents[i]) dtCrowdAgent();
444
 
                m_agents[i].active = 0;
 
444
                m_agents[i].active = false;
445
445
                if (!m_agents[i].corridor.init(m_maxPathResult))
446
446
                        return false;
447
447
        }
448
448
 
449
449
        for (int i = 0; i < m_maxAgents; ++i)
450
450
        {
451
 
                m_agentAnims[i].active = 0;
 
451
                m_agentAnims[i].active = false;
452
452
        }
453
453
 
454
454
        // The navquery is mostly used for local searches, no need for large node pool.
474
474
        return 0;
475
475
}
476
476
 
477
 
const int dtCrowd::getAgentCount() const
 
477
int dtCrowd::getAgentCount() const
478
478
{
479
479
        return m_maxAgents;
480
480
}
484
484
/// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
485
485
const dtCrowdAgent* dtCrowd::getAgent(const int idx)
486
486
{
 
487
        if (idx < 0 || idx >= m_maxAgents)
 
488
                return 0;
 
489
        return &m_agents[idx];
 
490
}
 
491
 
 
492
/// 
 
493
/// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
 
494
dtCrowdAgent* dtCrowd::getEditableAgent(const int idx)
 
495
{
 
496
        if (idx < 0 || idx >= m_maxAgents)
 
497
                return 0;
487
498
        return &m_agents[idx];
488
499
}
489
500
 
490
501
void dtCrowd::updateAgentParameters(const int idx, const dtCrowdAgentParams* params)
491
502
{
492
 
        if (idx < 0 || idx > m_maxAgents)
 
503
        if (idx < 0 || idx >= m_maxAgents)
493
504
                return;
494
505
        memcpy(&m_agents[idx].params, params, sizeof(dtCrowdAgentParams));
495
506
}
512
523
        if (idx == -1)
513
524
                return -1;
514
525
        
515
 
        dtCrowdAgent* ag = &m_agents[idx];
 
526
        dtCrowdAgent* ag = &m_agents[idx];              
516
527
 
 
528
        updateAgentParameters(idx, params);
 
529
        
517
530
        // Find nearest position on navmesh and place the agent there.
518
531
        float nearest[3];
519
 
        dtPolyRef ref;
520
 
        m_navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest);
 
532
        dtPolyRef ref = 0;
 
533
        dtVcopy(nearest, pos);
 
534
        dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
 
535
        if (dtStatusFailed(status))
 
536
        {
 
537
                dtVcopy(nearest, pos);
 
538
                ref = 0;
 
539
        }
521
540
        
522
541
        ag->corridor.reset(ref, nearest);
523
542
        ag->boundary.reset();
 
543
        ag->partial = false;
524
544
 
525
 
        updateAgentParameters(idx, params);
526
 
        
527
545
        ag->topologyOptTime = 0;
528
546
        ag->targetReplanTime = 0;
529
547
        ag->nneis = 0;
542
560
        
543
561
        ag->targetState = DT_CROWDAGENT_TARGET_NONE;
544
562
        
545
 
        ag->active = 1;
 
563
        ag->active = true;
546
564
 
547
565
        return idx;
548
566
}
555
573
{
556
574
        if (idx >= 0 && idx < m_maxAgents)
557
575
        {
558
 
                m_agents[idx].active = 0;
 
576
                m_agents[idx].active = false;
559
577
        }
560
578
}
561
579
 
562
580
bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos)
563
581
{
564
 
        if (idx < 0 || idx > m_maxAgents)
 
582
        if (idx < 0 || idx >= m_maxAgents)
565
583
                return false;
566
584
        
567
585
        dtCrowdAgent* ag = &m_agents[idx];
588
606
/// The request will be processed during the next #update().
589
607
bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
590
608
{
591
 
        if (idx < 0 || idx > m_maxAgents)
 
609
        if (idx < 0 || idx >= m_maxAgents)
592
610
                return false;
593
611
        if (!ref)
594
612
                return false;
610
628
 
611
629
bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
612
630
{
613
 
        if (idx < 0 || idx > m_maxAgents)
 
631
        if (idx < 0 || idx >= m_maxAgents)
614
632
                return false;
615
633
        
616
634
        dtCrowdAgent* ag = &m_agents[idx];
627
645
 
628
646
bool dtCrowd::resetMoveTarget(const int idx)
629
647
{
630
 
        if (idx < 0 || idx > m_maxAgents)
 
648
        if (idx < 0 || idx >= m_maxAgents)
631
649
                return false;
632
650
        
633
651
        dtCrowdAgent* ag = &m_agents[idx];
635
653
        // Initialize request.
636
654
        ag->targetRef = 0;
637
655
        dtVset(ag->targetPos, 0,0,0);
 
656
        dtVset(ag->dvel, 0,0,0);
638
657
        ag->targetPathqRef = DT_PATHQ_INVALID;
639
658
        ag->targetReplan = false;
640
659
        ag->targetState = DT_CROWDAGENT_TARGET_NONE;
683
702
                        dtPolyRef reqPath[MAX_RES];     // The path to the request location
684
703
                        int reqPathCount = 0;
685
704
 
686
 
                        // Quick seach towards the goal.
 
705
                        // Quick search towards the goal.
687
706
                        static const int MAX_ITER = 20;
688
 
                        m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter);
 
707
                        m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
689
708
                        m_navquery->updateSlicedFindPath(MAX_ITER, 0);
690
709
                        dtStatus status = 0;
691
710
                        if (ag->targetReplan) // && npath > 10)
705
724
                                if (reqPath[reqPathCount-1] != ag->targetRef)
706
725
                                {
707
726
                                        // Partial path, constrain target position inside the last polygon.
708
 
                                        status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos);
 
727
                                        status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
709
728
                                        if (dtStatusFailed(status))
710
729
                                                reqPathCount = 0;
711
730
                                }
729
748
 
730
749
                        ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
731
750
                        ag->boundary.reset();
 
751
                        ag->partial = false;
732
752
 
733
753
                        if (reqPath[reqPathCount-1] == ag->targetRef)
734
754
                        {
752
772
        {
753
773
                dtCrowdAgent* ag = queue[i];
754
774
                ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
755
 
                                                                                         ag->corridor.getTarget(), ag->targetPos, &m_filter);
 
775
                                                                                         ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->params.queryFilterType]);
756
776
                if (ag->targetPathqRef != DT_PATHQ_INVALID)
757
777
                        ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
758
778
        }
802
822
                                status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
803
823
                                if (dtStatusFailed(status) || !nres)
804
824
                                        valid = false;
805
 
                                
 
825
 
 
826
                                if (dtStatusDetail(status, DT_PARTIAL_RESULT))
 
827
                                        ag->partial = true;
 
828
                                else
 
829
                                        ag->partial = false;
 
830
 
806
831
                                // Merge result and existing path.
807
832
                                // The agent might have moved whilst the request is
808
833
                                // being processed, so the path may have changed.
849
874
                                        {
850
875
                                                // Partial path, constrain target position inside the last polygon.
851
876
                                                float nearest[3];
852
 
                                                status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest);
 
877
                                                status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
853
878
                                                if (dtStatusSucceed(status))
854
879
                                                        dtVcopy(targetPos, nearest);
855
880
                                                else
906
931
        for (int i = 0; i < nqueue; ++i)
907
932
        {
908
933
                dtCrowdAgent* ag = queue[i];
909
 
                ag->corridor.optimizePathTopology(m_navquery, &m_filter);
 
934
                ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
910
935
                ag->topologyOptTime = 0;
911
936
        }
912
937
 
923
948
                
924
949
                if (ag->state != DT_CROWDAGENT_STATE_WALKING)
925
950
                        continue;
926
 
 
927
 
                if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
928
 
                        continue;
929
951
                        
930
952
                ag->targetReplanTime += dt;
931
953
 
936
958
                float agentPos[3];
937
959
                dtPolyRef agentRef = ag->corridor.getFirstPoly();
938
960
                dtVcopy(agentPos, ag->npos);
939
 
                if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
 
961
                if (!m_navquery->isValidPolyRef(agentRef, &m_filters[ag->params.queryFilterType]))
940
962
                {
941
963
                        // Current location is not valid, try to reposition.
942
964
                        // TODO: this can snap agents, how to handle that?
943
965
                        float nearest[3];
 
966
                        dtVcopy(nearest, agentPos);
944
967
                        agentRef = 0;
945
 
                        m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
 
968
                        m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->params.queryFilterType], &agentRef, nearest);
946
969
                        dtVcopy(agentPos, nearest);
947
970
 
948
971
                        if (!agentRef)
949
972
                        {
950
973
                                // Could not find location in navmesh, set state to invalid.
951
974
                                ag->corridor.reset(0, agentPos);
 
975
                                ag->partial = false;
952
976
                                ag->boundary.reset();
953
977
                                ag->state = DT_CROWDAGENT_STATE_INVALID;
954
978
                                continue;
964
988
                        replan = true;
965
989
                }
966
990
 
 
991
                // If the agent does not have move target or is controlled by velocity, no need to recover the target nor replan.
 
992
                if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
 
993
                        continue;
 
994
 
967
995
                // Try to recover move request position.
968
996
                if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
969
997
                {
970
 
                        if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
 
998
                        if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filters[ag->params.queryFilterType]))
971
999
                        {
972
1000
                                // Current target is not valid, try to reposition.
973
1001
                                float nearest[3];
974
 
                                m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
 
1002
                                dtVcopy(nearest, ag->targetPos);
 
1003
                                ag->targetRef = 0;
 
1004
                                m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest);
975
1005
                                dtVcopy(ag->targetPos, nearest);
976
1006
                                replan = true;
977
1007
                        }
979
1009
                        {
980
1010
                                // Failed to reposition target, fail moverequest.
981
1011
                                ag->corridor.reset(agentRef, agentPos);
 
1012
                                ag->partial = false;
982
1013
                                ag->targetState = DT_CROWDAGENT_TARGET_NONE;
983
1014
                        }
984
1015
                }
985
1016
 
986
1017
                // If nearby corridor is not valid, replan.
987
 
                if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
 
1018
                if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->params.queryFilterType]))
988
1019
                {
989
1020
                        // Fix current path.
990
1021
//                      ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
1020
1051
        
1021
1052
        dtCrowdAgent** agents = m_activeAgents;
1022
1053
        int nagents = getActiveAgents(agents, m_maxAgents);
1023
 
        
 
1054
 
1024
1055
        // Check that all agents still have valid paths.
1025
1056
        checkPathValidity(agents, nagents, dt);
1026
1057
        
1051
1082
                // if it has become invalid.
1052
1083
                const float updateThr = ag->params.collisionQueryRange*0.25f;
1053
1084
                if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
1054
 
                        !ag->boundary.isValid(m_navquery, &m_filter))
 
1085
                        !ag->boundary.isValid(m_navquery, &m_filters[ag->params.queryFilterType]))
1055
1086
                {
1056
1087
                        ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
1057
 
                                                                m_navquery, &m_filter);
 
1088
                                                                m_navquery, &m_filters[ag->params.queryFilterType]);
1058
1089
                }
1059
1090
                // Query neighbour agents
1060
1091
                ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
1076
1107
                
1077
1108
                // Find corners for steering
1078
1109
                ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
1079
 
                                                                                                DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
 
1110
                                                                                                DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.queryFilterType]);
1080
1111
                
1081
1112
                // Check to see if the corner after the next corner is directly visible,
1082
1113
                // and short cut to there.
1083
1114
                if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
1084
1115
                {
1085
1116
                        const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
1086
 
                        ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
 
1117
                        ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->params.queryFilterType]);
1087
1118
                        
1088
1119
                        // Copy data for debug purposes.
1089
1120
                        if (debugIdx == i)
1118
1149
                if (overOffmeshConnection(ag, triggerRadius))
1119
1150
                {
1120
1151
                        // Prepare to off-mesh connection.
1121
 
                        const int idx = ag - m_agents;
 
1152
                        const int idx = (int)(ag - m_agents);
1122
1153
                        dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
1123
1154
                        
1124
1155
                        // Adjust the path over the off-mesh connection.
1128
1159
                        {
1129
1160
                                dtVcopy(anim->initPos, ag->npos);
1130
1161
                                anim->polyRef = refs[1];
1131
 
                                anim->active = 1;
 
1162
                                anim->active = true;
1132
1163
                                anim->t = 0.0f;
1133
1164
                                anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
1134
1165
                                
1200
1231
                                        continue;
1201
1232
                                if (distSqr > dtSqr(separationDist))
1202
1233
                                        continue;
1203
 
                                const float dist = sqrtf(distSqr);
 
1234
                                const float dist = dtMathSqrtf(distSqr);
1204
1235
                                const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
1205
1236
                                
1206
1237
                                dtVmad(disp, disp, diff, weight/dist);
1318
1349
                                float dist = dtVlenSqr(diff);
1319
1350
                                if (dist > dtSqr(ag->params.radius + nei->params.radius))
1320
1351
                                        continue;
1321
 
                                dist = sqrtf(dist);
 
1352
                                dist = dtMathSqrtf(dist);
1322
1353
                                float pen = (ag->params.radius + nei->params.radius) - dist;
1323
1354
                                if (dist < 0.0001f)
1324
1355
                                {
1363
1394
                        continue;
1364
1395
                
1365
1396
                // Move along navmesh.
1366
 
                ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
 
1397
                ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
1367
1398
                // Get valid constrained position back.
1368
1399
                dtVcopy(ag->npos, ag->corridor.getPos());
1369
1400
 
1371
1402
                if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
1372
1403
                {
1373
1404
                        ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
 
1405
                        ag->partial = false;
1374
1406
                }
1375
1407
 
1376
1408
        }
1387
1419
                if (anim->t > anim->tmax)
1388
1420
                {
1389
1421
                        // Reset animation
1390
 
                        anim->active = 0;
 
1422
                        anim->active = false;
1391
1423
                        // Prepare agent for walking.
1392
1424
                        ag->state = DT_CROWDAGENT_STATE_WALKING;
1393
1425
                        continue;
1413
1445
        }
1414
1446
        
1415
1447
}
1416
 
 
1417