detour 寻路核心逻辑 CrowdToolState::updateTick dtCrowd::update

CrowdToolState::updateTick(constfloatdt)

作为寻路模拟过程的主要函数主要就是干3件事：

1 crowd->update(dt, &m_agentDebug);

2 Update agent trails表示agent的足迹，不是寻路的必要部分，可以不用

3 m_agentDebug.vod->normalizeSamples();

后面两步都是用来demo展示用的

 

主要逻辑：

dtCrowd::update(constfloatdt,dtCrowdAgentDebugInfo*debug)：

 

当m_state->hilightAgent(ahit);选择了一个agent的时候，便设置了m_agentDebug.idx

debugIdx中就记录了我们点亮的是哪个agent

agent的active在remove的时候设置为false

 

接下来就是寻路部分：

 

关于寻路过程我的理解：

首先当接到request的时候，先进行短寻路

寻路不到target就进行长寻路

长寻路有迭代次数的限制不至于让一个tick走的太多

必须长寻路因为全靠短寻路结果可能根本不是最好的，可否一定能寻到目的地？

然后每隔一段时间进行optimize

为了处理出现误差和由于动态阻挡而导致的当前路径已经不是最优的情况

几个例子：之前被动态阻挡导致需要转弯，但是动态阻挡已经不在那了，这个时候隔一段时间进行一次优化是有意义的

optimize结果可能有两种：一是跟之前的路径能连上，另一种是连不上，连不上的话直接返回，也就是说没有进行优化。

那么如果当前的路径遇到了阻挡呢？就是在checkPathValidity中检测出来申请重新寻路。

 

步骤：

1 checkPathValidity

    总得来说就是先确保startpos和startRef的有效性

    以及在ag->corridor.fixPathStart确保path当中的第一个polygon是有效的

    其实就是让path[0]为agentRef

    而留下其他的不管，让replanner去调整

    其他还有三种需要replan的情况

2 updateMoveRequest

    InitSlicedFindPath 做寻路的初始化工作

    updateSlicedFindPath 主要工作：

            相当于进行了一次类似A*的短寻路，并且记录结果

    这个过程首先当接收到request的时候进行短寻路，最后收集寻路结果的时候，如果是刚才的replan的情况，也就是当前路径是正在被重新规划，则执行

    finalizeSlicedFindPathPartial尽可能使用已有路径，否则说明目标被重置，需要执行finalizeSlicedFindPath

    finalizeSlicedFindPath翻转了updateSlicedFindPath的类似A*寻路的路径结果并保存。

 

    经过短寻路没走到头的ag->targetState= DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE

    并且将agent addToPathQueue，然后再dtPathQueue::update中

    然后进行一次长寻路

    这次长寻路是从ag->corridor.getLastPoly()到ag->targetRef

    然后将两次寻路的结果merge到一块

    放到res中

    然后ag->corridor.setCorridor(targetPos, res,nres);

 

3 updateTopologyOptimization

      https://blog.csdn.net/icebergliu1234/article/details/88393735

 

4 对每一个agent的边界 进行更新，并且找到neibour放到neis里面

在m_grid分配一块item记录agent

for (inti= 0; i < nagents;++i)

{

    ap.collisionQueryRange = ap.radius * 12.0f;

表示多长的距离的移动算是移动了

updateThr =ag->params.collisionQueryRange*0.25f;是否进行update更新碰撞边界的阈值

 

ag->npos, ag->boundary.getCenter()这两个的距离超过了updateThr

或者ag->boundary中的poly有无效的，说明需要update

 

接下来更新边界

dtLocalBoundary::update

在移动了一定的距离后，更新碰撞边界

或者碰撞边界已经变得invalid

并且ag->nneis= getNeighbours查询neighbour agents并且记录在ag->neis里面

}

 

5 接下来找到要前往的corners

for (inti= 0; i < nagents;++i)

{

    ag->corridor.findCorners

    corners的信息被存储在dtCrowdAgent里的三个数组里面

    float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3];

unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS];目前是三种，起始,终点,offmeshconnection

    dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS];里面是poly的reference id

如果agent启用了DT_CROWD_OPTIMIZE_VIS也就是使用optimizePathVisibility来优化path

    做的事情就是shortcut到下一个可见的corner

}找到前往的corners完毕

 

 

6 接下来触发所有的off-meshconnections 这一段主要用来表现，所以可以由项目游戏模块自己来实现

for (inti= 0; i < nagents;++i)

{

    triggerRadius是触发半径

用overOffmeshConnection(constdtCrowdAgent*ag,constfloatradius)判断是否经过了OffmeshConnection

    如果是corners里面有OffmeshConnection，肯定是最后一个

    因为在findcorners里面就是这么设定的

    所以判断ag->cornerFlags[ag->ncorners-1]& DT_STRAIGHTPATH_OFFMESH_CONNECTION

    如果当前位置ag->npos与这个cornner的距离小于触发半径则overOffmeshConnection返回true

 

    if(overOffmeshConnection(ag,triggerRadius))

{

    说明triggerRadius范围内有OffmeshConnection

ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1],refs,anim->startPos,anim->endPos,m_navquery))

这个函数找到了OffmeshConnection的两个端点，作为anim->startPos和anim->endPos

    如果找到了

设置这个agent的dtCrowdAgentAnimation* anim

    设置播放时间tmax为以两倍ag->params.maxSpeed速度走完跳点的时间

    并且ag->state= DT_CROWDAGENT_STATE_OFFMESH;

}

 

 

}off-meshconnections触发完毕

 

7 接下来计算运动steering，根据配置来计算方向和速率：

for (inti= 0; i < nagents;++i)

{

    1  ag->targetState== DT_CROWDAGENT_TARGET_VELOCITY时候：

    ag->desiredSpeed= dtVlen(ag->targetPos);

    （crowd->requestMoveVelocity(i,vel);的时候设置的DT_CROWDAGENT_TARGET_VELOCITY

    见setMoveTarget）

    注意这里面ag->targetPos实际上在requestMoveVelocity是被设置成速度向量

    （calcVel(vel,ag->npos,p,ag->params.maxSpeed);）

 

    2  不是DT_CROWDAGENT_TARGET_VELOCITY的时候：

    如果使用了DT_CROWD_ANTICIPATE_TURNS转弯预测

calcSmoothSteerDirection光滑的移动方向计算也就是朝下下一个corner的向量的反方向偏移，形成一个圆滑的圆弧，

        dir[0]= dir0[0] - dir1[0]*len0*0.5f;

        dir[1] = 0;

        dir[2] = dir0[2]- dir1[2]*len0*0.5f;

        距离下一个corner越近，受到下下个corner的影响越小

0.5f可以被认为是一个圆滑系数，在0和1之间，0表示完全不圆滑，而1是不可以被设置的值，因为会“飞走”

否则

        calcStraightSteerDirection直接的移动方向计算朝向当前的corner

 

    设置slowDownRadius

    speedScale = getDistanceToGoal(ag,slowDownRadius)/ slowDownRadius;

    用来帮助agent在路径的最后降速

   关于getDistanceToGoal：https://blog.csdn.net/icebergliu1234/article/details/88386984

    == 

   

    所以speedScale在靠近最后的终点的时候会线性变小

    至此为止dvel都是归一化的方向

    dtVscale(dvel,dvel,ag->desiredSpeed* speedScale);

    也就是dvel乘以系数

 

}

 

8  Separation根据弹力散开  https://blog.csdn.net/icebergliu1234/article/details/88386999

DT_CROWD_SEPARATION表示是否要Separation

ag->neis是通过getNeighbours获取到的

 

==

在getNeighbours首先通过

grid->queryItems获取到一定范围内的item,在这里就是agents

然后通过距离的判断是否是neighbour

搜索半径：ag->params.collisionQueryRange

==

 

   

}

 

 

//至此steering算是算完了

把结果设置给ag->dvel

 

 

 

}计算运动steering完毕

 

 

 

 

9 接下来计算velocity

for(inti= 0; i < nagents;++i)

   

{

    1 如果这个agent是DT_CROWD_OBSTACLE_AVOIDANCE的

    动态阻挡

    对于这个agent的所有的neibour

    m_obstacleQuery->addCircle(nei->npos,nei->params.radius,nei->vel,nei->dvel);

    其中nei->vel是这个neibour的真实速度

    nei->dvel是将要达到的速度

    addCircle在dtObstacleCircle数组m_circles中使用一个位置并赋值

 

对于当前ag->boundary.getSegment

    addSegment在dtObstacleSegment数组m_segments中使用一个位置并赋值

   

 

    ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos,ag->params.radius,ag->desiredSpeed, ag->vel,ag->dvel,ag->nvel,params,vod);
 

prepare(pos,dvel);

dtObstacleAvoidanceQuery::prepare(constfloat*pos,constfloat*dvel)：

首先遍历所有circle

cir->p是圆心

cir->dp是我指向neibour的单位方向向量

dv是neibour与我的相对速度向量

cir->np是cir->dp的法线，与dv和cir->dp的方向有关

最后遍历所有线段判断我跟所有线段是否有touch

记录在seg->touch里面

准备完毕

 

m_params.horizTime？

m_vmax存这个agent的desireSpeed

 

 

    2 没有开启动态阻挡的：

    直接dtVcopy(ag->nvel,ag->dvel);

    Nvel就是考虑了DT_CROWD_OBSTACLE_AVOIDANCE的velocity

 

 

 

}velocity计算完毕

 

 

10 接下来对每一个agent进行integrate(ag,dt);

积分？

算出ag->npos应该移动的点

 

 

11接下来处理碰撞

for(int iter= 0; iter < 4; ++iter)进行四次迭代

{

for(inti= 0; i < nagents;++i)

    {

        ag->disp在这个过程中用于记录agent的偏移

 

        对于所有的neis求得与agent在xz平面的距离diff

        如果它们的距离小于两者半径的和

       

        float pen= (ag->params.radius+ nei->params.radius)- dist;

        表示插入了多少

       

        if(dist< 0.0001f)两个agent这种情况只能是一个在上一个在下

            则确保它们朝着不同方向分散

        否则pen =(1.0f/dist) * (pen*0.5f)* COLLISION_RESOLVE_FACTOR;

        可以认为pen是碰撞强度，与距离成反比，与插入值成正比

       

        dtVmad(ag->disp,ag->disp,diff,pen);

        ag->disp是位置偏移，diff是碰撞方向，pen是碰撞强度

       

w += 1.0f;记录我跟几个neibour有碰撞

如果w>1则偏移量ag->disp还要除以w

防止被多个agent碰撞合力过大的情况

 

}一个agent结束

 

 

    接下来对所有的agent真正进行位移

    dtVadd(ag->npos,ag->npos,ag->disp);

    ag->npos就是真正的agent的位置

}//碰撞迭代结束

 

 

12 接下来在navmesh上移动

for(inti= 0; i < nagents;++i)

{

 // Move along navmesh.
        ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);

 

在navigationmesh表面上移动

这个过程中corridor可能会发生变化以保持valid

新的位置将在调整之后的corridor的第一个poly里面

 

movePosition主要分两部分：

    1，  navquery->moveAlongSurface(m_path[0], m_pos,npos,filter,result,visited,&nvisited, MAX_VISITED);

    进行一次寻路过程，获得visited

   

    2 ， dtMergeCorridorStartMoved(m_path,m_npath,m_maxPath,visited,nvisited);

    注意visited里面furthestVisited之前的都不要了

 

 

 

    计算m_path[0]的y值并且赋值给m_pos

 

    最后if(ag->targetState== DT_CROWDAGENT_TARGET_NONE || ag->targetState== DT_CROWDAGENT_TARGET_VELOCITY)表示agent身上根本没有path

    也就是没有目标或者被速度控制的状态

    那么ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);

    这个函数让corridor的path[0]为ag->corridor.getFirstPoly()

    并且让corridor的pos和target都是ag->npos

 

}

 

最后update正在使用offmeshconnection的agent

for(inti= 0; i < m_maxAgents;++i)

{

        anim->t+= dt;

        如果动画播放完了则

        anim->active= false;重置动画

        ag->state= DT_CROWDAGENT_STATE_WALKING;接下来该走了

       

        否则ta= anim->tmax*0.15f;

        把动画分成两部分 0 到 ta, ta 到 tmax

        如果anim->t在第一部分

            constfloatu= tween(anim->t,0.0, ta);

            返回anim->t在0到ta之间的比值

            并且clamp到0和1之间

            然后dtVlerp(ag->npos,anim->initPos,anim->startPos,u);进行插值

        否则

            在ta,tb之间进行tween

            并且插值

}

 

 

}update完毕

 

 

 

 

 

 

 

===

 

dtMergeCorridorStartMoved(dtPolyRef*path,constintnpath,constintmaxPath, constdtPolyRef*visited,constintnvisited):

 

首先path和visited从后往前找path[i]== visited[j]

找到了之后

furthestPath= i;这个是path里面的下标

furthestVisited= j;这个是visited里面的下标

 

const int req= nvisited - furthestVisited;

const int orig= dtMin(furthestPath+1,npath);

接下来的就是

memmove(path+req,path+orig,size*sizeof(dtPolyRef));

for(inti= 0; i < req;++i)

        path[i]= visited[(nvisited-1)-i]; 

把visited里面furthestVisited之后的req个元素

连接到path里面furthestPath+1以及之后的元素前面

也就是原始的path前面的部分都不要了

 

 

 

注意与dtMergeCorridorStartShortcut区别，dtMergeCorridorStartShortcut中是：

visited里面的furthestVisited之前的部分

与path里面furthestPath以及后面的部分

连接起来

其req= furthestVisited;

visited是新的短寻路的结果，所以其实就是替换掉path中前面的已经不是足够optimized的部分

所以函数名里面有个start?

 

 

======

 

integrate(dtCrowdAgent*ag,constfloatdt)：

maxDelta用来表示一个agent可以有的最大加速度

dtVsub(dv,ag->nvel,ag->vel);dv用来表示速度变化之后与之前向量的差

maxDelta用来控制速度最大的变化

dtVadd(ag->vel,ag->vel,dv); ag->vel加上了限速之后的dv

dtVmad(ag->npos,ag->npos,ag->vel,dt);积分算出下一个点

 

 

======

 

 

calcSmoothSteerDirection(constdtCrowdAgent*ag,float*dir)：

获取下一个corner和下下个corner的点

分表是p0,p1

获取当前位置到它们的差dir0，dir1

并且获取dir0，dir1的长度len0，len1

然后按照下面的方法计算：

 

 

 

dtVscale(dir1,dir1,1.0f/len1);

   

    dir[0]= dir0[0] - dir1[0]*len0*0.5f;

    dir[1]= 0;

    dir[2]= dir0[2] - dir1[2]*len0*0.5f;

   

    dtVnormalize(dir);

   

 

也就是说dir会向着dir1的方向做偏移

最后将dir归一化

 

 

而另一个函数calcStraightSteerDirection(constdtCrowdAgent*ag,float*dir)：

直接用的dtVsub(dir,&ag->cornerVerts[0],ag->npos);

也就是直接指向第一个corner

 

====

CrowdToolState::setMoveTarget(constfloat* p, bool adjust)：

if(adjust)表示当按下shift键的时候

{

   

if(m_agentDebug.idx != -1) idx是在hilightAgent的时候设置的

    {

        calcVel(vel, ag->npos, p,ag->params.maxSpeed);

        vel就是在maxSpeed速度下移动的向量

        crowd->requestMoveVelocity(m_agentDebug.idx,vel);

       

 

 

}

}

calcVel(float*vel,constfloat*pos,constfloat*tgt,constfloatspeed):

从tgt到pos的向量

归一化之后

乘以speed

 

====

 

dtCrowd::requestMoveVelocity(constintidx,constfloat*vel)：

设置ag->targetPos为val

 

ag->targetPathqRef= DT_PATHQ_INVALID;

targetPathqRef记录的应该是dtPathQueue::request获取到的那个唯一ID （ref值）

设置成INVALID相当于不放在等待队列里面了

 

ag->targetReplan= false;

ag->targetState= DT_CROWDAGENT_TARGET_VELOCITY;

DT_CROWDAGENT_TARGET_VELOCITY的状态下update的时候不会重新进行规划

 

 

optimizePathVisibility (constfloat*next,constfloatpathOptimizationRange,

                                          dtNavMeshQuery*navquery,constdtQueryFilter*filter):

用visibilitysearch去优化corridor

 

dist= dtMin(dist+0.01f,pathOptimizationRange);

是当前点与next点的距离与pathOptimizationRange中的小值

 

只有当当前点和目标点是visible的并且比现在的路径更好的时候才会改变corridor

 

首先计算ray的长度，用来进行raycast

if(nres> 1 && t > 0.99f)就是说如果经过了poly并且点在最后实在poly里面，那就

dtMergeCorridorStartShortcut(m_path,m_npath,m_maxPath,res,nres);

 

-------- 

 

 

 

 

 

 

 

 

dtPathCorridor::findCorners(float* cornerVerts,unsignedchar*cornerFlags,

                              dtPolyRef*cornerPolys,constintmaxCorners,

                              dtNavMeshQuery*navquery,constdtQueryFilter*/*filter*/)

 

与dtNavMeshQuery::findStraightPath的功能本质上是一样的

findStraightPath得到conner信息，并进行修整（Prune）

首先如果两个conner离得太近要干掉一个

其次如果一个conner是DT_STRAIGHTPATH_OFFMESH_CONNECTION

那么后面的全干掉直接让ncorners= i+1;

 

-------------------------------------------

 

 

 

 

findStraightPath(constfloat*startPos,constfloat*endPos,

const dtPolyRef*path,const int pathSize,float*straightPath,

unsigned char* straightPathFlags,dtPolyRef* straightPathRefs, 

int* straightPathCount,const int maxStraightPath,const int options)：

 

end point关联的多边形id总是0

 

首先把startPos和endPos  clamp to其相应的poly里面去

closestPointOnPolyBoundary找到closestStartPos和closestEndPos

如果本身就在poly里面了则返回其本身

 

appendVertex来增加startPoint

 

遍历path

如果下一个node不是目标：

getPortalPoints获得path[i],path[i+1]的portalpoints

1 如果没有获取到说明path[i+1]is invalid polygon

这样就要把最后的endpoint Clamp to path[i]

closestPointOnPolyBoundary(path[i],endPos,closestEndPos)

然后把上一个node的当前poly的portal加入   appendVertex

然后把最后的closestEndPos加入

 

注意portal和之前的portalpoints的区别，这个是上一个node与当前node的portal点（straightPath中）和当前的endpos与left和right相交的点

是一个点，而不是left和right

 

如果已经到了最后一个node了，则

dtVcopy(left,closestEndPos);

dtVcopy(right,closestEndPos);

先把left和right设成closestEndPos

 

2 如果getPortalPoints获取到了portalpoints则:

 

 

 

对右侧点进行检查

if (dtTriArea2D(portalApex,portalRight,right)<= 0.0f)

{

    if(dtVequal(portalApex, portalRight)|| dtTriArea2D(portalApex,portalLeft,right)> 0.0f)

    {//满足这种条件的说明可以继续走而不用加portal

 

 

 

 

如果不满足条件，则leftIndex是上一次left能到达的位置，以portalLeft为endpos做射线，中间碰撞到的点都作为portal存起来（appendVertex）

 

Apex前移，也就是修改为left

把移动之后的portalApex也加入（appendVertex）

 

然后对

左侧点进行检查，处理方法是一样的

 

 

因为前面的判断条件是(i+1< pathSize)

所以最后一个节点还没有处理

所以要处理apexIndex,pathSize-1这一段的portal

然后把最后一个straightPath加进去

 

 

 

 

 

 

 

 

 

appendVertex就是根据输入的point根自己的straightPath[((*straightPathCount)-1)*3]

进行比较，如果一样，则updateflags and poly.

否则插入到

straightPath存的是pos

straightPathFlags

straightPathRefs

并且(*straightPathCount)++;

所以三个是一一对应的