/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. internal void convexSweepTest( btConvexShape castShape, ref btTransform convexFromWorld, ref btTransform convexToWorld, ConvexResultCallback resultCallback, double allowedCcdPenetration = btScalar.BT_ZERO ) { CProfileSample sample = new CProfileSample( "convexSweepTest" ); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; btTransformUtil.calculateVelocity( ref convexFromWorld, ref convexToWorld, 1.0f, out linVel, out angVel ); btVector3 zeroLinVel = btVector3.Zero; btTransform R = btTransform.Identity; R.m_basis = convexFromWorld.m_basis; castShape.calculateTemporalAabb( ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax ); } #if !USE_BRUTEFORCE_RAYBROADPHASE btSingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get(); convexCB.Initialize( castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration ); m_broadphasePairCache.rayTest( ref convexFromTrans.m_origin, ref convexToTrans.m_origin, convexCB, ref castShapeAabbMin, ref castShapeAabbMax ); BulletGlobals.SingleSweepCallbackPool.Free( convexCB ); #else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; for( i = 0; i < m_collisionObjects.Count; i++ ) { btCollisionObject collisionObject = m_collisionObjects[i]; //only perform raycast if filterMask matches if( resultCallback.needsCollision( collisionObject.getBroadphaseHandle() ) ) { //RigidcollisionObject collisionObject = ctrl.GetRigidcollisionObject(); btVector3 collisionObjectAabbMin, collisionObjectAabbMax; collisionObject.getCollisionShape().getAabb( collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax ); AabbExpand( collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax ); double hitLambda = (double)( 1.0 ); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; if( btRayAabb( convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal ) ) { objectQuerySingle( castShape, convexFromTrans, convexToTrans, collisionObject, collisionObject.getCollisionShape(), collisionObject.getWorldTransform(), resultCallback, allowedCcdPenetration ); } } } #endif //USE_BRUTEFORCE_RAYBROADPHASE }
static public double capsuleCapsuleDistance( out btVector3 normalOnB, out btVector3 pointOnB, double capsuleLengthA, double capsuleRadiusA, double capsuleLengthB, double capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, ref btTransform transformA, ref btTransform transformB, double distanceThreshold ) { btVector3 directionA; transformA.m_basis.getColumn( capsuleAxisA, out directionA ); btVector3 translationA; transformA.getOrigin( out translationA ); btVector3 directionB; transformB.m_basis.getColumn( capsuleAxisB, out directionB ); btVector3 translationB; transformB.getOrigin( out translationB ); // translation between centers btVector3 translation; translationB.Sub( ref translationA, out translation ); // compute the closest points of the capsule line segments btVector3 ptsVector; // the vector between the closest points btVector3 offsetA, offsetB; // offsets from segment centers to their closest points double tA, tB; // parameters on line segment segmentsClosestPoints( out ptsVector, out offsetA, out offsetB, out tA, out tB, ref translation, ref directionA, capsuleLengthA, ref directionB, capsuleLengthB ); double distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; if( distance > distanceThreshold ) { pointOnB = btVector3.Zero; normalOnB = btVector3.xAxis; return distance; } double lenSqr = ptsVector.length2(); if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' btVector3 q; btVector3.btPlaneSpace1( ref directionA, out normalOnB, out q ); } else { // compute the contact normal ptsVector.Mult( -btScalar.btRecipSqrt( lenSqr ), out normalOnB ); } btVector3 tmp; btVector3 tmp2; translationB.Add( ref offsetB, out tmp ); normalOnB.Mult( capsuleRadiusB, out tmp2 ); tmp.Add( ref tmp2, out pointOnB ); //pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB; return distance; }
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 ); }