コード例 #1
0
ファイル: ObstacleKDTree.cpp プロジェクト: argenos/Menge
		void ObstacleKDTree::queryTreeRecursive( ProximityQuery *filter, Vector2 pt, float& rangeSq, const ObstacleTreeNode* node) const {
			if (node == 0) {
				return;
			} else {
				const Obstacle* const obstacle1 = node->_obstacle;
				
				const Vector2 P0 = obstacle1->getP0();
				const Vector2 P1 = obstacle1->getP1();

				const float agentLeftOfLine = leftOf( P0, P1, pt);

				queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_left : node->_right));

				const float distSqLine = sqr(agentLeftOfLine) / absSq(P1 - P0);

				if (distSqLine < rangeSq) {
					if ( obstacle1->_doubleSided || agentLeftOfLine < 0.0f) {
						/*
						* Try obstacle at this node only if agent is on right side of
						* obstacle (and can see obstacle).
						*/
						float distSq = distSqPointLineSegment(node->_obstacle->getP0(), node->_obstacle->getP1(), pt);

						filter->filterObstacle(node->_obstacle, distSq);
						
						rangeSq = filter->getMaxObstacleRange();
					}

					/* Try other side of line. */
					queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_right : node->_left));
				}
			}
		}
コード例 #2
0
ファイル: Agent.cpp プロジェクト: whateverpal/lib
  void Agent::insertObstacleNeighbor(const Obstacle* obstacle, float rangeSq)
  {
    const Obstacle* const nextObstacle = obstacle->nextObstacle;

    const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);

    if (distSq < rangeSq) {
      obstacleNeighbors_.push_back(std::make_pair(distSq,obstacle));
      
      size_t i = obstacleNeighbors_.size() - 1;
      while (i != 0 && distSq < obstacleNeighbors_[i-1].first) {
        obstacleNeighbors_[i] = obstacleNeighbors_[i-1];
        --i;
      }
      obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
    }
  }