Beispiel #1
0
bool Polyhedron::IsConvex() const
{
	// This function is O(n^2).
	/** @todo Real-Time Collision Detection, p. 64:
		A faster O(n) approach is to compute for each face F of P the centroid C of F,
		and for all neighboring faces G of F test if C lies behind the supporting plane of
		G. If some C fails to lie behind the supporting plane of one or more neighboring
		faces, P is concave, and is otherwise assumed convex. However, note that just as the
		corresponding polygonal convexity test may fail for a pentagram this test may fail for,
		for example, a pentagram extruded out of its plane and capped at the ends. */

	for(int f = 0; f < NumFaces(); ++f)
	{
		Plane p = FacePlane(f);
		for(int i = 0; i < NumVertices(); ++i)
		{
			float d = p.SignedDistance(Vertex(i));
			if (d > 1e-3f) // Tolerate a small epsilon error.
			{
				printf("Distance of vertex %d from plane %d: %f", i, f, d);
				return false;
			}
		}
	}
	return true;
}
Beispiel #2
0
PBVolume<6> AABB::ToPBVolume() const
{
	PBVolume<6> pbVolume;
	for(int i = 0; i < 6; ++i)
		pbVolume.p[i] = FacePlane(i);

	return pbVolume;
}
Beispiel #3
0
bool Polyhedron::ContainsConvex(const float3 &point) const
{
	assume(IsConvex());
	for(int i = 0; i < NumFaces(); ++i)
		if (FacePlane(i).SignedDistance(point) > 0.f)
			return false;

	return true;
}
Beispiel #4
0
void AABB::GetFacePlanes(Plane *outPlaneArray) const
{
	assume(outPlaneArray);
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
	if (!outPlaneArray)
		return;
#endif
	for(int i = 0; i < 6; ++i)
		outPlaneArray[i] = FacePlane(i);
}
Beispiel #5
0
///
//	CheckPoint()
//
//	Test the point "alpha" of the way from P1 to P2 
//  See if it is on a face of the cube   
//	Consider only faces in "mask"                   
static
int CheckPoint(const vector3& p1, const vector3& p2, number alpha, long mask)
{
	vector3 plane_point;

	plane_point.x() = LERP(alpha, p1.x(), p2.x());
	plane_point.y() = LERP(alpha, p1.y(), p2.y());
	plane_point.z() = LERP(alpha, p1.z(), p2.z());
	return(FacePlane(plane_point) & mask);
}
Beispiel #6
0
void Polyhedron::OrientNormalsOutsideConvex()
{
	float3 center = v[0];
	for(size_t i = 1; i < v.size(); ++i)
		center += v[i];

	center /= (float)v.size();
	for(int i = 0; i < (int)f.size(); ++i)
		if (FacePlane(i).SignedDistance(center) > 0.f)
			f[i].FlipWindingOrder();
}
Beispiel #7
0
bool Polyhedron::Intersects(const LineSegment &lineSegment) const
{
	if (Contains(lineSegment))
		return true;
	for(int i = 0; i < NumFaces(); ++i)
	{
		float t;
		Plane plane = FacePlane(i);
		bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
		if (intersects && t >= 0.f && t <= 1.f)
			if (FaceContains(i, lineSegment.GetPoint(t)))
				return true;
	}

	return false;
}
Beispiel #8
0
bool Polyhedron::FacesAreNondegeneratePlanar(float epsilon) const
{
	for(int i = 0; i < (int)f.size(); ++i)
	{
		const Face &face = f[i];
		if (face.v.size() < 3)
			return false;
		if (face.v.size() >= 4)
		{
			Plane facePlane = FacePlane(i);
			for(int j = 0; j < (int)face.v.size(); ++j)
				if (facePlane.Distance(v[face.v[j]]) > epsilon)
					return false;
		}
	}

	return true;
}
Beispiel #9
0
bool Polyhedron::ClipLineSegmentToConvexPolyhedron(const float3 &ptA, const float3 &dir,
                                                   float &tFirst, float &tLast) const
{
	assume(IsConvex());

	// Intersect line segment against each plane.
	for(int i = 0; i < NumFaces(); ++i)
	{
		/* Denoting the dot product of vectors a and b with <a,b>, we have:

		   The points P on the plane p satisfy the equation <P, p.normal> == p.d.
		   The points P on the line have the parametric equation P = ptA + dir * t.
		   Solving for the distance along the line for intersection gives
		
		   t = (p.d - <p.normal, ptA>) / <p.normal, dir>.
		*/

		Plane p = FacePlane(i);
		float denom = Dot(p.normal, dir);
		float dist = p.d - Dot(p.normal, ptA);

		// Avoid division by zero. In this case the line segment runs parallel to the plane.
		if (Abs(denom) < 1e-5f)
		{
			// If <P, p.normal> < p.d, then the point lies in the negative halfspace of the plane, which is inside the polyhedron.
			// If <P, p.normal> > p.d, then the point lies in the positive halfspace of the plane, which is outside the polyhedron.
			// Therefore, if p.d - <ptA, p.normal> == dist < 0, then the whole line is outside the polyhedron.
			if (dist < 0.f)
				return false;
		}
		else
		{
			float t = dist / denom;
			if (denom < 0.f) // When entering halfspace, update tFirst if t is larger.
				tFirst = Max(t, tFirst);
			else // When exiting halfspace, updeate tLast if t is smaller.
				tLast = Min(t, tLast);

			if (tFirst > tLast)
				return false; // We clipped the whole line segment.
		}
	}
	return true;
}
Beispiel #10
0
void OBB::GetFacePlanes(Plane *outPlaneArray) const
{
    assert(outPlaneArray);
    for(int i = 0; i < 6; ++i)
        outPlaneArray[i] = FacePlane(i);
}
Beispiel #11
0
void Polyhedron::MergeConvex(const float3 &point)
{
//	LOGI("mergeconvex.");
	std::set<std::pair<int, int> > deletedEdges;
	std::map<std::pair<int, int>, int> remainingEdges;

	for(size_t i = 0; i < v.size(); ++i)
		if (point.DistanceSq(v[i]) < 1e-3f)
			return;

//	bool hadDisconnectedHorizon = false;

	for(int i = 0; i < (int)f.size(); ++i)
	{
		// Delete all faces that don't contain the given point. (they have point in their positive side)
		Plane p = FacePlane(i);
		Face &face = f[i];
		if (p.SignedDistance(point) > 1e-5f)
		{
			bool isConnected = (deletedEdges.empty());

			int v0 = face.v.back();
			for(size_t j = 0; j < face.v.size() && !isConnected; ++j)
			{
				int v1 = face.v[j];
				if (deletedEdges.find(std::make_pair(v1, v0)) != deletedEdges.end())
				{
					isConnected = true;
					break;
				}
				v0 = v1;
			}

			if (isConnected)
			{
				v0 = face.v.back();
				for(size_t j = 0; j < face.v.size(); ++j)
				{
					int v1 = face.v[j];
					deletedEdges.insert(std::make_pair(v0, v1));
			//		LOGI("Edge %d,%d is to be deleted.", v0, v1);
					v0 = v1;
				}
		//		LOGI("Deleting face %d: %s. Distance to vertex %f", i, face.ToString().c_str(), p.SignedDistance(point));
				std::swap(f[i], f.back());
				f.pop_back();
				--i;
				continue;
			}
//			else
//				hadDisconnectedHorizon = true;
		}

		int v0 = face.v.back();
		for(size_t j = 0; j < face.v.size(); ++j)
		{
			int v1 = face.v[j];
			remainingEdges[std::make_pair(v0, v1)] = i;
	//		LOGI("Edge %d,%d is to be deleted.", v0, v1);
			v0 = v1;
		}

	}

	// The polyhedron contained our point, nothing to merge.
	if (deletedEdges.empty())
		return;

	// Add the new point to this polyhedron.
//	if (!v.back().Equals(point))
		v.push_back(point);

/*
	// Create a look-up index of all remaining uncapped edges of the polyhedron.
	std::map<std::pair<int,int>, int> edgesToFaces;
	for(size_t i = 0; i < f.size(); ++i)
	{
		Face &face = f[i];
		int v0 = face.v.back();
		for(size_t j = 0; j < face.v.size(); ++j)
		{
			int v1 = face.v[j];
			edgesToFaces[std::make_pair(v1, v0)] = i;
			v0 = v1;
		}
	}
*/
	// Now fix all edges by adding new triangular faces for the point.
//	for(size_t i = 0; i < deletedEdges.size(); ++i)
	for(std::set<std::pair<int, int> >::iterator iter = deletedEdges.begin(); iter != deletedEdges.end(); ++iter)
	{
		std::pair<int, int> opposite = std::make_pair(iter->second, iter->first);
		if (deletedEdges.find(opposite) != deletedEdges.end())
			continue;

//		std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
//		std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
//		if (iter != edgesToFaces.end())
		{
			// If the adjoining face is planar to the triangle we'd like to add, instead extend the face to enclose
			// this vertex.
			//float3 newTriangleNormal = (v[v.size()-1]-v[iter->second]).Cross(v[iter->first]-v[iter->second]).Normalized();

			std::map<std::pair<int, int>, int>::iterator existing = remainingEdges.find(opposite);
			assert(existing != remainingEdges.end());
			MARK_UNUSED(existing);

#if 0			
			int adjoiningFace = existing->second;

			if (FaceNormal(adjoiningFace).Dot(newTriangleNormal) >= 0.99999f) ///\todo float3::IsCollinear
			{
				bool added = false;
				Face &adjoining = f[adjoiningFace];
				for(size_t i = 0; i < adjoining.v.size(); ++i)
					if (adjoining.v[i] == iter->second)
					{
						adjoining.v.insert(adjoining.v.begin() + i + 1, v.size()-1);
						added = true;
						/*
						int prev2 = (i + adjoining.v.size() - 1) % adjoining.v.size();
						int prev = i;
						int cur = i + 1;
						int next = (i + 2) % adjoining.v.size();
						int next2 = (i + 3) % adjoining.v.size();

						if (float3::AreCollinear(v[prev2], v[prev], v[cur]))
							adjoining.v.erase(adjoining.v.begin() + prev);
						else if (float3::AreCollinear(v[prev], v[cur], v[next]))
							adjoining.v.erase(adjoining.v.begin() + cur);
						else if (float3::AreCollinear(v[cur], v[next], v[next2]))
							adjoining.v.erase(adjoining.v.begin() + next2);
							*/

						break;
					}
				assert(added);
				assume(added);
			}
			else
#endif
//			if (!v[deletedEdges[i].first].Equals(point) && !v[deletedEdges[i].second].Equals(point))
			{
				Face tri;
				tri.v.push_back(iter->second);
				tri.v.push_back((int)v.size()-1);
				tri.v.push_back(iter->first);
				f.push_back(tri);
	//			LOGI("Added face %d: %s.", (int)f.size()-1, tri.ToString().c_str());
			}
		}
	}

#define mathasserteq(lhs, op, rhs) do { if (!((lhs) op (rhs))) { LOGE("Condition %s %s %s (%g %s %g) failed!", #lhs, #op, #rhs, (double)(lhs), #op, (double)(rhs)); assert(false); } } while(0)

//	mathasserteq(NumVertices() + NumFaces(), ==, 2 + NumEdges());
	assert(FaceIndicesValid());
//	assert(EulerFormulaHolds());
//	assert(IsClosed());
//	assert(FacesAreNondegeneratePlanar());
//	assert(IsConvex());

//	if (hadDisconnectedHorizon)
//		MergeConvex(point);
}
Beispiel #12
0
bool Polyhedron::FaceContains(int faceIndex, const float3 &worldSpacePoint, float polygonThickness) const
{
	// N.B. This implementation is a duplicate of Polygon::Contains, but adapted to avoid dynamic memory allocation
	// related to converting the face of a Polyhedron to a Polygon object.

	// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/

	const Face &face = f[faceIndex];
	const std::vector<int> &vertices = face.v;

	if (vertices.size() < 3)
		return false;

	Plane p = FacePlane(faceIndex);
	if (FacePlane(faceIndex).Distance(worldSpacePoint) > polygonThickness)
		return false;

	int numIntersections = 0;

	float3 basisU = v[vertices[1]] - v[vertices[0]];
	basisU.Normalize();
	float3 basisV = Cross(p.normal, basisU).Normalized();
	mathassert(basisU.IsNormalized());
	mathassert(basisV.IsNormalized());
	mathassert(basisU.IsPerpendicular(basisV));
	mathassert(basisU.IsPerpendicular(p.normal));
	mathassert(basisV.IsPerpendicular(p.normal));

	float2 localSpacePoint = float2(Dot(worldSpacePoint, basisU), Dot(worldSpacePoint, basisV));

	const float epsilon = 1e-4f;

	float2 p0 = float2(Dot(v[vertices.back()], basisU), Dot(v[vertices.back()], basisV)) - localSpacePoint;
	if (Abs(p0.y) < epsilon)
		p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
	for(size_t i = 0; i < vertices.size(); ++i)
	{
		float2 p1 = float2(Dot(v[vertices[i]], basisU), Dot(v[vertices[i]], basisV)) - localSpacePoint;
		if (Abs(p1.y) < epsilon)
			p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.

		if (p0.y * p1.y < 0.f)
		{
			if (p0.x > 1e-3f && p1.x > 1e-3f)
				++numIntersections;
			else
			{
				// P = p0 + t*(p1-p0) == (x,0)
				//     p0.x + t*(p1.x-p0.x) == x
				//     p0.y + t*(p1.y-p0.y) == 0
				//                 t == -p0.y / (p1.y - p0.y)

				// Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate.
				float2 d = p1 - p0;
				if (Abs(d.y) > 1e-5f)
				{
					float t = -p0.y / d.y;
					float x = p0.x + t * d.x;
					if (t >= 0.f && t <= 1.f && x > 1e-6f)
						++numIntersections;
				}
			}
		}
		p0 = p1;
	}

	return numIntersections % 2 == 1;
}
Beispiel #13
0
///
//	TriCubeIntersection()
//
//		Triangle t is compared with a unit cube,  
//		centered on the origin.                    
//		It returns INSIDE (0) or OUTSIDE(1) if t   
//		Intersects or does not intersect the cube. 
static
int TriCubeIntersection(const TRI& t)
{
	int v1_test,v2_test,v3_test;
	number d;
	vector3 vect12,vect13,norm;
	vector3 hitpp,hitpn,hitnp,hitnn;

	///
	//	First compare all three vertexes with all six face-planes 
	//	If any vertex is inside the cube, return immediately!     
	//
	if ((v1_test = FacePlane(t.m_P[0])) == INSIDE) return(INSIDE);
	if ((v2_test = FacePlane(t.m_P[1])) == INSIDE) return(INSIDE);
	if ((v3_test = FacePlane(t.m_P[2])) == INSIDE) return(INSIDE);

	///
	//	If all three vertexes were outside of one or more face-planes,
	//	return immediately with a trivial rejection!              
	//
	if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);

	///
	//	Now do the same trivial rejection test for the 12 edge planes 
	//
	v1_test |= Bevel2d(t.m_P[0]) << 8; 
	v2_test |= Bevel2d(t.m_P[1]) << 8; 
	v3_test |= Bevel2d(t.m_P[2]) << 8;
	if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);  

	///
	//	Now do the same trivial rejection test for the 8 corner planes
	//
	v1_test |= Bevel3d(t.m_P[0]) << 24; 
	v2_test |= Bevel3d(t.m_P[1]) << 24; 
	v3_test |= Bevel3d(t.m_P[2]) << 24; 
	if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);   

	///
	//	If vertex 1 and 2, as a pair, cannot be trivially rejected 
	//  by the above tests, then see if the v1-->v2 triangle edge  
	//	intersects the cube.  Do the same for v1-->v3 and v2-->v3. 
	//  Pass to the intersection algorithm the "OR" of the outcode 
	//  bits, so that only those cube faces which are spanned by   
	//  each triangle edge need be tested.                         
	//
	if ((v1_test & v2_test) == 0)
		if (CheckLine(t.m_P[0],t.m_P[1],v1_test|v2_test) == INSIDE) return(INSIDE);
	if ((v1_test & v3_test) == 0)
		if (CheckLine(t.m_P[0],t.m_P[2],v1_test|v3_test) == INSIDE) return(INSIDE);
	if ((v2_test & v3_test) == 0)
		if (CheckLine(t.m_P[1],t.m_P[2],v2_test|v3_test) == INSIDE) return(INSIDE);

	///
	//	By now, we know that the triangle is not off to any side,     
	//	 and that its sides do not penetrate the cube.  We must now   
	//	 test for the cube intersecting the interior of the triangle. 
	//	 We do this by looking for intersections between the cube     
	//	 diagonals and the triangle...first finding the intersection  
	//	 of the four diagonals with the plane of the triangle, and    
	//	 then if that intersection is inside the cube, pursuing       
	//	 whether the intersection point is inside the triangle itself. 

	//	 To find plane of the triangle, first perform crossproduct on  
	//	 two triangle side vectors to compute the normal vector.                                       
	SUB(t.m_P[0],t.m_P[1],vect12);
	SUB(t.m_P[0],t.m_P[2],vect13);
	CROSS(vect12,vect13,norm)

	///
	//	 The normal vector "norm" X,Y,Z components are the coefficients 
	//		 of the triangles AX + BY + CZ + D = 0 plane equation.  If we   
	//	 solve the plane equation for X=Y=Z (a diagonal), we get        
	//	 -D/(A+B+C) as a metric of the distance from cube center to the 
	//	 diagonal/plane intersection.  If this is between -0.5 and 0.5, 
	//	 the intersection is inside the cube.  If so, we continue by    
	//	 doing a point/triangle intersection.                           
	//	 Do this for all four diagonals.                                
	d = norm.x() * t.m_P[0].x() + norm.y() * t.m_P[0].y() + norm.z() * t.m_P[0].z();
	hitpp.x() = hitpp.y() = hitpp.z() = d / (norm.x() + norm.y() + norm.z());
	if (fabs(hitpp.x()) <= 0.5)
		if (PointTriangleIntersection(hitpp,t) == INSIDE) return(INSIDE);
	hitpn.z() = -(hitpn.x() = hitpn.y() = d / (norm.x() + norm.y() - norm.z()));
	if (fabs(hitpn.x()) <= 0.5)
		if (PointTriangleIntersection(hitpn,t) == INSIDE) return(INSIDE);
	hitnp.y() = -(hitnp.x() = hitnp.z() = d / (norm.x() - norm.y() + norm.z()));
	if (fabs(hitnp.x()) <= 0.5)
		if (PointTriangleIntersection(hitnp,t) == INSIDE) return(INSIDE);
	hitnn.y() = hitnn.z() = -(hitnn.x() = d / (norm.x() - norm.y() - norm.z()));
	if (fabs(hitnn.x()) <= 0.5)
		if (PointTriangleIntersection(hitnn,t) == INSIDE) return(INSIDE);

	///
	//	No edge touched the cube; no cube diagonal touched the triangle. 
	//  We're done...there was no intersection.                          
	//
	return(OUTSIDE);
}