BVB Source Codes

CRYENGINE Show CollisionAvoidanceSystem.cpp Source code

Return Download CRYENGINE: download CollisionAvoidanceSystem.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. #include "StdAfx.h"
  4. #include "CollisionAvoidanceSystem.h"
  5. #include "Walkability/WalkabilityCacheManager.h"
  6. #include "Navigation/NavigationSystem/NavigationSystem.h"
  7.  
  8. #include "DebugDrawContext.h"
  9.  
  10. //#pragma optimize("", off)
  11. //#pragma inline_depth(0)
  12.  
  13. CollisionAvoidanceSystem::CollisionAvoidanceSystem()
  14. {
  15. }
  16.  
  17. CollisionAvoidanceSystem::AgentID CollisionAvoidanceSystem::CreateAgent(tAIObjectID objectID)
  18. {
  19.         uint32 id = m_agents.size();
  20.         m_agents.resize(id + 1);
  21.  
  22.         m_agentAvoidanceVelocities.resize(m_agents.size());
  23.         m_agentAvoidanceVelocities[id].zero();
  24.  
  25.         m_agentObjectIDs.resize(m_agents.size());
  26.         m_agentObjectIDs[id] = objectID;
  27.  
  28.         m_agentNames.resize(m_agents.size());
  29.  
  30.         IAIObject* aiObject = gAIEnv.pAIObjectManager->GetAIObject(objectID);
  31.         m_agentNames[id] = aiObject->GetName();
  32.  
  33.         return id;
  34. }
  35.  
  36. CollisionAvoidanceSystem::ObstacleID CollisionAvoidanceSystem::CreateObstable()
  37. {
  38.         uint32 id = m_obstacles.size();
  39.         m_obstacles.resize(id + 1);
  40.  
  41.         return id;
  42. }
  43.  
  44. void CollisionAvoidanceSystem::RemoveAgent(AgentID agentID)
  45. {
  46. }
  47.  
  48. void CollisionAvoidanceSystem::RemoveObstacle(ObstacleID obstacleID)
  49. {
  50. }
  51.  
  52. void CollisionAvoidanceSystem::SetAgent(AgentID agentID, const Agent& params)
  53. {
  54.         m_agents[agentID] = params;
  55. }
  56.  
  57. const CollisionAvoidanceSystem::Agent& CollisionAvoidanceSystem::GetAgent(AgentID agentID) const
  58. {
  59.         return m_agents[agentID];
  60. }
  61.  
  62. void CollisionAvoidanceSystem::SetObstacle(ObstacleID obstacleID, const Obstacle& params)
  63. {
  64.         m_obstacles[obstacleID] = params;
  65. }
  66.  
  67. const CollisionAvoidanceSystem::Obstacle& CollisionAvoidanceSystem::GetObstacle(ObstacleID obstacleID) const
  68. {
  69.         return m_obstacles[obstacleID];
  70. }
  71.  
  72. const Vec2& CollisionAvoidanceSystem::GetAvoidanceVelocity(AgentID agentID)
  73. {
  74.         return m_agentAvoidanceVelocities[agentID];
  75. }
  76.  
  77. void CollisionAvoidanceSystem::Reset(bool bUnload)
  78. {
  79.         if (bUnload)
  80.         {
  81.                 stl::free_container(m_agents);
  82.                 stl::free_container(m_agentAvoidanceVelocities);
  83.                 stl::free_container(m_obstacles);
  84.  
  85.                 stl::free_container(m_agentObjectIDs);
  86.                 stl::free_container(m_agentNames);
  87.  
  88.                 stl::free_container(m_constraintLines);
  89.                 stl::free_container(m_nearbyAgents);
  90.                 stl::free_container(m_nearbyObstacles);
  91.         }
  92.         else
  93.         {
  94.                 m_agents.clear();
  95.                 m_agentAvoidanceVelocities.clear();
  96.                 m_obstacles.clear();
  97.  
  98.                 m_agentObjectIDs.clear();
  99.                 m_agentNames.clear();
  100.         }
  101. }
  102.  
  103. bool CollisionAvoidanceSystem::ClipPolygon(const Vec2* polygon, size_t vertexCount, const ConstraintLine& line, Vec2* output,
  104.                                            size_t* outputVertexCount) const
  105. {
  106.         bool shapeChanged = false;
  107.         const Vec2* outputStart = output;
  108.  
  109.         Vec2 v0 = polygon[0];
  110.         bool v0Side = line.direction.Cross(v0 - line.point) >= 0.0f;
  111.  
  112.         for (size_t i = 1; i <= vertexCount; ++i)
  113.         {
  114.                 const Vec2 v1 = polygon[i % vertexCount];
  115.                 const bool v1Side = line.direction.Cross(v1 - line.point) >= 0.0f;
  116.  
  117.                 if (v0Side && (v0Side == v1Side))
  118.                 {
  119.                         *(output++) = v1;
  120.                 }
  121.                 else if (v0Side != v1Side)
  122.                 {
  123.                         float det = line.direction.Cross(v1 - v0);
  124.  
  125.                         if (fabs_tpl(det) >= 0.000001f)
  126.                         {
  127.                                 shapeChanged = true;
  128.  
  129.                                 float detA = (v0 - line.point).Cross(v1 - v0);
  130.                                 float t = detA / det;
  131.  
  132.                                 *(output++) = line.point + line.direction * t;
  133.                         }
  134.  
  135.                         if (v1Side)
  136.                                 *(output++) = v1;
  137.                 }
  138.                 else
  139.                         shapeChanged = true;
  140.  
  141.                 v0 = v1;
  142.                 v0Side = v1Side;
  143.         }
  144.  
  145.         *outputVertexCount = static_cast<size_t>(output - outputStart);
  146.  
  147.         return shapeChanged;
  148. }
  149.  
  150. size_t CollisionAvoidanceSystem::ComputeFeasibleArea(const ConstraintLine* lines, size_t lineCount, float radius, Vec2* feasibleArea) const
  151. {
  152.         Vec2 buf0[FeasibleAreaMaxVertexCount];
  153.         Vec2* original = buf0;
  154.  
  155.         Vec2 buf1[FeasibleAreaMaxVertexCount];
  156.         Vec2* clipped = buf1;
  157.         Vec2* output = clipped;
  158.  
  159.         assert(3 + lineCount <= FeasibleAreaMaxVertexCount);
  160.  
  161.         const float HalfSize = 1.0f + radius;
  162.  
  163.         original[0] = Vec2(-HalfSize, HalfSize);
  164.         original[1] = Vec2(HalfSize, HalfSize);
  165.         original[2] = Vec2(HalfSize, -HalfSize);
  166.         original[3] = Vec2(-HalfSize, -HalfSize);
  167.  
  168.         size_t feasibleVertexCount = 4;
  169.         size_t outputCount = feasibleVertexCount;
  170.  
  171.         for (size_t i = 0; i < lineCount; ++i)
  172.         {
  173.                 const ConstraintLine& constraint = lines[i];
  174.  
  175.                 if (ClipPolygon(original, outputCount, constraint, clipped, &outputCount))
  176.                 {
  177.                         if (outputCount)
  178.                         {
  179.                                 output = clipped;
  180.                                 std::swap(original, clipped);
  181.                         }
  182.                         else
  183.                                 return 0;
  184.                 }
  185.         }
  186.  
  187.         PREFAST_SUPPRESS_WARNING(6385)
  188.         memcpy(feasibleArea, output, outputCount * sizeof(Vec2));
  189.  
  190.         return outputCount;
  191. }
  192.  
  193. bool CollisionAvoidanceSystem::ClipVelocityByFeasibleArea(const Vec2& velocity, Vec2* feasibleArea, size_t vertexCount,
  194.                                                           Vec2& output) const
  195. {
  196.         if (Overlap::Point_Polygon2D(velocity, feasibleArea, vertexCount))
  197.         {
  198.                 output = velocity;
  199.  
  200.                 return false;
  201.         }
  202.  
  203.         for (size_t i = 0; i < vertexCount; ++i)
  204.         {
  205.                 const Vec2 v0 = feasibleArea[i];
  206.                 const Vec2 v1 = feasibleArea[(i + 1) % vertexCount];
  207.  
  208.                 float a, b;
  209.                 if (Intersect::Lineseg_Lineseg2D(Lineseg(v0, v1), Lineseg(ZERO, velocity), a, b))
  210.                 {
  211.                         output = velocity * b;
  212.  
  213.                         return true;
  214.                 }
  215.         }
  216.  
  217.         output.zero();
  218.  
  219.         return true;
  220. }
  221.  
  222. size_t IntersectLineSegCircle(const Vec2& center, float radius, const Vec2& a, const Vec2& b, Vec2* output)
  223. {
  224.         const Vec2 v1 = b - a;
  225.         const float lineSegmentLengthSq = v1.GetLength2();
  226.         if (lineSegmentLengthSq < 0.00001f)
  227.                 return 0;
  228.  
  229.         const Vec2 v2 = center - a;
  230.         const float radiusSq = sqr(radius);
  231.  
  232.         const float dot = v1.Dot(v2);
  233.         const float dotProdOverLength = (dot / lineSegmentLengthSq);
  234.         const Vec2 proj1 = Vec2((dotProdOverLength * v1.x), (dotProdOverLength * v1.y));
  235.         const Vec2 midpt = Vec2(a.x + proj1.x, a.y + proj1.y);
  236.  
  237.         float distToCenterSq = sqr(midpt.x - center.x) + sqr(midpt.y - center.y);
  238.         if (distToCenterSq > radiusSq)
  239.                 return 0;
  240.  
  241.         if (fabs_tpl(distToCenterSq - radiusSq) < 0.00001f)
  242.         {
  243.                 output[0] = midpt;
  244.  
  245.                 return 1;
  246.         }
  247.  
  248.         float distToIntersection;
  249.         if (fabs_tpl(distToCenterSq) < 0.00001f)
  250.                 distToIntersection = radius;
  251.         else
  252.         {
  253.                 distToIntersection = sqrt_tpl(radiusSq - distToCenterSq);
  254.         }
  255.  
  256.         const Vec2 vIntersect = v1.GetNormalized() * distToIntersection;
  257.  
  258.         size_t resultCount = 0;
  259.         const Vec2 solution1 = midpt + vIntersect;
  260.         if ((solution1.x - a.x) * vIntersect.x + (solution1.y - a.y) * vIntersect.y > 0.0f &&
  261.             (solution1.x - b.x) * vIntersect.x + (solution1.y - b.y) * vIntersect.y < 0.0f)
  262.         {
  263.                 output[resultCount++] = solution1;
  264.         }
  265.  
  266.         const Vec2 solution2 = midpt - vIntersect;
  267.         if ((solution2.x - a.x) * vIntersect.x + (solution2.y - a.y) * vIntersect.y > 0.0f &&
  268.             (solution2.x - b.x) * vIntersect.x + (solution2.y - b.y) * vIntersect.y < 0.0f)
  269.         {
  270.                 output[resultCount++] = solution2;
  271.         }
  272.  
  273.         return resultCount;
  274. }
  275.  
  276. size_t CollisionAvoidanceSystem::ComputeOptimalAvoidanceVelocity(Vec2* feasibleArea, size_t vertexCount, const Agent& agent,
  277.                                                                  const float minSpeed, const float maxSpeed, CandidateVelocity* output) const
  278. {
  279.         const Vec2& desiredVelocity = agent.desiredVelocity;
  280.         const Vec2& currentVelocity = agent.currentVelocity;
  281.         if (vertexCount > 2)
  282.         {
  283.                 Vec2 velocity;
  284.  
  285.                 if (!ClipVelocityByFeasibleArea(desiredVelocity, feasibleArea, vertexCount, velocity))
  286.                 {
  287.                         output[0].velocity = desiredVelocity;
  288.                         output[0].distanceSq = 0.0f;
  289.  
  290.                         return 1;
  291.                 }
  292.                 else
  293.                 {
  294.                         const float MinSpeedSq = sqr(minSpeed);
  295.                         const float MaxSpeedSq = sqr(maxSpeed);
  296.  
  297.                         size_t candidateCount = 0;
  298.  
  299.                         float clippedSpeedSq = velocity.GetLength2();
  300.  
  301.                         if (clippedSpeedSq > MinSpeedSq)
  302.                         {
  303.                                 output[candidateCount].velocity = velocity;
  304.                                 output[candidateCount++].distanceSq = (velocity - desiredVelocity).GetLength2();
  305.                         }
  306.  
  307.                         Vec2 intersections[2];
  308.                         size_t intersectionCount = 0;
  309.                         const Vec2 center(ZERO);
  310.  
  311.                         for (size_t i = 0; i < vertexCount; ++i)
  312.                         {
  313.                                 const Vec2& a = feasibleArea[i];
  314.                                 const Vec2& b = feasibleArea[(i + 1) % vertexCount];
  315.  
  316.                                 const float aLenSq = a.GetLength2();
  317.                                 if ((aLenSq <= MaxSpeedSq) && (aLenSq > .0f))
  318.                                 {
  319.                                         output[candidateCount].velocity = a;
  320.                                         output[candidateCount++].distanceSq = (a - desiredVelocity).GetLength2();
  321.                                 }
  322.  
  323.                                 intersectionCount = IntersectLineSegCircle(center, maxSpeed, a, b, intersections);
  324.  
  325.                                 for (size_t ii = 0; ii < intersectionCount; ++ii)
  326.                                 {
  327.                                         const Vec2 candidateVelocity = intersections[ii];
  328.  
  329.                                         if ((candidateVelocity.GetLength2() > MinSpeedSq) /*&& (candidateVelocity.Dot(desiredVelocity) > 0.0f)*/)
  330.                                         {
  331.                                                 output[candidateCount].velocity = candidateVelocity;
  332.                                                 output[candidateCount++].distanceSq = (candidateVelocity - desiredVelocity).GetLength2();
  333.                                         }
  334.                                 }
  335.                         }
  336.  
  337.                         if (candidateCount)
  338.                         {
  339.                                 std::sort(&output[0], &output[0] + candidateCount);
  340.  
  341.                                 return candidateCount;
  342.                         }
  343.                 }
  344.         }
  345.  
  346.         return 0;
  347. }
  348.  
  349. bool CollisionAvoidanceSystem::FindFirstWalkableVelocity(AgentID agentID, CandidateVelocity* candidates,
  350.                                                          size_t candidateCount, Vec2& output) const
  351. {
  352.         const Agent& agent = m_agents[agentID];
  353.         const tAIObjectID aiObjectID = m_agentObjectIDs[agentID];
  354.  
  355.         CAIObject* aiObject = gAIEnv.pObjectContainer->GetAIObject(aiObjectID);
  356.         const CAIActor* actor = aiObject->CastToCAIActor();
  357.         if (!actor)
  358.                 return false;
  359.  
  360.         for (size_t i = 0; i < candidateCount; ++i)
  361.         {
  362.                 CandidateVelocity& candidate = candidates[i];
  363.  
  364.                 const Vec3 from = agent.currentLocation;
  365.                 const Vec3 to = agent.currentLocation + Vec3(candidate.velocity.x * 0.125f, candidate.velocity.y * 0.125f, 0.0f);
  366.  
  367.                 output = ClampSpeedWithNavigationMesh(actor->GetNavigationTypeID(), agent.currentLocation, agent.currentVelocity, candidate.velocity);
  368.                 if (output.GetLength2() < 0.1)
  369.                         continue;
  370.                 return true;
  371.         }
  372.  
  373.         return false;
  374. }
  375.  
  376. void CollisionAvoidanceSystem::Update(float updateTime)
  377. {
  378.         Agents::iterator it = m_agents.begin();
  379.         Agents::iterator end = m_agents.end();
  380.         size_t index = 0;
  381.  
  382.         const bool debugDraw = gAIEnv.CVars.DebugDraw > 0;
  383.         const float Epsilon = 0.00001f;
  384.         const size_t MaxAgentsConsidered = 8;
  385.  
  386.         for (; it != end; ++it, ++index)
  387.         {
  388.                 Agent& agent = *it;
  389.  
  390.                 Vec2& newVelocity = m_agentAvoidanceVelocities[index];
  391.                 newVelocity = agent.desiredVelocity;
  392.  
  393.                 float desiredSpeedSq = agent.desiredVelocity.GetLength2();
  394.                 if (desiredSpeedSq < Epsilon)
  395.                         continue;
  396.  
  397.                 m_constraintLines.clear();
  398.                 m_nearbyAgents.clear();
  399.                 m_nearbyObstacles.clear();
  400.  
  401.                 const float range = gAIEnv.CVars.CollisionAvoidanceRange;
  402.  
  403.                 ComputeNearbyObstacles(agent, index, range, m_nearbyObstacles);
  404.                 ComputeNearbyAgents(agent, index, range, m_nearbyAgents);
  405.  
  406.                 size_t obstacleConstraintCount = ComputeConstraintLinesForAgent(agent, index, 1.0f, m_nearbyAgents, MaxAgentsConsidered,
  407.                                                                                 m_nearbyObstacles, m_constraintLines);
  408.  
  409.                 size_t agentConstraintCount = m_constraintLines.size() - obstacleConstraintCount;
  410.                 size_t constraintCount = m_constraintLines.size();
  411.                 size_t considerCount = agentConstraintCount;
  412.  
  413.                 if (!constraintCount)
  414.                         continue;
  415.  
  416.                 Vec2 candidate = agent.desiredVelocity;
  417.                 // TODO: as a temporary solution, avoid to reset the new Velocity.
  418.                 // In this case if no ORCA speed can be found, we use our desired one
  419.                 //newVelocity.zero();
  420.  
  421.                 Vec2 feasibleArea[FeasibleAreaMaxVertexCount];
  422.                 size_t vertexCount = ComputeFeasibleArea(&m_constraintLines.front(), constraintCount, agent.maxSpeed,
  423.                                                          feasibleArea);
  424.  
  425.                 float minSpeed = gAIEnv.CVars.CollisionAvoidanceMinSpeed;
  426.  
  427.                 CandidateVelocity candidates[FeasibleAreaMaxVertexCount + 1]; // +1 for clipped desired velocity
  428.                 size_t candidateCount = ComputeOptimalAvoidanceVelocity(feasibleArea, vertexCount, agent, minSpeed, agent.maxSpeed, &candidates[0]);
  429.  
  430.                 if (!candidateCount || !FindFirstWalkableVelocity(index, candidates, candidateCount, newVelocity))
  431.                 {
  432.                         m_constraintLines.clear();
  433.  
  434.                         obstacleConstraintCount = ComputeConstraintLinesForAgent(agent, index, 0.25f, m_nearbyAgents, considerCount,
  435.                                                                                  m_nearbyObstacles, m_constraintLines);
  436.  
  437.                         agentConstraintCount = m_constraintLines.size() - obstacleConstraintCount;
  438.                         constraintCount = m_constraintLines.size();
  439.  
  440.                         while (considerCount > 0)
  441.                         {
  442.                                 vertexCount = ComputeFeasibleArea(&m_constraintLines.front(), m_constraintLines.size(), agent.maxSpeed,
  443.                                                                   feasibleArea);
  444.  
  445.                                 candidateCount = ComputeOptimalAvoidanceVelocity(feasibleArea, vertexCount, agent, minSpeed, agent.maxSpeed, &candidates[0]);
  446.  
  447.                                 if (candidateCount && !FindFirstWalkableVelocity(index, candidates, candidateCount, newVelocity))
  448.                                         break;
  449.  
  450.                                 if (m_nearbyAgents.empty())
  451.                                         break;
  452.  
  453.                                 const NearbyAgent& furthestNearbyAgent = m_nearbyAgents[considerCount - 1];
  454.                                 const Agent& furthestAgent = m_agents[furthestNearbyAgent.agentID];
  455.  
  456.                                 if (furthestNearbyAgent.distanceSq <= sqr(agent.radius + agent.radius + furthestAgent.radius))
  457.                                         break;
  458.  
  459.                                 --considerCount;
  460.                                 --constraintCount;
  461.                         }
  462.                 }
  463.  
  464.                 if (debugDraw)
  465.                 {
  466.                         if (IAIObject* object = gAIEnv.pAIObjectManager->GetAIObject(m_agentObjectIDs[index]))
  467.                         {
  468.                                 if (CAIActor* actor = object->CastToCAIActor())
  469.                                 {
  470.                                         if (*gAIEnv.CVars.DebugDrawCollisionAvoidanceAgentName &&
  471.                                             !stricmp(actor->GetName(), gAIEnv.CVars.DebugDrawCollisionAvoidanceAgentName))
  472.                                         {
  473.                                                 Vec3 agentLocation = actor->GetPhysicsPos();
  474.  
  475.                                                 CDebugDrawContext dc;
  476.  
  477.                                                 dc->DrawCircleOutline(agentLocation, agent.maxSpeed, Col_Blue);
  478.  
  479.                                                 dc->SetBackFaceCulling(false);
  480.                                                 dc->SetAlphaBlended(true);
  481.  
  482.                                                 Vec3 polygon3D[128];
  483.  
  484.                                                 for (size_t i = 0; i < vertexCount; ++i)
  485.                                                         polygon3D[i] = Vec3(agentLocation.x + feasibleArea[i].x, agentLocation.y + feasibleArea[i].y,
  486.                                                                             agentLocation.z + 0.005f);
  487.  
  488.                                                 ColorB polyColor(255, 255, 255, 128);
  489.                                                 polyColor.a = 96;
  490.  
  491.                                                 for (size_t i = 2; i < vertexCount; ++i)
  492.                                                         gEnv->pRenderer->GetIRenderAuxGeom()->DrawTriangle(polygon3D[0], polyColor, polygon3D[i - 1], polyColor,
  493.                                                                                                            polygon3D[i], polyColor);
  494.  
  495.                                                 ConstraintLines::iterator fit = m_constraintLines.begin();
  496.                                                 ConstraintLines::iterator fend = m_constraintLines.begin() + constraintCount;
  497.  
  498.                                                 ColorB lineColor[12] = {
  499.                                                         ColorB(Col_Orange,        0.5f),
  500.                                                         ColorB(Col_Tan,           0.5f),
  501.                                                         ColorB(Col_NavyBlue,      0.5f),
  502.                                                         ColorB(Col_Green,         0.5f),
  503.                                                         ColorB(Col_BlueViolet,    0.5f),
  504.                                                         ColorB(Col_IndianRed,     0.5f),
  505.                                                         ColorB(Col_ForestGreen,   0.5f),
  506.                                                         ColorB(Col_DarkSlateGrey, 0.5f),
  507.                                                         ColorB(Col_Turquoise,     0.5f),
  508.                                                         ColorB(Col_Gold,          0.5f),
  509.                                                         ColorB(Col_Khaki,         0.5f),
  510.                                                         ColorB(Col_CadetBlue,     0.5f),
  511.                                                 };
  512.  
  513.                                                 for (; fit != fend; ++fit)
  514.                                                 {
  515.                                                         const ConstraintLine& line = *fit;
  516.  
  517.                                                         ColorB color = lineColor[fit->objectID % 12];
  518.  
  519.                                                         if (line.flags & ConstraintLine::ObstacleConstraint)
  520.                                                                 color = Col_Grey;
  521.  
  522.                                                         DebugDrawConstraintLine(agentLocation, line, color);
  523.                                                 }
  524.                                         }
  525.                                 }
  526.                         }
  527.                 }
  528.         }
  529. }
  530.  
  531. size_t CollisionAvoidanceSystem::ComputeNearbyAgents(const Agent& agent, size_t agentIndex, float range,
  532.                                                      NearbyAgents& nearbyAgents) const
  533. {
  534.         const float Epsilon = 0.00001f;
  535.  
  536.         Agents::const_iterator ait = m_agents.begin();
  537.         Agents::const_iterator aend = m_agents.end();
  538.         size_t nearbyAgentIndex = 0;
  539.  
  540.         Vec3 agentLocation = agent.currentLocation;
  541.         Vec2 agentLookDirection = agent.currentLookDirection;
  542.  
  543.         for (; ait != aend; ++ait, ++nearbyAgentIndex)
  544.         {
  545.                 if (agentIndex != nearbyAgentIndex)
  546.                 {
  547.                         const Agent& otherAgent = *ait;
  548.  
  549.                         const Vec2 relativePosition = Vec2(otherAgent.currentLocation) - Vec2(agentLocation);
  550.                         const float distanceSq = relativePosition.GetLength2();
  551.  
  552.                         const bool nearby = distanceSq < sqr(range + otherAgent.radius);
  553.                         const bool sameFloor = fabs_tpl(otherAgent.currentLocation.z - agentLocation.z) < 2.0f;
  554.  
  555.                         //Note: For some reason different agents end with the same location,
  556.                         //yet the source of the problem has to be found
  557.                         const bool ignore = (distanceSq < 0.0001f);
  558.  
  559.                         if (nearby && sameFloor && !ignore)
  560.                         {
  561.                                 Vec2 direction = relativePosition.GetNormalized();
  562.                                 bool isVisible = agentLookDirection.Dot(Vec2(otherAgent.currentLocation) - (Vec2(agentLocation) - (direction * otherAgent.radius))) > 0.0f;
  563.  
  564.                                 //if (isVisible)
  565.                                 {
  566.                                         bool isMoving = otherAgent.desiredVelocity.GetLength2() >= Epsilon;
  567.                                         bool canSeeMe = true;//otherAgent.currentLookDirection.Dot(agentLocation - (otherAgent.currentLocation + (direction * agent.radius))) > 0.0f;
  568.  
  569.                                         nearbyAgents.push_back(NearbyAgent(distanceSq, static_cast<uint16>(nearbyAgentIndex),
  570.                                                                            (canSeeMe ? NearbyAgent::CanSeeMe : 0)
  571.                                                                            | (isMoving ? NearbyAgent::IsMoving : 0)));
  572.                                 }
  573.                         }
  574.                 }
  575.         }
  576.  
  577.         std::sort(nearbyAgents.begin(), nearbyAgents.end());
  578.  
  579.         return nearbyAgents.size();
  580. }
  581.  
  582. size_t CollisionAvoidanceSystem::ComputeNearbyObstacles(const Agent& agent, size_t agentIndex, float range,
  583.                                                         NearbyObstacles& nearbyObstacles) const
  584. {
  585.         const float Epsilon = 0.00001f;
  586.  
  587.         Obstacles::const_iterator oit = m_obstacles.begin();
  588.         Obstacles::const_iterator oend = m_obstacles.end();
  589.         uint16 obstacleIndex = 0;
  590.  
  591.         Vec3 agentLocation = agent.currentLocation;
  592.         Vec2 agentLookDirection = agent.currentLookDirection;
  593.  
  594.         for (; oit != oend; ++oit, ++obstacleIndex)
  595.         {
  596.                 const Obstacle& obstacle = *oit;
  597.  
  598.                 const Vec2 relativePosition = Vec2(obstacle.currentLocation) - Vec2(agentLocation);
  599.                 const float distanceSq = relativePosition.GetLength2();
  600.  
  601.                 const bool nearby = distanceSq < sqr(range + obstacle.radius);
  602.                 const bool sameFloor = fabs_tpl(obstacle.currentLocation.z - agentLocation.z) < 2.0f;
  603.  
  604.                 if (nearby && sameFloor)
  605.                 {
  606.                         Vec2 direction = relativePosition.GetNormalized();
  607.  
  608.                         //if (agent.currentLookDirection.Dot(relativePosition - (direction * obstacle.radius)) > 0.0f)
  609.                         nearbyObstacles.push_back(NearbyObstacle(distanceSq, obstacleIndex));
  610.                 }
  611.         }
  612.  
  613.         return nearbyObstacles.size();
  614. }
  615.  
  616. size_t CollisionAvoidanceSystem::ComputeConstraintLinesForAgent(const Agent& agent, size_t agentIndex, float timeHorizonScale,
  617.                                                                 NearbyAgents& nearbyAgents, size_t maxAgentsConsidered, NearbyObstacles& nearbyObstacles, ConstraintLines& lines) const
  618. {
  619.         const float Epsilon = 0.00001f;
  620.  
  621.         NearbyObstacles::const_iterator oit = nearbyObstacles.begin();
  622.         NearbyObstacles::const_iterator oend = nearbyObstacles.end();
  623.  
  624.         size_t obstacleCount = 0;
  625.  
  626.         for (; oit != oend; ++oit)
  627.         {
  628.                 const NearbyObstacle& nearbyObstacle = *oit;
  629.                 const Obstacle& obstacle = m_obstacles[nearbyObstacle.obstacleID];
  630.  
  631.                 ConstraintLine line;
  632.                 line.flags = ConstraintLine::ObstacleConstraint;
  633.                 line.objectID = nearbyObstacle.obstacleID;
  634.  
  635.                 ComputeObstacleConstraintLine(agent, obstacle, timeHorizonScale, line);
  636.  
  637.                 lines.push_back(line);
  638.                 ++obstacleCount;
  639.         }
  640.  
  641.         NearbyAgents::const_iterator ait = nearbyAgents.begin();
  642.         NearbyAgents::const_iterator aend = nearbyAgents.begin() + std::min<size_t>(nearbyAgents.size(), maxAgentsConsidered);
  643.  
  644.         for (; ait != aend; ++ait)
  645.         {
  646.                 const NearbyAgent& nearbyAgent = *ait;
  647.                 const Agent& otherAgent = m_agents[nearbyAgent.agentID];
  648.  
  649.                 ConstraintLine line;
  650.                 line.objectID = nearbyAgent.agentID;
  651.                 line.flags = ConstraintLine::AgentConstraint;
  652.  
  653.                 if (nearbyAgent.flags & NearbyAgent::IsMoving)
  654.                         ComputeAgentConstraintLine(agent, otherAgent, true, timeHorizonScale, line);
  655.                 else
  656.                 {
  657.                         Obstacle obstacle;
  658.                         obstacle.currentLocation = otherAgent.currentLocation;
  659.                         obstacle.radius = otherAgent.radius;
  660.  
  661.                         ComputeObstacleConstraintLine(agent, obstacle, timeHorizonScale, line);
  662.                 }
  663.  
  664.                 lines.push_back(line);
  665.         }
  666.  
  667.         return obstacleCount;
  668. }
  669.  
  670. void CollisionAvoidanceSystem::ComputeObstacleConstraintLine(const Agent& agent, const Obstacle& obstacle,
  671.                                                              float timeHorizonScale, ConstraintLine& line) const
  672. {
  673.         const Vec2 relativePosition = Vec2(obstacle.currentLocation) - Vec2(agent.currentLocation);
  674.  
  675.         const float distanceSq = relativePosition.GetLength2();
  676.         const float radii = agent.radius + obstacle.radius;
  677.         const float radiiSq = sqr(radii);
  678.  
  679.         static const float heuristicWeightForDistance = 0.01f;
  680.         static const float minimumTimeHorizonScale = 0.25f;
  681.         const float adjustedTimeHorizonScale = max(min(timeHorizonScale, (heuristicWeightForDistance * distanceSq)), minimumTimeHorizonScale);
  682.         const float TimeHorizon = gAIEnv.CVars.CollisionAvoidanceObstacleTimeHorizon * adjustedTimeHorizonScale;
  683.         const float TimeStep = gAIEnv.CVars.CollisionAvoidanceTimeStep;
  684.  
  685.         const float invTimeHorizon = 1.0f / TimeHorizon;
  686.         const float invTimeStep = 1.0f / TimeStep;
  687.  
  688.         Vec2 u;
  689.  
  690.         const Vec2 cutoffCenter = relativePosition * invTimeHorizon;
  691.  
  692.         if (distanceSq > radiiSq)
  693.         {
  694.                 Vec2 w = agent.desiredVelocity - cutoffCenter;
  695.                 const float wLenSq = w.GetLength2();
  696.  
  697.                 const float dot = w.Dot(relativePosition);
  698.  
  699.                 // compute closest point from relativeVelocity to the velocity object boundary
  700.                 if ((dot < 0.0f) && (sqr(dot) > radiiSq * wLenSq))
  701.                 {
  702.                         // w is pointing backwards from cone apex direction
  703.                         // closest point lies on the cutoff arc
  704.                         const float wLen = sqrt_tpl(wLenSq);
  705.                         w /= wLen;
  706.  
  707.                         line.direction = Vec2(w.y, -w.x);
  708.                         line.point = cutoffCenter + (radii * invTimeHorizon) * w;
  709.                 }
  710.                 else
  711.                 {
  712.                         // w is pointing into the cone
  713.                         // closest point is on an edge
  714.                         const float edge = sqrt_tpl(distanceSq - radiiSq);
  715.  
  716.                         if (LeftOf(relativePosition, w) > 0.0f)
  717.                         {
  718.                                 // left edge
  719.                                 line.direction = Vec2(relativePosition.x * edge - relativePosition.y * radii,
  720.                                                       relativePosition.x * radii + relativePosition.y * edge) / distanceSq;
  721.                         }
  722.                         else
  723.                         {
  724.                                 // right edge
  725.                                 line.direction = -Vec2(relativePosition.x * edge + relativePosition.y * radii,
  726.                                                        -relativePosition.x * radii + relativePosition.y * edge) / distanceSq;
  727.                         }
  728.  
  729.                         line.point = cutoffCenter + radii * invTimeHorizon * Vec2(-line.direction.y, line.direction.x);
  730.                 }
  731.         }
  732.         else if (distanceSq > 0.00001f)
  733.         {
  734.                 const float distance = sqrt_tpl(distanceSq);
  735.                 const Vec2 w = relativePosition / distance;
  736.  
  737.                 line.direction = Vec2(-w.y, w.x);
  738.  
  739.                 const Vec2 point = ((radii - distance) * invTimeStep) * w;
  740.                 const float dot = (agent.currentVelocity - point).Dot(line.direction);
  741.                 line.point = cutoffCenter + obstacle.radius * invTimeHorizon * Vec2(-line.direction.y, line.direction.x);
  742.         }
  743.         else
  744.         {
  745.                 const Vec2 w = agent.currentVelocity.GetNormalizedSafe(Vec2(0.0f, 1.0f));
  746.                 line.direction = Vec2(-w.y, w.x);
  747.  
  748.                 const float dot = agent.currentVelocity.Dot(line.direction);
  749.                 line.point = dot * line.direction - agent.currentVelocity;
  750.         }
  751. }
  752.  
  753. Vec2 CollisionAvoidanceSystem::ClampSpeedWithNavigationMesh(const NavigationAgentTypeID agentTypeID, const Vec3 agentPosition,
  754.                                                             const Vec2& currentVelocity, const Vec2& velocityToClamp) const
  755. {
  756.         Vec2 outputVelocity = velocityToClamp;
  757.         if (gAIEnv.CVars.CollisionAvoidanceClampVelocitiesWithNavigationMesh == 1)
  758.         {
  759.                 const float TimeHorizon = 0.25f * gAIEnv.CVars.CollisionAvoidanceAgentTimeHorizon;
  760.                 const float invTimeHorizon = 1.0f / gAIEnv.CVars.CollisionAvoidanceAgentTimeHorizon;
  761.                 const float TimeStep = gAIEnv.CVars.CollisionAvoidanceTimeStep;
  762.  
  763.                 const Vec3 from = agentPosition;
  764.                 const Vec3 to = agentPosition + Vec3(velocityToClamp.x, velocityToClamp.y, 0.0f);
  765.  
  766.                 if (NavigationMeshID meshID = gAIEnv.pNavigationSystem->GetEnclosingMeshID(agentTypeID, from))
  767.                 {
  768.                         const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
  769.                         const MNM::CNavMesh& navMesh = mesh.navMesh;
  770.  
  771.                         MNM::vector3_t startLoc = MNM::vector3_t(MNM::real_t(from.x), MNM::real_t(from.y), MNM::real_t(from.z));
  772.                         MNM::vector3_t endLoc = MNM::vector3_t(MNM::real_t(to.x), MNM::real_t(to.y), MNM::real_t(to.z));
  773.  
  774.                         const MNM::real_t horizontalRange(5.0f);
  775.                         const MNM::real_t verticalRange(1.0f);
  776.  
  777.                         MNM::TriangleID triStart = navMesh.GetTriangleAt(startLoc, verticalRange, verticalRange);
  778.  
  779.                         MNM::TriangleID triEnd = navMesh.GetTriangleAt(endLoc, verticalRange, verticalRange);
  780.                         if (!triEnd)
  781.                         {
  782.                                 MNM::vector3_t closestEndLocation;
  783.                                 triEnd = navMesh.GetClosestTriangle(endLoc, verticalRange, horizontalRange, nullptr, &closestEndLocation);
  784.                                 navMesh.PushPointInsideTriangle(triEnd, closestEndLocation, MNM::real_t(.05f));
  785.                                 endLoc = closestEndLocation;
  786.                         }
  787.  
  788.                         if (triStart && triEnd)
  789.                         {
  790.                                 MNM::CNavMesh::RayCastRequest<512> raycastRequest;
  791.                                 MNM::CNavMesh::ERayCastResult result = navMesh.RayCast(startLoc, triStart, endLoc, triEnd, raycastRequest);
  792.                                 if (result == MNM::CNavMesh::eRayCastResult_Hit)
  793.                                 {
  794.                                         const float velocityMagnitude = min(TimeStep, raycastRequest.hit.distance.as_float());
  795.                                         const Vec3 newEndLoc = agentPosition + ((endLoc.GetVec3() - agentPosition) * velocityMagnitude);
  796.                                         const Vec3 newVelocity = newEndLoc - agentPosition;
  797.                                         outputVelocity = Vec2(newVelocity.x, newVelocity.y) * invTimeHorizon;
  798.                                 }
  799.                                 else if (result == MNM::CNavMesh::eRayCastResult_NoHit)
  800.                                 {
  801.                                         const Vec3 newVelocity = endLoc.GetVec3() - agentPosition;
  802.                                         outputVelocity = Vec2(newVelocity.x, newVelocity.y);
  803.                                 }
  804.                                 else
  805.                                 {
  806.                                         assert(0);
  807.                                 }
  808.                         }
  809.                 }
  810.         }
  811.         return outputVelocity;
  812. }
  813.  
  814. void CollisionAvoidanceSystem::ComputeAgentConstraintLine(const Agent& agent, const Agent& obstacleAgent,
  815.                                                           bool reciprocal, float timeHorizonScale, ConstraintLine& line) const
  816. {
  817.         const Vec2 relativePosition = Vec2(obstacleAgent.currentLocation) - Vec2(agent.currentLocation);
  818.         const Vec2 relativeVelocity = agent.currentVelocity - obstacleAgent.currentVelocity;
  819.  
  820.         const float distanceSq = relativePosition.GetLength2();
  821.         const float radii = agent.radius + obstacleAgent.radius;
  822.         const float radiiSq = sqr(radii);
  823.  
  824.         const float TimeHorizon = timeHorizonScale *
  825.                                   (reciprocal ? gAIEnv.CVars.CollisionAvoidanceAgentTimeHorizon : gAIEnv.CVars.CollisionAvoidanceObstacleTimeHorizon);
  826.         const float TimeStep = gAIEnv.CVars.CollisionAvoidanceTimeStep;
  827.  
  828.         const float invTimeHorizon = 1.0f / TimeHorizon;
  829.         const float invTimeStep = 1.0f / TimeStep;
  830.  
  831.         Vec2 u;
  832.  
  833.         if (distanceSq > radiiSq)
  834.         {
  835.                 const Vec2 cutoffCenter = relativePosition * invTimeHorizon;
  836.  
  837.                 Vec2 w = relativeVelocity - cutoffCenter;
  838.                 const float wLenSq = w.GetLength2();
  839.  
  840.                 const float dot = w.Dot(relativePosition);
  841.  
  842.                 // compute closest point from relativeVelocity to the velocity object boundary
  843.                 if ((dot < 0.0f) && (sqr(dot) > radiiSq * wLenSq))
  844.                 {
  845.                         // w is pointing backwards from cone apex direction
  846.                         // closest point lies on the cutoff arc
  847.                         const float wLen = sqrt_tpl(wLenSq);
  848.                         w /= wLen;
  849.  
  850.                         line.direction = Vec2(w.y, -w.x);
  851.                         u = (radii * invTimeHorizon - wLen) * w;
  852.                 }
  853.                 else
  854.                 {
  855.                         // w is pointing into the cone
  856.                         // closest point is on an edge
  857.                         const float edge = sqrt_tpl(distanceSq - radiiSq);
  858.  
  859.                         if (LeftOf(relativePosition, w) > 0.0f)
  860.                         {
  861.                                 // left edge
  862.                                 line.direction = Vec2(relativePosition.x * edge - relativePosition.y * radii,
  863.                                                       relativePosition.x * radii + relativePosition.y * edge) / distanceSq;
  864.                         }
  865.                         else
  866.                         {
  867.                                 // right edge
  868.                                 line.direction = -Vec2(relativePosition.x * edge + relativePosition.y * radii,
  869.                                                        -relativePosition.x * radii + relativePosition.y * edge) / distanceSq;
  870.                         }
  871.  
  872.                         const float proj = relativeVelocity.Dot(line.direction);
  873.  
  874.                         u = proj * line.direction - relativeVelocity;
  875.                 }
  876.         }
  877.         else
  878.         {
  879.                 const float distance = sqrt_tpl(distanceSq);
  880.                 const Vec2 w = relativePosition / distance;
  881.  
  882.                 line.direction = Vec2(-w.y, w.x);
  883.  
  884.                 const Vec2 point = ((distance - radii) * invTimeStep) * w;
  885.                 const float dot = (relativeVelocity - point).Dot(line.direction);
  886.  
  887.                 u = point + dot * line.direction - relativeVelocity;
  888.         }
  889.  
  890.         const float effort = reciprocal ? 0.5f : 1.0f;
  891.         line.point = agent.currentVelocity + effort * u;
  892. }
  893.  
  894. bool CollisionAvoidanceSystem::FindLineCandidate(const ConstraintLine* lines, size_t lineCount, size_t lineNumber, float radius,
  895.                                                  const Vec2& velocity, Vec2& candidate) const
  896. {
  897.         const ConstraintLine& line = lines[lineNumber];
  898.  
  899.         const float discriminant = sqr(radius) - sqr(line.direction.Cross(line.point));
  900.  
  901.         if (discriminant < 0.0f)
  902.                 return false;
  903.  
  904.         const float discriminantSqrt = sqrt_tpl(discriminant);
  905.         const float dot = line.direction.Dot(line.point);
  906.  
  907.         float tLeft = -dot - discriminantSqrt;
  908.         float tRight = -dot + discriminantSqrt;
  909.  
  910.         for (size_t i = 0; i < lineNumber; ++i)
  911.         {
  912.                 const ConstraintLine& constraint = lines[i];
  913.  
  914.                 const float determinant = line.direction.Cross(constraint.direction);
  915.                 const float distanceSigned = constraint.direction.Cross(line.point - constraint.point);
  916.  
  917.                 // parallel constraints
  918.                 if (fabs_tpl(determinant) < 0.00001f)
  919.                 {
  920.                         if (distanceSigned < 0.0f)
  921.                                 return false;
  922.                         else
  923.                                 continue; // no constraint
  924.                 }
  925.  
  926.                 const float t = distanceSigned / determinant;
  927.  
  928.                 if (determinant > 0.0f)
  929.                         tRight = min(t, tRight);
  930.                 else
  931.                         tLeft = max(t, tLeft);
  932.  
  933.                 if (tLeft > tRight)
  934.                         return false;
  935.         }
  936.  
  937.         const float t = line.direction.Dot(velocity - line.point);
  938.  
  939.         if (t < tLeft)
  940.                 candidate = line.point + tLeft * line.direction;
  941.         else if (t > tRight)
  942.                 candidate = line.point + tRight * line.direction;
  943.         else
  944.                 candidate = line.point + t * line.direction;
  945.  
  946.         return true;
  947. }
  948.  
  949. bool CollisionAvoidanceSystem::FindCandidate(const ConstraintLine* lines, size_t lineCount, float radius, const Vec2& velocity,
  950.                                              Vec2& candidate) const
  951. {
  952.         if (velocity.GetLength2() > sqr(radius))
  953.                 candidate = velocity.GetNormalized() * radius;
  954.         else
  955.                 candidate = velocity;
  956.  
  957.         for (size_t i = 0; i < lineCount; ++i)
  958.         {
  959.                 const ConstraintLine& constraint = lines[i];
  960.  
  961.                 if (LeftOf(constraint.direction, candidate - constraint.point) < 0.0f)
  962.                 {
  963.                         if (!FindLineCandidate(lines, lineCount, i, radius, velocity, candidate))
  964.                                 return false;
  965.                 }
  966.         }
  967.  
  968.         return true;
  969. }
  970.  
  971. void CollisionAvoidanceSystem::DebugDrawConstraintLine(const Vec3& agentLocation, const ConstraintLine& line,
  972.                                                        const ColorB& color)
  973. {
  974.         CDebugDrawContext dc;
  975.  
  976.         dc->SetBackFaceCulling(false);
  977.         dc->SetDepthWrite(false);
  978.  
  979.         Vec3 v1 = agentLocation + line.point;
  980.         Vec3 v0 = v1 - line.direction * 10.0f;
  981.         Vec3 v2 = v1 + line.direction * 10.0f;
  982.  
  983.         dc->DrawLine(v0, color, v2, color, 5.0f);
  984.         dc->DrawArrow(v1, Vec2(-line.direction.y, line.direction.x) * 0.35f, 0.095f, color);
  985. }
  986.  
  987. void CollisionAvoidanceSystem::DebugDraw()
  988. {
  989.         CDebugDrawContext dc;
  990.  
  991.         dc->SetBackFaceCulling(false);
  992.         dc->SetDepthWrite(false);
  993.         dc->SetDepthTest(false);
  994.  
  995.         Agents::iterator it = m_agents.begin();
  996.         Agents::iterator end = m_agents.end();
  997.         uint32 index = 0;
  998.  
  999.         ColorB desiredColor = ColorB(Col_Black, 1.0f);
  1000.         ColorB newColor = ColorB(Col_DarkGreen, 0.5f);
  1001.  
  1002.         for (; it != end; ++it, ++index)
  1003.         {
  1004.                 Agent& agent = *it;
  1005.  
  1006.                 if (IAIObject* object = gAIEnv.pAIObjectManager->GetAIObject(m_agentObjectIDs[index]))
  1007.                 {
  1008.                         if (CAIActor* actor = object->CastToCAIActor())
  1009.                         {
  1010.                                 Vec3 agentLocation = actor->GetPhysicsPos();
  1011.                                 Vec2 agentAvoidanceVelocity = m_agentAvoidanceVelocities[index];
  1012.  
  1013.                                 //if ((agent.desiredVelocity - agentAvoidanceVelocity).GetLength2() > 0.000001f)
  1014.                                 dc->DrawArrow(agentLocation, agent.desiredVelocity, 0.135f, desiredColor);
  1015.  
  1016.                                 dc->DrawArrow(agentLocation, agentAvoidanceVelocity, 0.2f, newColor);
  1017.  
  1018.                                 dc->DrawRangeCircle(agentLocation + Vec3(0, 0, 0.3f), agent.radius, 0.1f, ColorF(0.196078f, 0.8f, 0.6f, 0.5f), ColorF(0.196078f, 0.196078f, 0.8f), true);
  1019.                         }
  1020.                 }
  1021.         }
  1022. }
  1023.  
downloadCollisionAvoidanceSystem.cpp Source code - Download CRYENGINE Source code
Related Source Codes/Software:
postal - 2017-06-11
reactide - Reactide is the first dedicated IDE for React web ... 2017-06-11
rkt - rkt is a pod-native container engine for Linux. It... 2017-06-11
uWebSockets - Tiny WebSockets https://for... 2017-06-11
realworld - TodoMVC for the RealWorld - Exemplary fullstack Me... 2017-06-11
CRYENGINE - CRYENGINE is a powerful real-time game development... 2017-06-11
goreplay - GoReplay is an open-source tool for capturing and ... 2017-06-10
pyenv - Simple Python version management 2017-06-10
redux-saga - An alternative side effect model for Redux apps ... 2017-06-10
angular-starter - 2017-06-10

 Back to top