コード例 #1
0
ファイル: GjkPairDetector.cs プロジェクト: d3x0r/Voxelarium
		internal  void getClosestPointsNonVirtual( btDiscreteCollisionDetectorInterface.ClosestPointInput input
			, btDiscreteCollisionDetectorInterface.Result output, btIDebugDraw debugDraw )
#endif
		{
			m_cachedSeparatingDistance = 0;

			double distance = btScalar.BT_ZERO;
			btVector3 normalInB = btVector3.Zero;

			btVector3 pointOnA, pointOnB = btVector3.Zero;
			btTransform localTransA; input.m_transformA.Get( out localTransA );
			btTransform localTransB; input.m_transformB.Get( out localTransB );
			btVector3 positionOffset; localTransA.m_origin.Add( ref localTransB.m_origin, out positionOffset );
			positionOffset.Mult( (double)( 0.5 ), out positionOffset );

			localTransA.m_origin.Sub( ref positionOffset, out localTransA.m_origin );
			localTransB.m_origin.Sub( ref positionOffset, out localTransB.m_origin );

			bool check2d = m_minkowskiA.isConvex2d() && m_minkowskiB.isConvex2d();

			double marginA = m_marginA;
			double marginB = m_marginB;

			gNumGjkChecks++;

			//for CCD we don't use margins
			if( m_ignoreMargin )
			{
				marginA = btScalar.BT_ZERO;
				marginB = btScalar.BT_ZERO;
			}

			m_curIter = 0;
			int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
			m_cachedSeparatingAxis.setValue( 0, 1, 0 );

			bool isValid = false;
			bool checkSimplex = false;
			bool checkPenetration = true;
			m_degenerateSimplex = 0;

			m_lastUsedMethod = -1;

			{
				double squaredDistance = btScalar.BT_LARGE_FLOAT;
				double delta = btScalar.BT_ZERO;

				double margin = marginA + marginB;



				m_simplexSolver.reset();

				for( ;;)
				//while (true)
				{

					btVector3 tmp;
					m_cachedSeparatingAxis.Invert( out tmp );

					btVector3 seperatingAxisInA; localTransA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
					btVector3 seperatingAxisInB; localTransA.m_basis.ApplyInverse( ref m_cachedSeparatingAxis, out seperatingAxisInB );


					btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
					btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );

					btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
					btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
					btScalar.Dbg( "pWorld is " + pWorld + " qWorld is " + qWorld );

					if( check2d )
					{
						pWorld[2] = 0;
						qWorld[2] = 0;
					}

					btVector3 w; pWorld.Sub( ref qWorld, out w );
					delta = m_cachedSeparatingAxis.dot( ref w );

					// potential exit, they don't overlap
					if( ( delta > (double)( 0.0 ) ) && ( delta * delta > squaredDistance * input.m_maximumDistanceSquared ) )
					{
						m_degenerateSimplex = 10;
						checkSimplex = true;
						//checkPenetration = false;
						break;
					}

					//exit 0: the new point is already in the simplex, or we didn't come any closer
					if( m_simplexSolver.inSimplex( ref w ) )
					{
						m_degenerateSimplex = 1;
						checkSimplex = true;
						break;
					}
					// are we getting any closer ?
					double f0 = squaredDistance - delta;
					double f1 = squaredDistance * REL_ERROR2;
					btScalar.Dbg( "f0 is " + f0.ToString( "g17" ) + " f1 is " + f1.ToString( "g17" ) );

					if( f0 <= f1 )
					{
						if( f0 <= btScalar.BT_ZERO )
						{
							m_degenerateSimplex = 2;
						}
						else
						{
							m_degenerateSimplex = 11;
						}
						checkSimplex = true;
						break;
					}

					//add current vertex to simplex
					m_simplexSolver.addVertex( ref w, ref pWorld, ref qWorld );
					btVector3 newCachedSeparatingAxis;

					//calculate the closest point to the origin (update vector v)
					if( !m_simplexSolver.closest( out newCachedSeparatingAxis ) )
					{
						m_degenerateSimplex = 3;
						checkSimplex = true;
						break;
					}

					if( newCachedSeparatingAxis.length2() < REL_ERROR2 )
					{
						m_cachedSeparatingAxis = newCachedSeparatingAxis;
						m_degenerateSimplex = 6;
						checkSimplex = true;
						break;
					}

					double previousSquaredDistance = squaredDistance;
					squaredDistance = newCachedSeparatingAxis.length2();
#if asdfasdf
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
			if (squaredDistance>previousSquaredDistance)
			{
				m_degenerateSimplex = 7;
				squaredDistance = previousSquaredDistance;
                checkSimplex = false;
                break;
			}
#endif //


					//redundant m_simplexSolver.compute_points(pointOnA, pointOnB);

					//are we getting any closer ?
					if( previousSquaredDistance - squaredDistance <= btScalar.SIMD_EPSILON * previousSquaredDistance )
					{
						//				m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
						checkSimplex = true;
						m_degenerateSimplex = 12;

						break;
					}

					m_cachedSeparatingAxis = newCachedSeparatingAxis;

					//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
					if( m_curIter++ > gGjkMaxIter )
					{
#if DEBUG
						Console.WriteLine( "btGjkPairDetector maxIter exceeded:{0}", m_curIter );
						Console.WriteLine( "sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4},shapeTypeB={5}\n",
							  m_cachedSeparatingAxis.x,
							  m_cachedSeparatingAxis.y,
							  m_cachedSeparatingAxis.z,
							  squaredDistance,
							  m_minkowskiA.getShapeType(),
							  m_minkowskiB.getShapeType() );

#endif
						break;

					}


					bool check = ( !m_simplexSolver.fullSimplex() );
					//bool check = (!m_simplexSolver.fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver.maxVertex());

					if( !check )
					{
						//do we need this backup_closest here ?
						//				m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
						m_degenerateSimplex = 13;
						break;
					}
				}

				if( checkSimplex )
				{
					m_simplexSolver.compute_points( out pointOnA, out pointOnB );
					btScalar.Dbg( "new simplex points " + pointOnA.ToString() + " and " + pointOnB );
					normalInB = m_cachedSeparatingAxis;

					double lenSqr = m_cachedSeparatingAxis.length2();

					//valid normal
					if( lenSqr < 0.0001 )
					{
						m_degenerateSimplex = 5;
					}
					if( lenSqr > btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
					{
						double rlen = btScalar.BT_ONE / btScalar.btSqrt( lenSqr );
						normalInB.Mult( rlen, out normalInB );
						//normalInB *= rlen; //normalize

						double s = btScalar.btSqrt( squaredDistance );

						Debug.Assert( s > (double)( 0.0 ) );
						pointOnA.SubScale( ref m_cachedSeparatingAxis, ( marginA / s ), out pointOnA );
						pointOnB.AddScale( ref m_cachedSeparatingAxis, ( marginB / s ), out pointOnB );
						//pointOnA -= m_cachedSeparatingAxis * ( marginA / s );
						//pointOnB += m_cachedSeparatingAxis * ( marginB / s );
						distance = ( ( btScalar.BT_ONE / rlen ) - margin );
						isValid = true;

						m_lastUsedMethod = 1;
					}
					else
					{
						m_lastUsedMethod = 2;
					}
				}

				bool catchDegeneratePenetrationCase =
					( m_catchDegeneracies != 0 
					  && m_penetrationDepthSolver != null 
					  && m_degenerateSimplex != 0 
					  && ( ( distance + margin ) < 0.01 ) );

				//if (checkPenetration && !isValid)
				if( checkPenetration && ( !isValid || catchDegeneratePenetrationCase ) )
				{
					//penetration case

					//if there is no way to handle penetrations, bail out
					if( m_penetrationDepthSolver != null )
					{
						// Penetration depth case.
						btVector3 tmpPointOnA, tmpPointOnB;

						gNumDeepPenetrationChecks++;
						m_cachedSeparatingAxis.setZero();

						bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
							m_simplexSolver,
							m_minkowskiA, m_minkowskiB,
							ref localTransA, ref localTransB,
							ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
							debugDraw
							);
						btScalar.Dbg( "points are " + tmpPointOnA.ToString() + " and " + tmpPointOnB.ToString() );

						if( isValid2 )
						{
							btVector3 tmpNormalInB; tmpPointOnB.Sub( ref tmpPointOnA, out tmpNormalInB );
							double lenSqr = tmpNormalInB.length2();
							if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
							{
								tmpNormalInB = m_cachedSeparatingAxis;
								lenSqr = m_cachedSeparatingAxis.length2();
							}

							if( lenSqr > ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
							{
								tmpNormalInB.Mult( btScalar.btSqrt( lenSqr ), out tmpNormalInB );
								btVector3 tmp;
								tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
								double distance2 = -tmp.length();
								m_lastUsedMethod = 3;
								//only replace valid penetrations when the result is deeper (check)
								if( !isValid || ( distance2 < distance ) )
								{
									distance = distance2;
									pointOnA = tmpPointOnA;
									pointOnB = tmpPointOnB;
									normalInB = tmpNormalInB;

									///todo: need to track down this EPA penetration solver degeneracy
									///the penetration solver reports penetration but the contact normal
									///connecting the contact points is pointing in the opposite direction
									///until then, detect the issue and revert the normal
									{
										btScalar d1 = 0;
										{
											normalInB.Invert( out tmp );
											btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInA );
											btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref tmp, out seperatingAxisInB );


											btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
											btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );

											btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
											btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
											btVector3 w; pWorld.Sub( ref qWorld, out w );
											d1 = ( tmp ).dot( w );
										}
										btScalar d0 = btScalar.BT_ZERO;
										{
											normalInB.Invert( out tmp );
											btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
											btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInB ) ;


											btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
											btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );

											btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
											btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
											btVector3 w; pWorld.Sub( ref qWorld, out w );
											d0 = normalInB.dot( w );
										}
										if( d1 > d0 )
										{
											m_lastUsedMethod = 10;
											normalInB *= -1;
										}

									}
									isValid = true;
								}
								else
								{
									m_lastUsedMethod = 8;
								}
							}
							else
							{
								m_lastUsedMethod = 9;
							}
						}
						else

						{
							///this is another degenerate case, where the initial GJK calculation reports a degenerate case
							///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
							///reports a valid positive distance. Use the results of the second GJK instead of failing.
							///thanks to Jacob.Langford for the reproduction case
							///http://code.google.com/p/bullet/issues/detail?id=250


							if( m_cachedSeparatingAxis.length2() > btScalar.BT_ZERO )
							{
								btVector3 tmp;
								tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
								double distance2 = tmp.length() - margin;
								//only replace valid distances when the distance is less
								btScalar.Dbg( "old distance " + distance2.ToString( "g17" ) + " new distance " + distance.ToString( "g17" ) );
								if( !isValid || ( distance2 < distance ) )
								{
									distance = distance2;
									pointOnA = tmpPointOnA;
									pointOnB = tmpPointOnB;
									pointOnA.SubScale( ref m_cachedSeparatingAxis, marginA, out pointOnA );
									pointOnB.AddScale( ref m_cachedSeparatingAxis, marginB, out pointOnA );
									//pointOnA -= m_cachedSeparatingAxis * marginA;
									//pointOnB += m_cachedSeparatingAxis * marginB;
									normalInB = m_cachedSeparatingAxis;
									normalInB.normalize();

									isValid = true;
									m_lastUsedMethod = 6;
								}
								else
								{
									m_lastUsedMethod = 5;
								}
							}
						}

					}

				}
			}

			btScalar.Dbg( "Pair detector : valid=" + (isValid?"1":"0" )+ " distance=" + distance.ToString( "g17" ) + " maxDistance=" + input.m_maximumDistanceSquared.ToString( "g17" ) );

			if( isValid && ( ( distance < 0 ) || ( distance * distance < input.m_maximumDistanceSquared ) ) )
			{

				m_cachedSeparatingAxis = normalInB;
				m_cachedSeparatingDistance = distance;
				btVector3 tmp;
				pointOnB.Add( ref positionOffset, out tmp );
				output.addContactPoint(
					ref normalInB,
					ref tmp,
					distance );

			}


		}