Exemplo n.º 1
0
		public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans
														, btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap
														, btCollisionObject collisionObject
														, ref btTransform colObjTransform
														, ConvexResultCallback resultCallback, double allowedPenetration )
		{
			//btCollisionShape collisionShape = colObjWrap.getCollisionShape();
			//btTransform colObjWorldTransform = colObjWrap.m_worldTransform;

			if( collisionShape.isConvex() )
			{
				//CProfileSample sample = new CProfileSample("convexSweepConvex");
				btConvexCast.CastResult castResult = new btConvexCast.CastResult();
				castResult.m_allowedPenetration = allowedPenetration;
				castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//??

				btConvexShape convexShape = (btConvexShape)collisionShape;
				btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();

				btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
				//	new btGjkEpaPenetrationDepthSolver();

				btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
				convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver );
				//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
				//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);

				btConvexCast castPtr = convexCaster1;

				if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
				{
					//add hit
					if( castResult.m_normal.length2() > (double)( 0.0001 ) )
					{
						if( castResult.m_fraction < resultCallback.m_closestHitFraction )
						{
							castResult.m_normal.normalize();
							LocalConvexResult localConvexResult =
								new LocalConvexResult
								(
								collisionObject,
								null,
								ref castResult.m_normal,
								ref castResult.m_hitPoint,
								castResult.m_fraction
								);

							bool normalInWorldSpace = true;
							resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );

						}
					}
				}
				BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver );
				BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
				BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 );
			}
			else
			{
				if( collisionShape.isConcave() )
				{
					if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
					{
						//CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh");
						btConcaveShape triangleMesh = (btConcaveShape)collisionShape;
						btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
						btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
						btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
						// rotation of box in local mesh space = MeshRotation^-1  ConvexToRotation
						btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
						rotationXform.m_origin = btVector3.Zero;
						//ConvexCast::CastResult

						BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
						tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans
							, resultCallback, collisionObject
							, triangleMesh, ref colObjTransform );
						tccb.m_hitFraction = resultCallback.m_closestHitFraction;
						tccb.m_allowedPenetration = allowedPenetration;
						btVector3 boxMinLocal, boxMaxLocal;
						castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
						triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal );

						BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
					}
					else
					{
						if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE )
						{
							btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get();
							castResult.m_allowedPenetration = allowedPenetration;
							castResult.m_fraction = resultCallback.m_closestHitFraction;
							btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape;
							btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
							convexCaster1.Initialize( castShape, planeShape );
							btConvexCast castPtr = convexCaster1;

							if( castPtr.calcTimeOfImpact(ref  convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
							{
								//add hit
								if( castResult.m_normal.length2() > (double)( 0.0001 ) )
								{
									if( castResult.m_fraction < resultCallback.m_closestHitFraction )
									{
										castResult.m_normal.normalize();
										LocalConvexResult localConvexResult = new LocalConvexResult
											(
											collisionObject,
											null,
											ref castResult.m_normal,
											ref castResult.m_hitPoint,
											castResult.m_fraction
											);

										bool normalInWorldSpace = true;
										resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
									}
								}
							}

						}
						else
						{
							//CProfileSample sample = new CProfileSample("convexSweepConcave");
							btConcaveShape concaveShape = (btConcaveShape)collisionShape;
							btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
							btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
							btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
							// rotation of box in local mesh space = MeshRotation^-1  ConvexToRotation
							btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
							rotationXform.m_origin = btVector3.Zero;


							BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
							tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject
								, concaveShape, ref colObjTransform );
							tccb.m_hitFraction = resultCallback.m_closestHitFraction;
							tccb.m_allowedPenetration = allowedPenetration;
							btVector3 boxMinLocal, boxMaxLocal;
							castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );

							btVector3 rayAabbMinLocal = convexFromLocal;
							rayAabbMinLocal.setMin( ref convexToLocal );
							btVector3 rayAabbMaxLocal = convexFromLocal;
							rayAabbMaxLocal.setMax( ref convexToLocal );
							rayAabbMinLocal += boxMinLocal;
							rayAabbMaxLocal += boxMaxLocal;
							concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
							BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
						}
					}
				}
				else
				{
					///@todo : use AABB tree or other BVH acceleration structure!
					if( collisionShape.isCompound() )
					{
						CProfileSample sample = new CProfileSample( "convexSweepCompound" );
						btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
						int i = 0;
						for( i = 0; i < compoundShape.getNumChildShapes(); i++ )
						{
							//btTransform childTrans = compoundShape.getChildTransform( i );
							btCollisionShape childCollisionShape = compoundShape.getChildShape( i );
							btTransform childWorldTrans; colObjTransform.Apply( ref  compoundShape.m_children.InternalArray[i].m_transform
										, out childWorldTrans );


							LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback );

							//btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get();
							//tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i );

							objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans
								, childCollisionShape
								, collisionObject
								, ref childWorldTrans
								, my_cb, allowedPenetration );
							//BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj );
						}
					}
				}
			}
		}
Exemplo n.º 2
0
		public static void rayTestSingleInternal( ref btTransform rayFromTrans, ref btTransform rayToTrans,
												btCollisionShape collisionShape,
												btCollisionObject collisionObject,
												ref btTransform useTransform,
												RayResultCallback resultCallback )
		{
			btSphereShape pointShape = BulletGlobals.SphereShapePool.Get();
			pointShape.Initialize( btScalar.BT_ZERO );
			pointShape.setMargin( 0f );
			btConvexShape castShape = pointShape;
			//btCollisionShape collisionShape = collisionObjectWrap.getCollisionShape();
			btTransform colObjWorldTransform = useTransform;// collisionObjectWrap.m_worldTransform;

			if( collisionShape.isConvex() )
			{
				//		CProfileSample sample = new CProfileSample("rayTestConvex");
				btConvexCast.CastResult castResult = new btConvexCast.CastResult();

				castResult.m_fraction = resultCallback.m_closestHitFraction;

				btConvexShape convexShape = (btConvexShape)collisionShape;
				btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
				//new btVoronoiSimplexSolver();
				btSubsimplexConvexCast subSimplexConvexCaster = BulletGlobals.SubSimplexConvexCastPool.Get();
				subSimplexConvexCaster.Initialize( castShape, convexShape, simplexSolver );

				btGjkConvexCast gjkConvexCaster = BulletGlobals.GjkConvexCastPool.Get();
				gjkConvexCaster.Initialize( castShape, convexShape, simplexSolver );
				BulletGlobals.SubSimplexConvexCastPool.Free( subSimplexConvexCaster );
				BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );

				//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);

				btConvexCast convexCasterPtr = null;
				//use kF_UseSubSimplexConvexCastRaytest by default
				if( ( resultCallback.m_flags & (uint)btTriangleRaycastCallback.EFlags.kF_UseGjkConvexCastRaytest ) != 0 )
					convexCasterPtr = gjkConvexCaster;
				else
					convexCasterPtr = subSimplexConvexCaster;

				btConvexCast convexCaster = convexCasterPtr;

				if( convexCaster.calcTimeOfImpact( ref rayFromTrans, ref rayToTrans
					, ref useTransform/*collisionObjectWrap.m_worldTransform*/
					, ref useTransform/*collisionObjectWrap.m_worldTransform*/
					, castResult ) )
				{
					//add hit
					if( castResult.m_normal.length2() > (double)( 0.0001 ) )
					{
						if( castResult.m_fraction < resultCallback.m_closestHitFraction )
						{
							//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
#if USE_SUBSIMPLEX_CONVEX_CAST
							//rotate normal into worldspace
							castResult.m_normal = rayFromTrans.getBasis()  castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST

							castResult.m_normal.normalize();
							LocalRayResult localRayResult = new LocalRayResult
								(
								collisionObject,
								null,
								castResult.m_normal,
								castResult.m_fraction
								);

							bool normalInWorldSpace = true;
							resultCallback.addSingleResult( localRayResult, normalInWorldSpace );

						}
					}
				}
				BulletGlobals.GjkConvexCastPool.Free( gjkConvexCaster );
			}
			else
			{
				if( collisionShape.isConcave() )
				{
					btTransform worldTocollisionObject; colObjWorldTransform.inverse( out worldTocollisionObject );
					btVector3 tmp;
					rayFromTrans.getOrigin( out tmp );
					btVector3 rayFromLocal; worldTocollisionObject.Apply( ref tmp, out rayFromLocal );
					rayToTrans.getOrigin( out tmp );
					btVector3 rayToLocal; worldTocollisionObject.Apply( ref tmp, out rayToLocal );

					//			CProfileSample sample = new CProfileSample("rayTestConcave");
#if SUPPORT_TRIANGLE_MESH
					if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
					{
						///optimized version for btBvhTriangleMeshShape
						btBvhTriangleMeshShape triangleMesh = (btBvhTriangleMeshShape)collisionShape;

						BridgeTriangleRaycastCallback rcb( rayFromLocal, rayToLocal,&resultCallback, collisionObjectWrap.getCollisionObject(), triangleMesh, colObjWorldTransform);
						rcb.m_hitFraction = resultCallback.m_closestHitFraction;
						triangleMesh.performRaycast( &rcb, rayFromLocal, rayToLocal );
					}
					else
#endif
					{
						//generic (slower) case
						btConcaveShape concaveShape = (btConcaveShape)collisionShape;

						//ConvexCast::CastResult


						BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback( ref rayFromLocal, ref rayToLocal, resultCallback
							, collisionObject, concaveShape, ref colObjWorldTransform );
						rcb.m_hitFraction = resultCallback.m_closestHitFraction;

						btVector3 rayAabbMinLocal = rayFromLocal;
						rayAabbMinLocal.setMin( ref rayToLocal );
						btVector3 rayAabbMaxLocal = rayFromLocal;
						rayAabbMaxLocal.setMax( ref rayToLocal );

						concaveShape.processAllTriangles( rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
					}
				}
				else
				{
					//			CProfileSample sample = new CProfileSample("rayTestCompound");
					if( collisionShape.isCompound() )
					{

						btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
						btDbvt dbvt = compoundShape.getDynamicAabbTree();


						RayTester rayCB = new RayTester(
									collisionObject,
									compoundShape,
									ref colObjWorldTransform,
									ref rayFromTrans,
									ref rayToTrans,
									resultCallback );
#if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
						if( dbvt != null )
						{
							btTransform tmp;
							colObjWorldTransform.inverseTimes( ref rayFromTrans, out tmp );
							btVector3 localRayFrom; tmp.getOrigin( out localRayFrom );
							colObjWorldTransform.inverseTimes( ref rayToTrans, out tmp );
							btVector3 localRayTo; tmp.getOrigin( out localRayTo );
							btDbvt.rayTest( dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB );
						}
						else
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
						{
							for( int i = 0, n = compoundShape.getNumChildShapes(); i < n; ++i )
							{
								rayCB.Process( i );
							}
						}
					}
				}
			}
			BulletGlobals.SphereShapePool.Free( pointShape );
		}