module gedo.math.collision;



public import gedo.math.vector;
private import std.math;



public void antiPassing(Vector pc0, float r0, Vector pp0, Vector pc1, float r1, Vector pp1)
{
	Vector d0;
	.gedo.math.vector.subtract(d0, pc0, pp0);

	Vector d1;
	.gedo.math.vector.subtract(d1, pc1, pp1);

	Vector vd;
	.gedo.math.vector.subtract(vd, d1, d0);

	auto a = .gedo.math.vector.square(vd);
	if(a != 0.0)
	{
		Vector ppd;

		.gedo.math.vector.subtract(ppd, pp1, pp0);

		auto b = .gedo.math.vector.dot(ppd, vd);
		if(b <= -0.5)
		{
			auto r = r0 + r1;

			auto c = .gedo.math.vector.square(ppd) - r*r;

			auto d = b*b - a*c;

			if(d >= 0.0)
			{
				d = sqrt(d);
				a = 1.0 / a;

				auto t0 = -(b + d) * a;
				auto t1 = -(b - d) * a;
				auto tc = (t0 + t1) * 0.5;

				if(abs(tc - 0.5) < (t1 - t0 + 1)*0.5)
				{
					/*
					if(t0 < 0.0)
					{
						pc0[] = pp0[];
						pc1[] = pp1[];
					}
					else
					*/
					{
						t0 -= 1.0/128.0;

						.gedo.math.vector.multiply(pc0, d0, t0);
						.gedo.math.vector.add(pc0, pp0);

						.gedo.math.vector.multiply(pc1, d1, t0);
						.gedo.math.vector.add(pc1, pp1);

						t0 = (1.0 - t0)*0.5;
						.gedo.math.vector.add(vd, d1, d0);
						.gedo.math.vector.multiply(vd, t0);
						.gedo.math.vector.add(pc0, vd);
						.gedo.math.vector.add(pc1, vd);
					}
				}
			}
		}
	}
}



/*
public void antiPassing(Vector p0, float r0, Vector d0, Vector p1, float r1, Vector d1)
{
	Vector[2] p;

	.gedo.math.vector.subtract(p[0], p0, d0);
	.gedo.math.vector.subtract(p[1], p1, d1);

	Vector pd;
	.gedo.math.vector.subtract(pd, p[1], p[0]);

	Vector dd;
	.gedo.math.vector.subtract(dd, d0, d1);

	if(dot(pd, dd) > 0.75)
	{
		auto pdl = magnitude(pd);

		auto rsum = r0 + r1 + magnitude(dd);

		if(pdl < rsum)
		{
			auto t = r1 / (r0 + r1);

			Vector pc;
			.gedo.math.vector.blend(pc, t, p0, 1.0-t, p1);

			if(pdl != 0.0)
			{
				.gedo.math.vector.multiply(pd, 1.0 / pdl);
			}
			else
			{
				pd[0] = 1.0;
			}

			//p0[] = pc[] - (pd[] * r0);
			.gedo.math.vector.multiply(p0, pd, -r0);
			.gedo.math.vector.add(p0, pc);

			//p1[] = pc[] + (pd[] * r1);
			.gedo.math.vector.multiply(p1, pd, r1);
			.gedo.math.vector.add(p1, pc);
		}
	}
}
*/



public void antiPassing(Vector p0, Vector s0, Vector d0, Vector p1, Vector s1, Vector d1)
{
	if(abs(p1[2] - p0[2]) < s0[2] + s1[2])
	{
		Vector2D[2] p;

		p[0][0] = p0[0] - d0[0];
		p[0][1] = p0[1] - d0[1];

		p[1][0] = p1[0] - d1[0];
		p[1][1] = p1[1] - d1[1];

		Vector2D pd;
		.gedo.math.vector.subtract(pd, p[1], p[0]);

		Vector2D dd;
		dd[0] = d0[0] - d1[0];
		dd[1] = d0[1] - d1[1];

		if(dot(pd, dd) > 0.75)
		{
			auto pdl = magnitude(pd);

			auto rsum = s0[0] + s1[0] + magnitude(dd);

			if(pdl < rsum)
			{
				float t = s1[0] / (s0[0] + s1[0]);

				//Vector2D pc = (p[0][] * s1[0]) + (p[1][] * s0[0]);
				Vector2D pc;
				.gedo.math.vector.blend(pc, t, p[0], 1.0-t, p[1]);

				if(pdl != 0.0)
				{
					.gedo.math.vector.multiply(pd, 1.0 / pdl);
				}
				else
				{
					pd[0] = 1.0;
				}

				p0[0] = pc[0] - (pd[0] * s0[0]);
				p0[1] = pc[1] - (pd[1] * s0[0]);

				p1[0] = pc[0] + (pd[0] * s1[0]);
				p1[1] = pc[1] + (pd[1] * s1[0]);
			}
		}
	}
}



public bool isColliding(Vector p0, float r0, Vector p1, float r1, Vector c)
{
	Vector pd;
	.gedo.math.vector.subtract(pd, p1, p0);

	auto pdl = square(pd);
	auto sum = r0 + r1;

	if(pdl < sum*sum)
	{
		auto t = r1 / sum;

		.gedo.math.vector.blend(c, t, p0, 1.0-t, p1);

		return true;
	}

	return false;
}



public bool isColliding(Vector p0, float r0, Vector[] p1, float r1, Vector c)
{
	assert(p1.length != 0);

	if(p1.length == 1)
	{
		return isColliding(p0, r0, p1[0], r1, c);
	}

	foreach(i, a; p1[0 .. length-1])
	{
		Vector b;
		.gedo.math.vector.subtract(b, p1[i+1], a);

		auto lb = magnitude(b);

		if(lb != 0.0)
		{
			.gedo.math.vector.multiply(b, 1.0/lb);

			Vector d;
			.gedo.math.vector.subtract(d, p0, a);

			auto u = dot(b, d);
			if(u <= 0.0)
			{
				b[] = a[];
			}
			else if(u >= lb)
			{
				b[] = p1[i+1][];
			}
			else
			{
				//b[] = a[] + (b[] * u);
				.gedo.math.vector.multiply(b, u);
				.gedo.math.vector.add(b, a);
			}
		}
		else
		{
			b[] = a[];
		}

		if(isColliding(p0, r0, b, r1, c))
		{
			return true;
		}
	}

	return false;
}

