/*
 * 实现碰撞检测中一个经典的Sweep And Prune算法
 * 参考文章：D. Baraff (Ph. D thesis), p 52.
 * http://www.cs.cmu.edu/~baraff/papers/thesis-part1.ps.Z
 */

#include <iostream>
#include <vector>
#include <map>
#include <list>
#include <algorithm>

void sweepAndPrune(std::vector<std::tuple<bool, unsigned int, double>> values, std::map<unsigned int, std::list<unsigned int>> &collision)
{
	// 根据区间端点位置对各个区间端点进行排序
	std::sort(values.begin(), values.end(), [](std::tuple<bool, unsigned int, double> a, std::tuple<bool, unsigned int, double> b) { return std::get<2>(a) < std::get<2>(b); });
	// 输出排序之后的结果
	for (auto ite = values.begin(); ite != values.end(); ite++)
	{
		auto t = std::get<0>(*ite) ? "b" : "e";
		auto idx = std::get<1>(*ite);
		std::cout << idx << "," << t << ": " << std::get<2>(*ite) << std::endl;
	}
	std::list<unsigned int> activeList = {};
	// 遍历区间端点，生成每个区间对应的重叠区间列表。如果区间端点为起始点，那么就activeList中的区间索引就是该区间的重叠区间，并将该区间添加到activeList中
	// 否则就在activeList中删除该区间索引
	for (auto ite = values.begin(); ite != values.end(); ite++)
	{
		auto idx = std::get<1>(*ite);
		if (std::get<0>(*ite)) // b
		{
			collision[idx] = activeList;
			activeList.push_back(idx);
		}
		else // e
		{
			activeList.remove(idx);
		}
	}

}

int main()
{
	// 在一个坐标轴上的多个区间
	std::vector<std::pair<double, double>> interal = { {3.0,5.0},{7.0,18.0},{0.0,8.0},{10.0,16.0},{6.0,12.0},{1.0,4.0} };
	for (auto ite = interal.begin(); ite != interal.end(); ite++)
	{
		std::cout << "[" << ite->first << "," << ite->second << "]\n";
	}
	// 保存每个区间端点是否为起始点（b:true, e:false），索引值，位置
	std::vector<std::tuple<bool,unsigned int, double>> values;
	for (unsigned int i = 0; i < interal.size(); i++)
	{
		values.push_back({ true, i,interal[i].first });
		values.push_back({ false,i,interal[i].second });
	}
	std::map<unsigned int, std::list<unsigned int>> collision;
	sweepAndPrune(values, collision);
	// 输出具有重叠的线段
	for (auto ite = collision.begin(); ite != collision.end(); ite++)
	{
		std::cout << "the " << ite->first + 1<< "th segment is overlapping with segments: ";
		for (auto i : ite->second)
		{
			std::cout << i + 1 <<",";
		}
		std::cout << "."<<std::endl;
	}
	return 0;
}