Example #1
0
vec Polygon::PointOnEdge(float normalizedDistance) const
{
	if (p.empty())
		return vec::nan;
	if (p.size() < 2)
		return p[0];
	normalizedDistance = Frac(normalizedDistance); // Take modulo 1 so we have the range [0,1[.
	float perimeter = Perimeter();
	float d = normalizedDistance * perimeter;
	for(int i = 0; i < NumVertices(); ++i)
	{
		LineSegment edge = Edge(i);
		float len = edge.Length();
		assume(len != 0.f && "Degenerate Polygon detected!");
		if (d <= len)
			return edge.GetPoint(d / len);
		d -= len;
	}
	mathassert(false && "Polygon::PointOnEdge reached end of loop which shouldn't!");
	return p[0];
}
Example #2
0
int Sphere::Intersects(const LineSegment &l, vec *intersectionPoint, vec *intersectionNormal, float *d, float *d2) const
{
	float t1, t2;
	int numIntersections = IntersectLine(l.a, l.Dir(), pos, r, t1, t2);

	if (numIntersections == 0)
		return 0;

	float lineLength = l.Length();
	if (t2 < 0.f || t1 > lineLength)
		return 0;
	vec hitPoint = l.GetPoint(t1 / lineLength);
	if (intersectionPoint)
		*intersectionPoint = hitPoint;
	if (intersectionNormal)
		*intersectionNormal = (hitPoint - pos).Normalized();
	if (d)
		*d = t1 / lineLength;
	if (d2)
		*d2 = t2 / lineLength;

	return true;
}
Example #3
0
float Ray::Distance(const LineSegment &other, float &d, float &d2) const
{
	vec c = ClosestPoint(other, d, d2);
	return c.Distance(other.GetPoint(d2));
}
Example #4
0
bool Polygon_Intersects_Polygon(const Polygon &poly, const T &other, float polygonThickness)
{
	Plane plane = poly.PlaneCCW();
	Plane plane2 = other.PlaneCCW();

	if (!plane.normal.Cross(plane2.normal).IsZero())
	{
		// General strategy: If two polygon/triangle objects intersect, one 
		// of them must have an edge that passes through the interior of the other object.
		// Test each edge of the this object against intersection of the interior of the other polygon,
		// and vice versa.
		for(int i = 0; i < other.NumEdges(); ++i)
		{
			LineSegment lineSegment = other.Edge(i);
			float t;
			bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
			if (!intersects || t < 0.f || t > 1.f)
				continue;

			if (poly.Contains(lineSegment.GetPoint(t)))
				return true;
		}
	
		for(int i = 0; i < poly.NumEdges(); ++i)
		{
			LineSegment lineSegment = poly.Edge(i);
			float t;
			bool intersects = Plane::IntersectLinePlane(plane2.normal, plane2.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
			if (!intersects || t < 0.f || t > 1.f)
				continue;

			if (other.Contains(lineSegment.GetPoint(t)))
				return true;
		}
		return false;
	}
	else // The two polygons are coplanar. Perform the intersection test in 2D.
	{
		float poly0Pos = plane.normal.Dot(poly.Vertex(0));
		float poly1Pos = plane.normal.Dot(other.Vertex(0));
		if (Abs(poly0Pos-poly1Pos) > polygonThickness)
			return false;

		if (other.Contains(poly.Vertex(0), FLOAT_INF) || poly.Contains(other.Vertex(0), FLOAT_INF))
			return true;

		vec basisU = plane.normal.Perpendicular();
		vec basisV = plane.normal.AnotherPerpendicular();

		vec pivot = poly.Vertex(0);
		vec pt = poly.Vertex(poly.NumVertices()-1)-pivot;
		float2 a1 = float2(basisU.Dot(pt), basisV.Dot(pt));
		for(int i = 0; i < poly.NumVertices(); ++i)
		{
			pt = poly.Vertex(i)-pivot;
			float2 a2 = float2(basisU.Dot(pt), basisV.Dot(pt));

			pt = other.Vertex(other.NumVertices()-1)-pivot;
			float2 b1 = float2(basisU.Dot(pt), basisV.Dot(pt));
			for(int j = 0; j < other.NumVertices(); ++j)
			{
				pt = other.Vertex(j)-pivot;
				float2 b2 = float2(basisU.Dot(pt), basisV.Dot(pt));

				float s, t;
				if (LineSegment2DLineSegment2DIntersect(a1, a2-a1, b1, b2-b1, s, t))
					return true;

				b1 = b2;
			}
			a1 = a2;
		}

		return false;
	}
}
Example #5
0
///\todo Enable this codepath. This if rom Geometric Tools for Computer Graphics,
/// but the algorithm in the book is broken and does not take into account the
/// direction of the gradient to determine the proper region of intersection.
/// Instead using a slower code path above.
/// [groupSyntax]
float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const
{
	float3 e0 = b - a;
	float3 e1 = c - a;
	float3 v_p = a - lineSegment.a;
	float3 d = lineSegment.b - lineSegment.a;

	// Q(u,v) = a + u*e0 + v*e1
	// L(t)   = ls.a + t*d
	// Minimize the distance |Q(u,v) - L(t)|^2 under u >= 0, v >= 0, u+v <= 1, t >= 0, t <= 1.

	float v_p_dot_e0 = Dot(v_p, e0);
	float v_p_dot_e1 = Dot(v_p, e1);
	float v_p_dot_d = Dot(v_p, d);

	float3x3 m;
	m[0][0] = Dot(e0, e0); m[0][1] = Dot(e0, e1); m[0][2] = -Dot(e0, d);
	m[1][0] =     m[0][1]; m[1][1] = Dot(e1, e1); m[1][2] = -Dot(e1, d);
	m[2][0] =     m[0][2]; m[2][1] =     m[1][2]; m[2][2] =  Dot(d, d);

	float3 B(-v_p_dot_e0, -v_p_dot_e1, v_p_dot_d);

	float3 uvt;
	bool success = m.SolveAxb(B, uvt);
	if (!success)
	{
		float t1, t2, t3;
		float s1, s2, s3;
		LineSegment e1 = Edge(0);
		LineSegment e2 = Edge(1);
		LineSegment e3 = Edge(2);
		float d1 = e1.Distance(lineSegment, &t1, &s1);
		float d2 = e2.Distance(lineSegment, &t2, &s2);
		float d3 = e3.Distance(lineSegment, &t3, &s3);
		if (d1 < d2 && d1 < d3)
		{
			if (otherPt)
				*otherPt = lineSegment.GetPoint(s1);
			return e1.GetPoint(t1);
		}
		else if (d2 < d3)
		{
			if (otherPt)
				*otherPt = lineSegment.GetPoint(s2);
			return e2.GetPoint(t2);
		}
		else
		{
			if (otherPt)
				*otherPt = lineSegment.GetPoint(s3);
			return e3.GetPoint(t3);
		}
	}

	if (uvt.x < 0.f)
	{
		// Clamp to u == 0 and solve again.
		float m_00 = m[2][2];
		float m_01 = -m[1][2];
		float m_10 = -m[2][1];
		float m_11 = m[1][1];
		float det = m_00 * m_11 - m_01 * m_10;
		float v = m_00 * B[1] + m_01 * B[2];
		float t = m_10 * B[1] + m_11 * B[2];
		v /= det;
		t /= det;
		if (v < 0.f)
		{
			// Clamp to v == 0 and solve for t.
			t = B[2] / m[2][2];
			t = Clamp01(t); // The solution for t must also be in the range [0,1].
			// The solution is (u,v,t)=(0,0,t).
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return a;
		}
		else if (v > 1.f)
		{
			// Clamp to v == 1 and solve for t.
			t = (B[2] - m[2][1]) / m[2][2];
			t = Clamp01(t);
			// The solution is (u,v,t)=(0,1,t).
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return c; // == a + v*e1
		}
		else if (t < 0.f)
		{
			// Clamp to t == 0 and solve for v.
			v = B[1] / m[1][1];
//			mathassert(EqualAbs(v, Clamp01(v)));
			v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
			// The solution is (u,v,t)=(0,v,0).
			if (otherPt)
				*otherPt = lineSegment.a;
			return a + v * e1;
		}
		else if (t > 1.f)
		{
			// Clamp to t == 1 and solve for v.
			v = (B[1] - m[1][2]) / m[1][1];
//			mathassert(EqualAbs(v, Clamp01(v)));
			v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
			// The solution is (u,v,t)=(0,v,1).
			if (otherPt)
				*otherPt = lineSegment.b;
			return a + v * e1;
		}
		else
		{
			// The solution is (u,v,t)=(0,v,t).
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return a + v * e1;
		}
	}
	else if (uvt.y < 0.f)
	{
		// Clamp to v == 0 and solve again.
		float m_00 = m[2][2];
		float m_01 = -m[0][2];
		float m_10 = -m[2][0];
		float m_11 = m[0][0];
		float det = m_00 * m_11 - m_01 * m_10;
		float u = m_00 * B[0] + m_01 * B[2];
		float t = m_10 * B[0] + m_11 * B[2];
		u /= det;
		t /= det;

		if (u < 0.f)
		{
			// Clamp to u == 0 and solve for t.
			t = B[2] / m[2][2];
			t = Clamp01(t); // The solution for t must also be in the range [0,1].
			// The solution is (u,v,t)=(0,0,t).
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return a;
		}
		else if (u > 1.f)
		{
			// Clamp to u == 1 and solve for t.
			t = (B[2] - m[2][0]) / m[2][2];
			t = Clamp01(t); // The solution for t must also be in the range [0,1].
			// The solution is (u,v,t)=(1,0,t).
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return b;
		}
		else if (t < 0.f)
		{
			// Clamp to t == 0 and solve for u.
			u = B[0] / m[0][0];
//			mathassert(EqualAbs(u, Clamp01(u)));
			u = Clamp01(u); // The solution for u must also be in the range [0,1].
			if (otherPt)
				*otherPt = lineSegment.a;
			return a + u * e0;
		}
		else if (t > 1.f)
		{
			// Clamp to t == 1 and solve for u.
			u = (B[0] - m[0][2]) / m[0][0];
//			mathassert(EqualAbs(u, Clamp01(u)));
			u = Clamp01(u); // The solution for u must also be in the range [0,1].
			if (otherPt)
				*otherPt = lineSegment.b;
			return a + u * e0;
		}
		else
		{
			// The solution is (u, 0, t).
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return a + u * e0;
		}
	}
	else if (uvt.z < 0.f)
	{
		if (otherPt)
			*otherPt = lineSegment.a;
		// Clamp to t == 0 and solve again.
		float m_00 = m[1][1];
		float m_01 = -m[0][1];
		float m_10 = -m[1][0];
		float m_11 = m[0][0];
		float det = m_00 * m_11 - m_01 * m_10;
		float u = m_00 * B[0] + m_01 * B[1];
		float v = m_10 * B[0] + m_11 * B[1];
		u /= det;
		v /= det;
		if (u < 0.f)
		{
			// Clamp to u == 0 and solve for v.
			v = B[1] / m[1][1];
			v = Clamp01(v);
			return a + v*e1;
		}
		else if (v < 0.f)
		{
			// Clamp to v == 0 and solve for u.
			u = B[0] / m[0][0];
			u = Clamp01(u);
			return a + u*e0;
		}
		else if (u+v > 1.f)
		{
			// Set v = 1-u and solve again.
//			u = (B[0] - m[0][0]) / (m[0][0] - m[0][1]);
//			mathassert(EqualAbs(u, Clamp01(u)));
//			u = Clamp01(u); // The solution for u must also be in the range [0,1].
//			return a + u*e0;

			// Clamp to v = 1-u and solve again.
			float m_00 = m[2][2];
			float m_01 = m[1][2] - m[0][2];
			float m_10 = m_01;
			float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
			float det = m_00 * m_11 - m_01 * m_10;
			float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
			float b1 = -m[1][2] + v_p_dot_d;
			float u = m_00 * b0 + m_01 * b1;
			u /= det;
			u = Clamp01(u);

			float t = m_10 * b0 + m_11 * b1;
			t /= det;
			t = Clamp01(t);
			if (otherPt)
				*otherPt = lineSegment.GetPoint(t);
			return a + u*e0 + (1.f-u)*e1;
		}
		else
		{
			// The solution is (u, v, 0)
			return a + u * e0 + v * e1;
		}
	}
	else if (uvt.z > 1.f)
	{
		if (otherPt)
			*otherPt = lineSegment.b;
		// Clamp to t == 1 and solve again.
		float m_00 = m[1][1];
		float m_01 = -m[0][1];
		float m_10 = -m[1][0];
		float m_11 = m[0][0];
		float det = m_00 * m_11 - m_01 * m_10;
		float u = m_00 * (B[0]-m[0][2]) + m_01 * (B[1]-m[1][2]);
		float v = m_10 * (B[0]-m[0][2]) + m_11 * (B[1]-m[1][2]);
		u /= det;
		v /= det;
		if (u < 0.f)
		{
			// Clamp to u == 0 and solve again.
			v = (B[1] - m[1][2]) / m[1][1];
			v = Clamp01(v);
			return a + v*e1;
		}
		else if (u > 1.f)
		{
			// Clamp to u == 1 and solve again.
			v = (B[1] - m[1][0] - m[1][2]) / m[1][1];
			v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
			// The solution is (u,v,t)=(1,v,1).
			return a + e0 + v*e1;
		}
		else if (u+v > 1.f)
		{
			// Set v = 1-u and solve again.

			// Q(u,1-u) = a + u*e0 + e1 - u*e1 = a+e1 + u*(e0-e1)
			// L(1)   = ls.a + t*d = ls.b
			// Minimize the distance |Q(u,1-u) - L(1)| = |a+e1+ls.b + u*(e0-e1)|

			// |K + u*(e0-e1)|^2 = (K,K) + 2*u(K,e0-e1) + u^2 * (e0-e1,e0-e1)

			// grad = 2*(K,e0-e1) + 2*u*(e0-e1,e0-e1) == 0
			//                                      u == (K,e1-e0) / (e0-e1,e0-e1)

			u = (B[0] - m[0][1] - m[0][2]) / (m[0][0] - m[0][1]);
//			u = Dot(a + e1 + lineSegment.b, e1 - e0) / Dot(e0-e1, e0-e1);

//			mathassert(EqualAbs(u, Clamp01(u)));
			u = Clamp01(u);
			return a + u*e0 + (1-u)*e1;
		}
		else
		{
			// The solution is (u, v, 1)
			return a + u*e0 + v*e1;
		}
	}
	else if (uvt.x + uvt.y > 1.f)
	{
		// Clamp to v = 1-u and solve again.
		float m_00 = m[2][2];
		float m_01 = m[1][2] - m[0][2];
		float m_10 = m_01;
		float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
		float det = m_00 * m_11 - m_01 * m_10;
		float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
		float b1 = -m[1][2] + v_p_dot_d;
		float u = m_00 * b0 + m_01 * b1;
		float t = m_10 * b0 + m_11 * b1;
		u /= det;
		t /= det;

		t = Clamp01(t);
		if (otherPt)
			*otherPt = lineSegment.GetPoint(t);

		if (u < 0.f)
		{
			// The solution is (u,v,t)=(0,1,t)
			return c;
		}
		if (u > 1.f)
		{
			// The solution is (u,v,t)=(1,0,t)
			return b;
		}
		mathassert(t >= 0.f);
		mathassert(t <= 1.f);
		return a + u*e0 + (1.f-u)*e1;
	}
	else // All parameters are within range, so the triangle and the line segment intersect, and the intersection point is the closest point.
	{
		if (otherPt)
			*otherPt = lineSegment.GetPoint(uvt.z);
		return a + uvt.x * e0 + uvt.y * e1;
	}
}