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 ); } } } } }
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 ); }