Reciprocal Velocity Obstacle(RVO) : RVO2

앞선 개념

Velocity Obstacle(VO)란?

새로운 개념인 Reciprocal Velocity Obstacle(이하 RVO)를 제안함
각 에이전트가 다른 에이전트와 명시적으로 통신하지 않고 독립적으로 움직이는 경우를 고려함

다른 객체들이 유사한 충돌 회피 추론을 암묵적으로 가정함으로써 다른 객체의 반응적 행동을 고려함
정적 장애물과 이동 장애물을 모두 포함하는 인구 밀도가 높은 환경에서 수백 개의 에이전트를 탐색하는 것에 개념을 적용하고, 도전적인 시나리오에서 실시간 및 확장 가능한 성능을 달성한다는 것을 보여줌
사용하는 ORCA 개념의 경우 로보틱스에서 로봇간의 충돌을 막기 위해서도 쓰임

 

수정하고자 한 것

Velocity Obstacle(VO)에서 발생하는 진동현상을 수정하고자 제작함

 

핵심 내용

움직이는 장애물들 사이에서의 자연스러운 움직임을 위해 도입된 VO(Velocity Obstacle) 개념의 확장

ORCA(Optimal Reciprocal Collision Avoidance)라는 개념을 도입하여 각 객체간의 속도를 계산한 후 그 책임을 각각 반틈씩 가지게 만들어서 군중의 뭉치는 현상을 줄어들도록 만드는 것

결국 병목 현상(Bottleneck)을 개선하기 위해서 제작

 

- 민코우스키 합 (민코프스키 덧셈)

기하학에서, 유클리드 공간의 위치벡터 A와 B의 두 집합의 민코스키 합(팽창)은 A에 있는 모든 벡터를 B에 있는 각각의 벡터에 더해서 만들어진다.
 

 
 
민코프스키 덧셈은 장애물 사이로 지나는 모션계획에 사용된다.

 

추가 참고 자료

Minkowski sum of convex polygons - Algorithms for Competitive Programming

민코프스키 덧셈 - 위키백과, 우리 모두의 백과사전

 

 

실제 작동 RVO2 Library

💡RVOSimulator-> doStep()

 

doStep을 실행하면 한프레임이 진행된다.

void RVOSimulator::doStep(float powfScalar)
{
    kdTree_->buildAgentTree();

    this->powfScalar = powfScalar;

#ifdef _OPENMP
#pragma omp parallel for
#endif
    for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
        agents_[i]->computeNeighbors();
        agents_[i]->computeNewVelocity();
        //agents_[i]->computeNewVelocityE();
    }

#ifdef _OPENMP
#pragma omp parallel for
#endif
    for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
        agents_[i]->update();
    }

    globalTime_ += timeStep_;
}

 

먼저 kdTree에 대한 정보들을 다시 생성한다.

void KdTree::buildAgentTree()
{
	if (agents_.size() < sim_->agents_.size()) {
		for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) {
			agents_.push_back(sim_->agents_[i]);
		}

		agentTree_.resize(2 * agents_.size() - 1);
	}

	if (!agents_.empty()) {
		buildAgentTreeRecursive(0, agents_.size(), 0);
	}
}

 

현재 agent의 수와 시뮬레이션에서 가지고 있는 agent의 수가 변경될 때 (즉, 추가될 때) 새로운 agent를 vector에 추가한다.

그리고 Agent kd-tree를 생성한다.

 

void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
{
	agentTree_[node].begin = begin;
	agentTree_[node].end = end;
	agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
	agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();

	for (size_t i = begin + 1; i < end; ++i) {
		agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x());
		agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x());
		agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y());
		agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y());
	}

	if (end - begin > MAX_LEAF_SIZE) {
		/* No leaf node. (leaf node : 부모가 없는 최상위 노드) */
		const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY);
		const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY));

		size_t left = begin;
		size_t right = end;

		while (left < right) {
			while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) {
				++left;
			}

			while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) {
				--right;
			}

			if (left < right) {
				std::swap(agents_[left], agents_[right - 1]);
				++left;
				--right;
			}
		}

		if (left == begin) {
			++left;
			++right;
		}

		agentTree_[node].left = node + 1;
		agentTree_[node].right = node + 2 * (left - begin);

		buildAgentTreeRecursive(begin, left, agentTree_[node].left);
		buildAgentTreeRecursive(left, end, agentTree_[node].right);
	}
}

 

 

💡Agent -> computeNeighbors()

void Agent::computeNeighbors()
{
	obstacleNeighbors_.clear();
	float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
	sim_->kdTree_->computeObstacleNeighbors(this, rangeSq);

	agentNeighbors_.clear();

	if (maxNeighbors_ > 0) {
		rangeSq = sqr(neighborDist_);
		sim_->kdTree_->computeAgentNeighbors(this, rangeSq);
	}
}

 

먼저 Agent의 이웃들(나랑 같이 움직이고 있다고 가정하는 Agent들에 대해 계산을 한다.

rangeSq는 속도와 timeHorizonObst(옵션)에 따라 달라지도록 만든 sqr(루트)를 적용한 반지름이다.

해당 변수를 kdTree->computeAgentNeighbors() 함수로 넘겨준다.

이중에 RVO는 Nearest Neighbor 방법을 사용하는듯하다.

computeObstacleNeighbors는 움직이지 않는 방해물을 계산할 때 사용하려고 계산한다.

💡k-d Tree
k차원 공간의 점들을 구조화하는 공간 분할 자료 구조
보통 다차원 공간에서 탐색할 때 사용되는 기법
Range Search, Nearest neighbor 등의 문제에 적용

💡kNN
주어진 Point와 가장 가까운(주어진 set안의) 점을 찾는 것
1. k 이웃의 기준을 정의
2. test 셋의 개별 포인트에 대해서 k 개의 이웃을 찾음
3. 이웃의 output을 가지고 예측값을 생성

 

void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const
{
    queryAgentTreeRecursive(agent, rangeSq, 0);
}

 

kd-tree 내에서 위치를 찾아서 insert해준다.

찾은 agentTree가 10개 이하면 그대로 삽입하고, 그렇지 않으면 kd-tree 내에서 위치를 다시 정확하게 탐색한다.

kNN에서 주어진 set이 10개라고 생각하면 된다.

void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const
{
	//agentTree의 크기가 10보다 작다면 agent의 이웃에 해당 agent를 insert
	if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) {
		for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
			agent->insertAgentNeighbor(agents_[i], rangeSq);
		}
	}
	else {
		const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY));

		const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY));

		if (distSqLeft < distSqRight) {
			if (distSqLeft < rangeSq) {
				queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);

				if (distSqRight < rangeSq) {
					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
				}
			}
		}
		else {
			if (distSqRight < rangeSq) {
				queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);

				if (distSqLeft < rangeSq) {
					queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
				}
			}
		}

	}
}

 

💡Agent -> computeNewVelocity()

 

주변 이웃을 결정하였으면 새로운 속도를 탐색한다.

새로운 속도를 위해 ORCA 라인을 새롭게 만든다.

행렬식의 절대값은 도형의 면적을 의미한다.

 

		const Obstacle *obstacle1 = obstacleNeighbors_[i].second;
		const Obstacle *obstacle2 = obstacle1->nextObstacle_;

		const Vector2 relativePosition1 = obstacle1->point_ - position_;
		const Vector2 relativePosition2 = obstacle2->point_ - position_;

		/*
		 * Check if velocity obstacle of obstacle is already taken care of by
		 * previously constructed obstacle ORCA lines.
		 * 장애물의 속도 장애물이 이전에 구축된 장애물 ORCA 라인에 의해 
		 * 이미 처리되었는지 확인합니다.
		 */
		bool alreadyCovered = false;

		for (size_t j = 0; j < orcaLines_.size(); ++j) {
			//det : 행렬식, radius_ : agent의 반지름
			if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON 
			&& det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >=  -RVO_EPSILON) {
				alreadyCovered = true;
				break;
			}
		}

		if (alreadyCovered) {
			continue;
		}

 

먼저 구축된 장애물 ORCA 라인에 의해 이미 처리되었는지 확인하고, 처리 되었다면 생략한다.

그렇지 않다면 장애물 ORCA 라인을 만든다.

	/* Create obstacle ORCA lines. */
	for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {
	
		/* Not yet covered. Check for collisions. */

		const float distSq1 = absSq(relativePosition1);
		const float distSq2 = absSq(relativePosition2);

		const float radiusSq = sqr(radius_);

		const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_;
		const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector);
		const float distSqLine = absSq(-relativePosition1 - s * obstacleVector);

		Line line;

		if (s < 0.0f && distSq1 <= radiusSq) {
			/* Collision with left vertex. Ignore if non-convex. */
			if (obstacle1->isConvex_) {
				line.point = Vector2(0.0f, 0.0f);
				line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
				orcaLines_.push_back(line);
			}

			continue;
		}
		else if (s > 1.0f && distSq2 <= radiusSq) {
			/* Collision with right vertex. Ignore if non-convex
			 * or if it will be taken care of by neighoring obstace */
			if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) {
				line.point = Vector2(0.0f, 0.0f);
				line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
				orcaLines_.push_back(line);
			}

			continue;
		}
		else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) {
			/* Collision with obstacle segment. */
			line.point = Vector2(0.0f, 0.0f);
			line.direction = -obstacle1->unitDir_;
			orcaLines_.push_back(line);
			continue;
		}

		//아래 코드들 실행
        //...

 

		/*
		 * No collision.
		 * Compute legs. When obliquely viewed, both legs can come from a single
		 * vertex. Legs extend cut-off line when nonconvex vertex.
		 * 충돌은 없습니다.
		 * leg를 계산하세요. 비스듬히 볼 때, 두 leg는 하나의 꼭짓점에서 나올 수 있습니다. 
		 * leg는 볼록하지 않은 꼭짓점에서 절단선을 연장합니다.
		 */

		Vector2 leftLegDirection, rightLegDirection;

		if (s < 0.0f && distSqLine <= radiusSq) {
			/*
			 * Obstacle viewed obliquely so that left vertex 
			 * defines velocity obstacle.
			 * 왼쪽 꼭짓점이 속도 장애물을 정의하도록 비스듬히 보이는 장애물.
			 */
			if (!obstacle1->isConvex_) {
				/* Ignore obstacle. */
				continue;
			}

			obstacle2 = obstacle1;

			const float leg1 = std::sqrt(distSq1 - radiusSq);
			leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
			rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
		}
		else if (s > 1.0f && distSqLine <= radiusSq) {
			/*
			 * Obstacle viewed obliquely so that
			 * right vertex defines velocity obstacle.
			 * 오른쪽 꼭짓점이 속도 장애물을 정의하도록 비스듬히 보이는 장애물.
			 */
			if (!obstacle2->isConvex_) {
				/* Ignore obstacle. */
				continue;
			}

			obstacle1 = obstacle2;

			const float leg2 = std::sqrt(distSq2 - radiusSq);
			leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
			rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
		}
		else {
			/* Usual situation. */
			if (obstacle1->isConvex_) {
				const float leg1 = std::sqrt(distSq1 - radiusSq);
				leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
			}
			else {
				/* Left vertex non-convex; left leg extends cut-off line. */
				leftLegDirection = -obstacle1->unitDir_;
			}

			if (obstacle2->isConvex_) {
				const float leg2 = std::sqrt(distSq2 - radiusSq);
				rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
			}
			else {
				/* Right vertex non-convex; right leg extends cut-off line. */
				rightLegDirection = obstacle1->unitDir_;
			}
		}

/*
		 * Legs can never point into neighboring edge when convex vertex,
		 * take cutoff-line of neighboring edge instead. If velocity projected on
		 * "foreign" leg, no constraint is added.
		 */

		const Obstacle *const leftNeighbor = obstacle1->prevObstacle_;

		bool isLeftLegForeign = false;
		bool isRightLegForeign = false;

		if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) {
			/* Left leg points into obstacle. */
			leftLegDirection = -leftNeighbor->unitDir_;
			isLeftLegForeign = true;
		}

		if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) {
			/* Right leg points into obstacle. */
			rightLegDirection = obstacle2->unitDir_;
			isRightLegForeign = true;
		}

		/* Compute cut-off centers. */
		const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_);
		const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_);
		const Vector2 cutoffVec = rightCutoff - leftCutoff;

		/* Project current velocity on velocity obstacle. */

		/* Check if current velocity is projected on cutoff circles. */
		const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec));
		const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection);
		const float tRight = ((velocity_ - rightCutoff) * rightLegDirection);

		if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) {
			/* Project on left cut-off circle. */
			const Vector2 unitW = normalize(velocity_ - leftCutoff);

			line.direction = Vector2(unitW.y(), -unitW.x());
			line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW;
			orcaLines_.push_back(line);
			continue;
		}
		else if (t > 1.0f && tRight < 0.0f) {
			/* Project on right cut-off circle. */
			const Vector2 unitW = normalize(velocity_ - rightCutoff);

			line.direction = Vector2(unitW.y(), -unitW.x());
			line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW;
			orcaLines_.push_back(line);
			continue;
		}

/*
		 * Project on left leg, right leg, or cut-off line, whichever is closest
		 * to velocity.
		 */
		const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
		const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
		const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));

		if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
			/* Project on cut-off line. */
			line.direction = -obstacle1->unitDir_;
			line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
			orcaLines_.push_back(line);
			continue;
		}
		else if (distSqLeft <= distSqRight) {
			/* Project on left leg. */
			if (isLeftLegForeign) {
				continue;
			}

			line.direction = leftLegDirection;
			line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
			orcaLines_.push_back(line);
			continue;
		}
		else {
			/* Project on right leg. */
			if (isRightLegForeign) {
				continue;
			}

			line.direction = -rightLegDirection;
			line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
			orcaLines_.push_back(line);
			continue;
		}

 

	const size_t numObstLines = orcaLines_.size();

	const float invTimeHorizon = 1.0f / timeHorizon_;

	/* Create agent ORCA lines. */
	for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
		const Agent *const other = agentNeighbors_[i].second;

		const Vector2 relativePosition = other->position_ - position_;
		const Vector2 relativeVelocity = velocity_ - other->velocity_;
		const float distSq = absSq(relativePosition);
		const float combinedRadius = radius_ + other->radius_;
		const float combinedRadiusSq = sqr(combinedRadius);

		Line line;
		Vector2 u;

		if (distSq > combinedRadiusSq) {
			/* No collision. */
			const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
			/* Vector from cutoff center to relative velocity. */
			const float wLengthSq = absSq(w);

			const float dotProduct1 = w * relativePosition;

			if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
				/* Project on cut-off circle. */
				const float wLength = std::sqrt(wLengthSq);
				const Vector2 unitW = w / wLength;

				line.direction = Vector2(unitW.y(), -unitW.x());
				u = (combinedRadius * invTimeHorizon - wLength) * unitW;
			}
			else {
				/* Project on legs. */
				const float leg = std::sqrt(distSq - combinedRadiusSq);

				if (det(relativePosition, w) > 0.0f) {
					/* Project on left leg. */
					line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
				}
				else {
					/* Project on right leg. */
					line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
				}

				const float dotProduct2 = relativeVelocity * line.direction;

				u = dotProduct2 * line.direction - relativeVelocity;
			}
		}
		else {
			/* Collision. Project on cut-off circle of time timeStep. */
			const float invTimeStep = 1.0f / sim_->timeStep_;

			/* Vector from cutoff center to relative velocity. */
			const Vector2 w = relativeVelocity - invTimeStep * relativePosition;

			const float wLength = abs(w);
			const Vector2 unitW = w / wLength;

			line.direction = Vector2(unitW.y(), -unitW.x());
			u = (combinedRadius * invTimeStep - wLength) * unitW;
		}

		line.point = velocity_ + 0.5f * u;
		orcaLines_.push_back(line);
	}

	size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);

	if (lineFail < orcaLines_.size()) {
		linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
	}

 

계산된 ORCA 라인들과 Linear Programming을 이용해서 새로운 속도를 얻는다.

💡Linear Programming
다양한 제약 조건을 고려하면서 선형 함수를 최대화하거나 최소화하는 수학적 모델링 기술

size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);

if (lineFail < orcaLines_.size()) {
	linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
}

 

result == newVelocity_랑 같다.

size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
	if (directionOpt) {
		//최적화되어 있는 방향. 이 경우 최적화 속도는 단위 길이입니다.
		result = optVelocity * radius;
	}
	else if (absSq(optVelocity) > sqr(radius)) {
		/* 가장 가까운 점과 바깥쪽 원을 최적화합니다. */
		result = normalize(optVelocity) * radius;
	}
	else {
		/* 가장 가까운 점과 안쪽 원을 최적화합니다. */
		result = optVelocity;
	}

	for (size_t i = 0; i < lines.size(); ++i) {
		//두 선이 같으면 0을 출력
		if (det(lines[i].direction, lines[i].point - result) > 0.0f) { // 평행사변형의 면적
			/* 결과가 제약조건 i를 만족하지 않습니다. 새로운 최적의 결과를 계산합니다. */
			const Vector2 tempResult = result;

			if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
				result = tempResult;
				return i;
			}
		}
	}

	return lines.size();
}

 

 

bool linearProgram1(const std::vector<Line> &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
	//스칼라 곱, 벡터의 내적
	const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
	const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);

	if (discriminant < 0.0f) {
		/* 최대 속도 원은 line lineNo를 완전히 무효화합니다. */
		return false;
	}

	const float sqrtDiscriminant = std::sqrt(discriminant);
	float tLeft = -dotProduct - sqrtDiscriminant;
	float tRight = -dotProduct + sqrtDiscriminant;

	for (size_t i = 0; i < lineNo; ++i) {
		//분모
		const float denominator = det(lines[lineNo].direction, lines[i].direction);
		//분자
		const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);
		//fabs() : 절대값 연산
		if (std::fabs(denominator) <= RVO_EPSILON) {
			/* Lines lineNo과 i는 (거의) 나란하다. */
			if (numerator < 0.0f) {
				return false;
			}
			else {
				continue;
			}
		}

		const float t = numerator / denominator;

		if (denominator >= 0.0f) {
			/* Line i bounds line lineNo on the right. */
			tRight = std::min(tRight, t);
		}
		else {
			/* Line i bounds line lineNo on the left. */
			tLeft = std::max(tLeft, t);
		}

		if (tLeft > tRight) {
			return false;
		}
	}

	if (directionOpt) {
		/* 최적화 방향 */
		if (optVelocity * lines[lineNo].direction > 0.0f) {
			/* 오른쪽으로 극단적으로. */
			result = lines[lineNo].point + tRight * lines[lineNo].direction;
		}
		else {
			/* 왼쪽으로 극단적으로. */
			result = lines[lineNo].point + tLeft * lines[lineNo].direction;
		}
	}
	else {
		/* 가장 가까운 지점을 최적화합니다. */
		const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);

		if (t < tLeft) {
			result = lines[lineNo].point + tLeft * lines[lineNo].direction;
		}
		else if (t > tRight) {
			result = lines[lineNo].point + tRight * lines[lineNo].direction;
		}
		else {
			result = lines[lineNo].point + t * lines[lineNo].direction;
		}
	}

	return true;
}

 

void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result)
{
	float distance = 0.0f;

	for (size_t i = beginLine; i < lines.size(); ++i) {
		if (det(lines[i].direction, lines[i].point - result) > distance) {
			/* 결과가 선 i의 제약 조건을 만족하지 않습니다. */
			std::vector<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(numObstLines));

			for (size_t j = numObstLines; j < i; ++j) {
				Line line;

				float determinant = det(lines[i].direction, lines[j].direction);

				if (std::fabs(determinant) <= RVO_EPSILON) {
					/* Line i 과 line j 는 나란하다 */
					if (lines[i].direction * lines[j].direction > 0.0f) {
						/* Line i 과 line j 는 같은 방향을 가리킨다 */
						continue;
					}
					else {
						/* Line i 과 line j 는 반대 방향을 가리킨다 */
						line.point = 0.5f * (lines[i].point + lines[j].point);
					}
				}
				else {
					line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
				}

				line.direction = normalize(lines[j].direction - lines[i].direction);
				projLines.push_back(line);
			}

			const Vector2 tempResult = result;

			if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) {
				/* 원칙적으로 이런 일은 없어야 합니다. 
				 * 그 결과는 정의상 이 선형 프로그램의 실행 가능 영역에 이미 존재합니다.
				 * 실패할 경우 작은 부동 소수점 오차로 인해 발생하며 현재 결과는 그대로 
				 * 유지됩니다.
				 */
				result = tempResult;
			}

			distance = det(lines[i].direction, lines[i].point - result);
		}
	}
}

 

연관 키워드

half plane, ORCA, Linear Programming, Velocity Obstacle, 민코우스키 합

 

RVO 라이브러리를 이용한 군중 시뮬레이션 예제 모습

 

* 장애물에서 걸리는 현상이 많이 나오는 이유는 RVO 자체가 움직이는 Obstacle들이 서로 책임을 반틈씩 나누어 가지기 때문에 고정 장애물에서는 효과가 덜 나는 것으로 추측*

 

 

참고 사이트

[Algorithm] Linear Programming (1)

Optimal Reciprocal Collision Avoidance (ORCA)

Collision Avoidance

Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation