예제 #1
0
		public bool closestPtPointTetrahedron( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d, btSubSimplexClosestResult finalResult )
		{
			btSubSimplexClosestResult tempResult = new btSubSimplexClosestResult();

			// Start out assuming point inside all halfspaces, so closest to itself
			finalResult.m_closestPointOnSimplex = p;
			finalResult.m_usedVertices = 0;
			finalResult.m_usedVertices |= btUsageBitfield.usedVertexA;
			finalResult.m_usedVertices |= btUsageBitfield.usedVertexB;
			finalResult.m_usedVertices |= btUsageBitfield.usedVertexC;
			finalResult.m_usedVertices |= btUsageBitfield.usedVertexD;

			int pointOutsideABC = pointOutsideOfPlane( ref p, ref a, ref b, ref c, ref d );
			int pointOutsideACD = pointOutsideOfPlane( ref p, ref a, ref c, ref d, ref b );
			int pointOutsideADB = pointOutsideOfPlane( ref p, ref a, ref d, ref b, ref c );
			int pointOutsideBDC = pointOutsideOfPlane( ref p, ref b, ref d, ref c, ref a );

			if( pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0 )
			{
				finalResult.m_degenerate = true;
				return false;
			}

			if( pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0 )
			{
				return false;
			}


			double bestSqDist = btScalar.BT_MAX_FLOAT;
			// If point outside face abc then compute closest point on abc
			btVector3 tmp;

			if( pointOutsideABC != 0 )
			{
				closestPtPointTriangle( ref p, ref a, ref b, ref c, ref tempResult );
				//btVector3 q = tempResult.m_closestPointOnSimplex;
				tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
				double sqDist = ( tmp ).dot( ref tmp );
				// Update best closest point if (squared) distance is less than current best
				if( sqDist < bestSqDist )
				{
					bestSqDist = sqDist;
					finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
					//convert result bitmask!
					finalResult.m_usedVertices = 0;
					finalResult.m_usedVertices = tempResult.m_usedVertices
							& ( btUsageBitfield.usedVertexA | btUsageBitfield.usedVertexB | btUsageBitfield.usedVertexC );
					finalResult.setBarycentricCoordinates(
							tempResult.m_barycentricCoord0,
							tempResult.m_barycentricCoord1,
							tempResult.m_barycentricCoord2,
							0
					);

				}
			}


			// Repeat test for face acd
			if( pointOutsideACD != 0 )
			{
				closestPtPointTriangle( ref p, ref a, ref c, ref d, ref tempResult );
				//btVector3 q = tempResult.m_closestPointOnSimplex;
				//convert result bitmask!

				tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
				double sqDist = ( tmp ).dot( ref tmp );
				//double sqDist = ( q - p ).dot( q - p );
				if( sqDist < bestSqDist )
				{
					bestSqDist = sqDist;
					finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
					finalResult.m_usedVertices = 0;
					finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA );

					finalResult.m_usedVertices |= ((tempResult.m_usedVertices & btUsageBitfield.usedVertexB) != 0)? btUsageBitfield.usedVertexC : 0;
                    finalResult.m_usedVertices |= ((tempResult.m_usedVertices & btUsageBitfield.usedVertexC) != 0)? btUsageBitfield.usedVertexD : 0;
					finalResult.setBarycentricCoordinates(
							tempResult.m_barycentricCoord0,
							0,
							tempResult.m_barycentricCoord1,
							tempResult.m_barycentricCoord2
					);

				}
			}
			// Repeat test for face adb


			if( pointOutsideADB != 0 )
			{
				closestPtPointTriangle( ref p, ref a, ref d, ref b, ref tempResult );
				//btVector3 q = tempResult.m_closestPointOnSimplex;
				//convert result bitmask!

				tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
				double sqDist = ( tmp ).dot( ref tmp );
				//double sqDist = ( q - p ).dot( q - p );
				if( sqDist < bestSqDist )
				{
					bestSqDist = sqDist;
					finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
					finalResult.m_usedVertices = 0;
					finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA );
					finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexC ) != 0 ) ? btUsageBitfield.usedVertexB : 0;
					finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexB ) != 0 ) ? btUsageBitfield.usedVertexD : 0;
					finalResult.setBarycentricCoordinates(
							tempResult.m_barycentricCoord0,
							tempResult.m_barycentricCoord2,
							0,
							tempResult.m_barycentricCoord3
					);

				}
			}
			// Repeat test for face bdc


			if( pointOutsideBDC != 0 )
			{
				closestPtPointTriangle( ref p, ref b, ref d, ref c, ref tempResult );
				//btVector3 q = tempResult.m_closestPointOnSimplex;
				//convert result bitmask!
				tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
				double sqDist = ( tmp ).dot( ref tmp );
				//double sqDist = ( q - p ).dot( q - p );
				if( sqDist < bestSqDist )
				{
					bestSqDist = sqDist;
					finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
					finalResult.m_usedVertices = 0;
					//
					finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexA ) != 0 ) ? btUsageBitfield.usedVertexB : 0;
					finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexC ) != 0 ) ? btUsageBitfield.usedVertexC : 0;
					finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexB ) != 0 ) ? btUsageBitfield.usedVertexD : 0;

					finalResult.setBarycentricCoordinates(
							0,
							tempResult.m_barycentricCoord0,
							tempResult.m_barycentricCoord2,
							tempResult.m_barycentricCoord2
					);

				}
			}

			//help! we ended up full !

			if( ( finalResult.m_usedVertices & btUsageBitfield.usedAll ) == btUsageBitfield.usedAll )
			{
				return true;
			}

			return true;
		}
예제 #2
0
		public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result )
		{
			result.m_usedVertices = 0;//.reset();

			// Check if P in vertex region outside A
			btVector3 ab; b.Sub( ref a, out ab );
			btVector3 ac; c.Sub( ref a, out ac );
			btVector3 ap; p.Sub( ref a, out ap );
			double d1 = ab.dot( ref ap );
			double d2 = ac.dot( ref ap );
			if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) )
			{
				result.m_closestPointOnSimplex = a;
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.setBarycentricCoordinates( 1, 0, 0 );
				return true;// a; // barycentric coordinates (1,0,0)
			}

			// Check if P in vertex region outside B
			btVector3 bp; p.Sub( ref b, out bp );
			double d3 = ab.dot( ref bp );
			double d4 = ac.dot( ref bp );
			if( d3 >= (double)( 0.0 ) && d4 <= d3 )
			{
				result.m_closestPointOnSimplex = b;
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.setBarycentricCoordinates( 0, 1, 0 );

				return true; // b; // barycentric coordinates (0,1,0)
			}
			// Check if P in edge region of AB, if so return projection of P onto AB
			double vc = d1 * d4 - d3 * d2;
			btVector3 tmp;
			if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) )
			{
				double v = d1 / ( d1 - d3 );
				ab.Mult( v, out tmp );
				a.Add( ref tmp, out result.m_closestPointOnSimplex );
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.setBarycentricCoordinates( 1 - v, v, 0 );
				return true;
				//return a + v * ab; // barycentric coordinates (1-v,v,0)
			}

			// Check if P in vertex region outside C
			btVector3 cp; p.Sub( ref c, out cp );
			double d5 = ab.dot( ref cp );
			double d6 = ac.dot( ref cp );
			if( d6 >= (double)( 0.0 ) && d5 <= d6 )
			{
				result.m_closestPointOnSimplex = c;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 0, 0, 1 );
				return true;//c; // barycentric coordinates (0,0,1)
			}

			// Check if P in edge region of AC, if so return projection of P onto AC
			double vb = d5 * d2 - d1 * d6;
			if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) )
			{
				double w = d2 / ( d2 - d6 );
				ac.Mult( w, out tmp );
				a.Add( ref tmp, out result.m_closestPointOnSimplex );
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 1 - w, 0, w );
				return true;
				//return a + w * ac; // barycentric coordinates (1-w,0,w)
			}

			// Check if P in edge region of BC, if so return projection of P onto BC
			double va = d3 * d6 - d5 * d4;
			if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) )
			{
				double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
				btVector3 tmp2;
				c.Sub( ref b, out tmp2 );
				tmp2.Mult( w, out tmp );
				b.Add( ref tmp, out result.m_closestPointOnSimplex );
				//result.m_closestPointOnSimplex = b + w * ( c - b );
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 0, 1 - w, w );
				return true;
				// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
			}

			// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
			{
				double denom = (double)( 1.0 ) / ( va + vb + vc );
				double v = vb * denom;
				double w = vc * denom;
				ab.Mult( v, out ab );
				ac.Mult( w, out ac );
				a.Add( ref ab, out tmp );
				tmp.Add( ref ac, out result.m_closestPointOnSimplex );

				//result.m_closestPointOnSimplex = a + ab * v + ac * w;
				result.m_usedVertices |= btUsageBitfield.usedVertexA;
				result.m_usedVertices |= btUsageBitfield.usedVertexB;
				result.m_usedVertices |= btUsageBitfield.usedVertexC;
				result.setBarycentricCoordinates( 1 - v - w, v, w );
			}
			return true;
			//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = (double)(1.0) - v - w

		}