/* internal void Initialize( btCollisionAlgorithmConstructionInfo ci) { base.Initialize( ci ); } */ internal override void processCollision( btCollisionObjectWrapper a , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo c, btManifoldResult d) { }
internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //(void)dispatchInfo; if( m_manifoldPtr == null ) return; resultOut.setPersistentManifold( m_manifoldPtr ); btSphereShape sphere0 = (btSphereShape)body0Wrap.getCollisionShape(); btSphereShape sphere1 = (btSphereShape)body1Wrap.getCollisionShape(); btVector3 diff; body0Wrap.m_collisionObject.m_worldTransform.m_origin.Sub( ref body1Wrap.m_collisionObject.m_worldTransform.m_origin, out diff ); double len = diff.length(); double radius0 = sphere0.getRadius(); double radius1 = sphere1.getRadius(); #if CLEAR_MANIFOLD m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting #endif ///iff distance positive, don't generate a new contact if( len > ( radius0 + radius1 ) ) { #if !CLEAR_MANIFOLD resultOut.refreshContactPoints(); #endif //CLEAR_MANIFOLD return; } ///distance (negative means penetration) double dist = len - ( radius0 + radius1 ); btVector3 normalOnSurfaceB = btVector3.xAxis; if( len > btScalar.SIMD_EPSILON ) { normalOnSurfaceB = diff / len; } ///point on A (worldspace) ///btVector3 pos0 = col0.getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) btVector3 pos1; body1Wrap.m_collisionObject.m_worldTransform.m_origin.AddScale( ref normalOnSurfaceB, radius1, out pos1 ); /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.addContactPoint( ref normalOnSurfaceB, ref pos1, dist ); #if !CLEAR_MANIFOLD resultOut.refreshContactPoints(); #endif //CLEAR_MANIFOLD }
internal override void processCollision( btCollisionObjectWrapper body0Wrap, ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap, ref btTransform body1Transform, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { if( m_manifoldPtr == null ) return; btBoxShape box0 = (btBoxShape)body0Wrap.m_shape; btBoxShape box1 = (btBoxShape)body1Wrap.m_shape; /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.setPersistentManifold( m_manifoldPtr ); #if !USE_PERSISTENT_CONTACTS m_manifoldPtr.clearManifold(); #endif //USE_PERSISTENT_CONTACTS btDiscreteCollisionDetectorInterface.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput(); input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT; input.m_transformA = body0Transform; input.m_transformB = body1Transform; //btBoxBoxDetector detector = BulletGlobals. // new btBoxBoxDetectors( box0, box1 ); btBoxBoxDetector.getClosestPoints( box0, box1, input, resultOut, dispatchInfo.m_debugDraw ); #if USE_PERSISTENT_CONTACTS // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if( m_ownManifold ) { resultOut.refreshContactPoints(); } #endif //USE_PERSISTENT_CONTACTS }
// // Convex-Convex collision algorithm // internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { if( m_manifoldPtr == null ) { //swapped? m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject ); m_ownManifold = true; } resultOut.setPersistentManifold( m_manifoldPtr ); //comment-out next line to test multi-contact generation //resultOut.getPersistentManifold().clearManifold(); btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape(); btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape(); btVector3 normalOnB; btVector3 pointOnBWorld; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) && ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) ) { btCapsuleShape capsuleA = (btCapsuleShape)min0; btCapsuleShape capsuleB = (btCapsuleShape)min1; // btVector3 localScalingA = capsuleA.getLocalScaling(); // btVector3 localScalingB = capsuleB.getLocalScaling(); double threshold = m_manifoldPtr.getContactBreakingThreshold(); double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld , capsuleA.getHalfHeight(), capsuleA.getRadius() , capsuleB.getHalfHeight(), capsuleB.getRadius() , capsuleA.getUpAxis(), capsuleB.getUpAxis() , ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold ); if( dist < threshold ) { Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ); resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist ); } resultOut.refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0) #endif //USE_SEPDISTANCE_UTIL2 { btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get(); input.Initialize(); btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get(); gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver ); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA( min0 ); gjkPairDetector.setMinkowskiB( min1 ); #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { //if (dispatchInfo.m_convexMaxDistanceUseCPT) //{ // input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold(); //} else //{ input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold(); // } input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0Transform; input.m_transformB = body1Transform; #if USE_SEPDISTANCE_UTIL2 double sepDist = 0; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 if( min0.isPolyhedral() && min1.isPolyhedral() ) { btDummyResult dummy = new btDummyResult(); ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin(); double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin(); btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin ); btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0; btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1; if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null ) { double threshold = m_manifoldPtr.getContactBreakingThreshold(); double minDist = -1e30f; btVector3 sepNormalWorldSpace; bool foundSepAxis = true; if( dispatchInfo.m_enableSatConvex ) { foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis( polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, out sepNormalWorldSpace, resultOut ); } else { #if ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw ); //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN //double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); //if (l2>SIMD_EPSILON) { sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin(); #if ZERO_MARGIN foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0; #else foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin()); #endif } } if( foundSepAxis ) { // Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z); btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform , minDist - threshold, threshold, resultOut ); } if( m_ownManifold ) { resultOut.refreshContactPoints(); } BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); return; } else { //we can also deal with convex versus triangle (without connectivity data) if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE ) { btVertexArray vertices = new btVertexArray(); btTriangleShape tri = (btTriangleShape)polyhedronB; btVector3 tmp; body1Transform.Apply( ref tri.m_vertices1, out tmp ); vertices.Add( ref tmp ); body1Transform.Apply( ref tri.m_vertices2, out tmp ); vertices.Add( ref tmp ); body1Transform.Apply( ref tri.m_vertices3, out tmp ); vertices.Add( ref tmp ); //tri.initializePolyhedralFeatures(); double threshold = m_manifoldPtr.getContactBreakingThreshold(); btVector3 sepNormalWorldSpace; double minDist = -btScalar.BT_LARGE_FLOAT; double maxDist = threshold; bool foundSepAxis = false; if( false ) { polyhedronB.initializePolyhedralFeatures(); foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis( polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, out sepNormalWorldSpace, resultOut ); // Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z); btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace , polyhedronA.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut ); } else { #if ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw ); #endif//ZERO_MARGIN double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if( l2 > btScalar.SIMD_EPSILON ) { gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace ); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin(); //foundSepAxis = true; btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace , polyhedronA.getConvexPolyhedron(), ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut ); } else { //sepNormalWorldSpace = btVector3.Zero; } } if( m_ownManifold ) { resultOut.refreshContactPoints(); } BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); return; } } } gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw ); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if( m_numPerturbationIterations != 0 && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold ) { int i; btVector3 v0, v1; btVector3 sepNormalWorldSpace; double l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if( l2 > btScalar.SIMD_EPSILON ) { gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace ); btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 ); bool perturbeA = true; double angleLimit = 0.125f * btScalar.SIMD_PI; double perturbeAngle; double radiusA = min0.getAngularMotionDisc(); double radiusB = min1.getAngularMotionDisc(); if( radiusA < radiusB ) { perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA; perturbeA = true; } else { perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB; perturbeA = false; } if( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if( perturbeA ) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for( i = 0; i < m_numPerturbationIterations; i++ ) { if( v0.length2() > btScalar.SIMD_EPSILON ) { btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle ); double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) ); btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle ); if( perturbeA ) { btQuaternion tmpq; btQuaternion tmpq2; rotq.inverse( out tmpq ); btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 ); btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq ); btMatrix3x3 m = new btMatrix3x3( ref tmpq ); btMatrix3x3 m2; body0Transform.getBasis( out m2 ); btMatrix3x3 m3; btMatrix3x3.Mult( ref m, ref m2, out m3 ); input.m_transformA.setBasis( ref m3 ); input.m_transformB = body1Transform; #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0); #endif //DEBUG_CONTACTS } else { btQuaternion tmpq; btQuaternion tmpq2; rotq.inverse( out tmpq ); btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 ); btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq ); btMatrix3x3 m = new btMatrix3x3( ref tmpq ); btMatrix3x3 m2; body1Transform.getBasis( out m2 ); btMatrix3x3 m3; btMatrix3x3.Mult( ref m, ref m2, out m3 ); input.m_transformA = body0Transform; input.m_transformB.setBasis( ref m3 ); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0); #endif } btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get(); if( perturbeA ) perturbedResultOut.Initialize( resultOut , ref input.m_transformA, ref input.m_transformB , ref input.m_transformA , perturbeA , dispatchInfo.m_debugDraw ); else perturbedResultOut.Initialize( resultOut , ref input.m_transformA, ref input.m_transformB , ref input.m_transformB , perturbeA , dispatchInfo.m_debugDraw ); gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw ); BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut ); } } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector ); } if( m_ownManifold ) { resultOut.refreshContactPoints(); } }
internal void Initialize( btManifoldResult originalResult , ref btTransform transformA, ref btTransform transformB , ref btTransform unPerturbedTransform, bool perturbA, btIDebugDraw debugDrawer ) { m_originalManifoldResult = ( originalResult ); m_transformA = ( transformA ); m_transformB = ( transformB ); m_unPerturbedTransform = ( unPerturbedTransform ); m_perturbA = ( perturbA ); m_debugDrawer = ( debugDrawer ); }
internal override double calculateTimeOfImpact( btCollisionObject col0, btCollisionObject col1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //not yet return btScalar.BT_ONE; }
internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; btCollisionObjectWrapper otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; Debug.Assert( colObjWrap.getCollisionShape().isCompound() ); btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() ); ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid if( compoundShape.getUpdateRevision() != m_compoundShapeRevision ) { ///clear and update all removeChildAlgorithms(); preallocateChildAlgorithms( body0Wrap, body1Wrap ); m_compoundShapeRevision = compoundShape.getUpdateRevision(); } if( m_childCollisionAlgorithms.Count == 0 ) return; btDbvt tree = compoundShape.getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get(); callback.Initialize( colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, m_childCollisionAlgorithms.InternalArray, m_sharedManifold); ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; btManifoldArray manifoldArray = new btManifoldArray(); for( i = 0; i < m_childCollisionAlgorithms.Count; i++ ) { if( m_childCollisionAlgorithms[i] != null ) { m_childCollisionAlgorithms[i].getAllContactManifolds( manifoldArray ); for( int m = 0; m < manifoldArray.Count; m++ ) { if( manifoldArray[m].m_cachedPoints > 0 ) { resultOut.setPersistentManifold( manifoldArray[m] ); resultOut.refreshContactPoints(); resultOut.setPersistentManifold( null );//??necessary? } } manifoldArray.Count = ( 0 ); } } } if( tree != null ) { btVector3 localAabbMin, localAabbMax; btTransform otherInCompoundSpace; if( m_isSwapped ) body1Transform.inverseTimes(ref body0Transform, out otherInCompoundSpace); else body0Transform.inverseTimes( ref body1Transform, out otherInCompoundSpace ); otherObjWrap.getCollisionShape().getAabb( ref otherInCompoundSpace, out localAabbMin, out localAabbMax ); btDbvt.btDbvtVolume bounds = btDbvt.btDbvtVolume.FromMM( ref localAabbMin, ref localAabbMax ); //process all children, that overlap with the given AABB bounds btDbvt.CollideTV( tree.m_root, ref bounds, callback ); } else { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.Count; int i; for( i = 0; i < numChildren; i++ ) { callback.ProcessChildShape( compoundShape.getChildShape( i ), i ); } } { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.Count; int i; //btManifoldArray manifoldArray; btCollisionShape childShape = null; //btITransform orgTrans; btTransform newChildWorldTrans; btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; for( i = 0; i < numChildren; i++ ) { if( m_childCollisionAlgorithms[i] != null ) { childShape = compoundShape.getChildShape( i ); //if not longer overlapping, remove the algorithm //orgTrans = colObjWrap.m_worldTransform; //btTransform childTrans = compoundShape.getChildTransform( i ); ( ( m_isSwapped ) ? body1Transform : body0Transform ).Apply( ref compoundShape.m_children.InternalArray[i].m_transform, out newChildWorldTrans ); //perform an AABB check first childShape.getAabb( ref newChildWorldTrans, out aabbMin0, out aabbMax0 ); if( m_isSwapped ) otherObjWrap.m_shape.getAabb( ref body0Transform, out aabbMin1, out aabbMax1 ); else otherObjWrap.m_shape.getAabb( ref body1Transform, out aabbMin1, out aabbMax1 ); if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) ) { //m_childCollisionAlgorithms[i].~btCollisionAlgorithm(); m_dispatcher.freeCollisionAlgorithm( m_childCollisionAlgorithms[i] ); m_childCollisionAlgorithms[i] = null; } } } } }
internal override double calculateTimeOfImpact( btCollisionObject a, btCollisionObject b, btDispatcherInfo c , btManifoldResult d ) { return btScalar.BT_ONE; }
internal override void processCollision( btCollisionObjectWrapper col0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper col1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { if( m_manifoldPtr == null ) return; btCollisionObjectWrapper sphereObjWrap = m_swapped ? col1Wrap : col0Wrap; btCollisionObjectWrapper triObjWrap = m_swapped ? col0Wrap : col1Wrap; btSphereShape sphere = (btSphereShape)sphereObjWrap.getCollisionShape(); btTriangleShape triangle = (btTriangleShape)triObjWrap.getCollisionShape(); /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.setPersistentManifold( m_manifoldPtr ); SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get(); detector.Initialize( sphere, triangle, m_manifoldPtr.getContactBreakingThreshold()); btDiscreteCollisionDetectorInterface.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get(); input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;///@todo: tighter bounds if( m_swapped ) { input.m_transformA = body1Transform; input.m_transformB = body0Transform; } else { input.m_transformA = body0Transform; input.m_transformB = body1Transform; } bool swapResults = m_swapped; detector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw, swapResults ); BulletGlobals.ClosestPointInputPool.Free( input ); BulletGlobals.SphereTriangleDetectorPool.Free( detector ); if( m_ownManifold ) resultOut.refreshContactPoints(); }
internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //(void)dispatchInfo; if( m_manifoldPtr == null ) return; btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap; btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap; btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape; btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape(); //btVector3 planeNormal; //planeShape.getPlaneNormal().Copy( out planeNormal ); double planeConstant = planeShape.getPlaneConstant(); if( m_swapped ) collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap , ref body1Transform , planeObjWrap , ref body0Transform , dispatchInfo, resultOut , ref planeShape.m_planeNormal, planeConstant ); else collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap , ref body0Transform , planeObjWrap , ref body1Transform , dispatchInfo, resultOut , ref planeShape.m_planeNormal, planeConstant ); /* btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap; btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap; btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape; btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.m_shape; bool hasCollision = false; btVector3 planeNormal; planeShape.m_planeNormal.Copy( out planeNormal ); double planeConstant = planeShape.getPlaneConstant(); btTransform planeInConvex; convexObjWrap.getWorldTransform().inverseTimes( planeObjWrap.getWorldTransform(), out planeInConvex ); btTransform convexInPlaneTrans; planeObjWrap.getWorldTransform().inverseTimes( convexObjWrap.getWorldTransform(), out convexInPlaneTrans ); btVector3 invPlaneNormal; planeNormal.Invert( out invPlaneNormal ); btVector3 tmp; planeInConvex.getBasis().Apply( planeNormal, out tmp ); btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx ); btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane ); double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant ); btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( planeNormal, -distance, out vtxInPlaneProjected ); btVector3 vtxInPlaneWorld; planeObjWrap.getWorldTransform().Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld ); hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold(); resultOut.setPersistentManifold( m_manifoldPtr ); if( hasCollision ) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap.getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; resultOut.addContactPoint( ref normalOnSurfaceB, ref pOnB, distance ); } */ //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: //they keep on rolling forever because of the additional off-center contact points //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) if( convexShape.isPolyhedral() && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold ) { btVector3 v0, v1; btVector3.btPlaneSpace1( ref planeShape.m_planeNormal, out v0, out v1 ); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects double angleLimit = 0.125f * btScalar.SIMD_PI; double perturbeAngle; double radius = convexShape.getAngularMotionDisc(); perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radius; if( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle ); double interval = btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ); for( int i = 0; i < m_numPerturbationIterations; i++ ) { double iterationAngle = i * interval; btQuaternion rotq = new btQuaternion( ref planeShape.m_planeNormal, iterationAngle ); btQuaternion rotqInv; rotq.inverse( out rotqInv ); btQuaternion tmpq, tmpq2; rotqInv.Mult( ref perturbeRot, out tmpq ); tmpq.Mult( ref rotq, out tmpq2 ); if( m_swapped ) collideSingleContact( true, ref tmpq2 , convexObjWrap, ref body1Transform , planeObjWrap, ref body0Transform , dispatchInfo , resultOut, ref planeShape.m_planeNormal, planeConstant ); else collideSingleContact( true, ref tmpq2 , convexObjWrap, ref body0Transform , planeObjWrap, ref body1Transform, dispatchInfo , resultOut, ref planeShape.m_planeNormal, planeConstant ); } } if( m_ownManifold ) { if( m_manifoldPtr.m_cachedPoints != 0 ) { resultOut.refreshContactPoints(); } } }
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot , btCollisionObjectWrapper convexObjWrap , ref btTransform convexTransform , btCollisionObjectWrapper planeObjWrap , ref btTransform planeTransform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut , ref btVector3 planeNormal, double planeConstant ) { //btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap; //btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap; btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape(); btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape(); bool hasCollision = false; //planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal ); //double planeConstant = planeShape.getPlaneConstant(); btTransform convexWorldTransform = convexTransform; //btTransform planeWorldTransform = planeObjWrap.m_worldTransform; btTransform convexInPlaneTrans; planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); if( usePertube ) { //now perturbe the convex-world transform btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot ); btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe ); convexWorldTransform.m_basis = tmpPerturbe; //convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot ); } btTransform planeInConvex; convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex ); btVector3 tmp, tmp2; planeNormal.Invert( out tmp ); planeInConvex.m_basis.Apply( ref tmp, out tmp2 ); btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx ); btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane ); double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant ); btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected ); btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld ); hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold(); resultOut.setPersistentManifold( m_manifoldPtr ); if( hasCollision ) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB ); btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) ); resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance ); } }
internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //(void)resultOut; //(void)dispatchInfo; btCollisionObject convexbody = m_isSwapped ? body1 : body0; btCollisionObject triBody = m_isSwapped ? body0 : body1; //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //only perform CCD above a certain threshold, this prevents blocking on the long run //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... double squareMot0 = btVector3.btDelLength2( ref convexbody.m_interpolationWorldTransform.m_origin, ref convexbody.m_worldTransform.m_origin ); if( squareMot0 < convexbody.getCcdSquareMotionThreshold() ) { return btScalar.BT_ONE; } //ref btVector3 from = convexbody.m_worldTransform.getOrigin(); //btVector3 to = convexbody.m_interpolationWorldTransform.getOrigin(); //todo: only do if the motion exceeds the 'radius' btTransform triInv; triBody.m_worldTransform.inverse( out triInv ); btTransform convexFromLocal; triInv.Apply( ref convexbody.m_worldTransform, out convexFromLocal ); btTransform convexToLocal; triInv.Apply( ref convexbody.m_interpolationWorldTransform, out convexToLocal ); if( triBody.getCollisionShape().isConcave() ) { btVector3 rayAabbMin = convexFromLocal.m_origin; rayAabbMin.setMin( ref convexToLocal.m_origin ); btVector3 rayAabbMax = convexFromLocal.m_origin; rayAabbMax.setMax( ref convexToLocal.m_origin ); double ccdRadius0 = convexbody.getCcdSweptSphereRadius(); rayAabbMin.AddScale( ref btVector3.One, -ccdRadius0, out rayAabbMin ); rayAabbMax.AddScale( ref btVector3.One, ccdRadius0, out rayAabbMax ); double curHitFraction = btScalar.BT_ONE; //is this available? LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback ( ref convexFromLocal, ref convexToLocal, convexbody.getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody.getHitFraction(); btCollisionObject concavebody = triBody; btConcaveShape triangleMesh = (btConcaveShape)concavebody.getCollisionShape(); if( triangleMesh != null ) { triangleMesh.processAllTriangles( raycastCallback, ref rayAabbMin, ref rayAabbMax ); } if( raycastCallback.m_hitFraction < convexbody.getHitFraction() ) { convexbody.setHitFraction( raycastCallback.m_hitFraction ); return raycastCallback.m_hitFraction; } } return btScalar.BT_ONE; }
internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { btCollisionObjectWrapper convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; btCollisionObjectWrapper triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; if( triBodyWrap.getCollisionShape().isConcave() ) { btConcaveShape concaveShape = (btConcaveShape)triBodyWrap.getCollisionShape(); if( convexBodyWrap.getCollisionShape().isConvex() ) { double collisionMarginTriangle = concaveShape.getMargin(); resultOut.setPersistentManifold( m_btConvexTriangleCallback.m_manifoldPtr ); if( m_isSwapped ) m_btConvexTriangleCallback.setTimeStepAndCounters( collisionMarginTriangle, dispatchInfo , convexBodyWrap, ref body1Transform , triBodyWrap, ref body0Transform , resultOut ); else m_btConvexTriangleCallback.setTimeStepAndCounters( collisionMarginTriangle, dispatchInfo , convexBodyWrap, ref body0Transform , triBodyWrap, ref body1Transform , resultOut ); m_btConvexTriangleCallback.m_manifoldPtr.setBodies( convexBodyWrap.m_collisionObject, triBodyWrap.m_collisionObject ); concaveShape.processAllTriangles( m_btConvexTriangleCallback, ref m_btConvexTriangleCallback.m_aabbMin, ref m_btConvexTriangleCallback.m_aabbMax ); resultOut.refreshContactPoints(); m_btConvexTriangleCallback.clearWrapperData(); } } }
public void setTimeStepAndCounters( double collisionMarginTriangle , btDispatcherInfo dispatchInfo, btCollisionObjectWrapper convexBodyWrap , ref btTransform convexTransform , btCollisionObjectWrapper triBodyWrap , ref btTransform triBodyTransform , btManifoldResult resultOut ) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; m_dispatchInfoPtr = dispatchInfo; m_collisionMarginTriangle = collisionMarginTriangle; m_resultOut = resultOut; //recalc aabbs btTransform convexInTriangleSpace; btTransform invTrans; triBodyTransform.inverse( out invTrans ); invTrans.Apply( ref convexTransform, out convexInTriangleSpace ); btCollisionShape convexShape = m_convexBodyWrap.getCollisionShape(); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody.m_collisionShape); convexShape.getAabb( ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax ); double extraMargin = collisionMarginTriangle; btVector3 extra = new btVector3( extraMargin ); m_aabbMax += extra; m_aabbMin -= extra; }
internal override double calculateTimeOfImpact( btCollisionObject col0, btCollisionObject col1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //(void)resultOut; //(void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0.m_worldTransform, double resultFraction = btScalar.BT_ONE; btVector3 tmp; col0.m_interpolationWorldTransform.m_origin.Sub( ref col0.m_worldTransform.m_origin, out tmp ); double squareMot0 = ( tmp ).length2(); col1.m_interpolationWorldTransform.m_origin.Sub( ref col1.m_worldTransform.m_origin, out tmp ); double squareMot1 = ( tmp ).length2(); if( squareMot0 < col0.getCcdSquareMotionThreshold() && squareMot1 < col1.getCcdSquareMotionThreshold() ) return resultFraction; if( disableCcd ) return btScalar.BT_ONE; //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) /// Convex0 against sphere for Convex1 { btConvexShape convex0 = (btConvexShape)col0.getCollisionShape(); using( btSphereShape sphere1 = BulletGlobals.SphereShapePool.Get() ) { sphere1.Initialize( col1.getCcdSweptSphereRadius() ); //todo: allow non-zero sphere sizes, for better approximation btConvexCast.CastResult result = BulletGlobals.CastResultPool.Get(); btVoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get(); //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get(); ccd1.Initialize( convex0, sphere1, voronoiSimplex ); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if( ccd1.calcTimeOfImpact( ref col0.m_worldTransform, ref col0.m_interpolationWorldTransform, ref col1.m_worldTransform, ref col1.m_interpolationWorldTransform, result ) ) { //store result.m_fraction in both bodies if( col0.getHitFraction() > result.m_fraction ) col0.setHitFraction( result.m_fraction ); if( col1.getHitFraction() > result.m_fraction ) col1.setHitFraction( result.m_fraction ); if( resultFraction > result.m_fraction ) resultFraction = result.m_fraction; } BulletGlobals.GjkConvexCastPool.Free( ccd1 ); } } /// Sphere (for convex0) against Convex1 { btConvexShape convex1 = (btConvexShape)( col1.getCollisionShape() ); using( btSphereShape sphere0 = BulletGlobals.SphereShapePool.Get() ) { sphere0.Initialize( col0.getCcdSweptSphereRadius() ); //todo: allow non-zero sphere sizes, for better approximation btConvexCast.CastResult result = BulletGlobals.CastResultPool.Get(); btVoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get(); //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get(); ccd1.Initialize( sphere0, convex1, voronoiSimplex ); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if( ccd1.calcTimeOfImpact( ref col0.m_worldTransform, ref col0.m_interpolationWorldTransform, ref col1.m_worldTransform, ref col1.m_interpolationWorldTransform, result ) ) { //store result.m_fraction in both bodies if( col0.getHitFraction() > result.m_fraction ) col0.setHitFraction( result.m_fraction ); if( col1.getHitFraction() > result.m_fraction ) col1.setHitFraction( result.m_fraction ); if( resultFraction > result.m_fraction ) resultFraction = result.m_fraction; } BulletGlobals.GjkConvexCastPool.Free( ccd1 ); } } return resultFraction; }
internal btCompoundCompoundLeafCallback( btCollisionObjectWrapper compound1ObjWrap, btCollisionObjectWrapper compound0ObjWrap, btDispatcher dispatcher, btDispatcherInfo dispatchInfo, btManifoldResult resultOut, btHashedSimplePairCache childAlgorithmsCache, btPersistentManifold sharedManifold ) { m_numOverlapPairs = ( 0 ); m_compound0ColObjWrap = ( compound1ObjWrap ); m_compound1ColObjWrap = ( compound0ObjWrap ); m_dispatcher = ( dispatcher ); m_dispatchInfo = ( dispatchInfo ); m_resultOut = ( resultOut ); m_childCollisionAlgorithmCache = ( childAlgorithmsCache ); m_sharedManifold = ( sharedManifold ); }
internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { btCollisionObjectWrapper col0ObjWrap = body0Wrap; btCollisionObjectWrapper col1ObjWrap = body1Wrap; Debug.Assert( col0ObjWrap.getCollisionShape().isCompound() ); Debug.Assert( col1ObjWrap.getCollisionShape().isCompound() ); btCompoundShape compoundShape0 = (btCompoundShape)( col0ObjWrap.getCollisionShape() ); btCompoundShape compoundShape1 = (btCompoundShape)( col1ObjWrap.getCollisionShape() ); btDbvt tree0 = compoundShape0.getDynamicAabbTree(); btDbvt tree1 = compoundShape1.getDynamicAabbTree(); if( tree0 == null || tree1 == null ) { base.processCollision( body0Wrap, ref body0Transform, body1Wrap, ref body1Transform, dispatchInfo, resultOut ); return; } ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid if( ( compoundShape0.getUpdateRevision() != m_compoundShapeRevision0 ) || ( compoundShape1.getUpdateRevision() != m_compoundShapeRevision1 ) ) { ///clear all removeChildAlgorithms(); m_compoundShapeRevision0 = compoundShape0.getUpdateRevision(); m_compoundShapeRevision1 = compoundShape1.getUpdateRevision(); } ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; btManifoldArray manifoldArray = new btManifoldArray(); btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray(); for( i = 0; i < pairs.Count; i++ ) { if( pairs[i].m_userPointer != null ) { btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer; algo.getAllContactManifolds( manifoldArray ); for( int m = 0; m < manifoldArray.Count; m++ ) { if( manifoldArray[m].m_cachedPoints != 0 ) { resultOut.setPersistentManifold( manifoldArray[m] ); resultOut.refreshContactPoints(); resultOut.setPersistentManifold( null ); } } manifoldArray.Count =( 0 ); } } } btCompoundCompoundLeafCallback callback = new btCompoundCompoundLeafCallback ( col0ObjWrap, col1ObjWrap,this.m_dispatcher, dispatchInfo, resultOut, this.m_childCollisionAlgorithmCache, m_sharedManifold); btTransform xform; body0Transform.inverseTimes( ref body1Transform, out xform ); MycollideTT( tree0.m_root, tree1.m_root, ref xform, callback ); //Console.WriteLine("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); //remove non-overlapping child pairs { Debug.Assert( m_removePairs.Count == 0 ); //iterate over all children, perform an AABB check inside ProcessChildShape btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray(); int i; //btManifoldArray manifoldArray; btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; for( i = 0; i < pairs.Count; i++ ) { if( pairs[i].m_userPointer != null ) { btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer; { btCollisionShape childShape0 = null; btTransform newChildWorldTrans0; //btTransform orgInterpolationTrans0; childShape0 = compoundShape0.getChildShape( pairs[i].m_indexA ); //orgInterpolationTrans0 = col0ObjWrap.m_worldTransform; btTransform childTrans0 = compoundShape0.getChildTransform( pairs[i].m_indexA ); body0Transform.Apply( ref childTrans0, out newChildWorldTrans0 ); childShape0.getAabb( ref newChildWorldTrans0, out aabbMin0, out aabbMax0 ); } { btCollisionShape childShape1 = null; btTransform newChildWorldTrans1; childShape1 = compoundShape1.getChildShape( pairs[i].m_indexB ); btTransform childTrans1 = compoundShape1.getChildTransform( pairs[i].m_indexB ); body1Transform.Apply( ref childTrans1, out newChildWorldTrans1 ); childShape1.getAabb( ref newChildWorldTrans1, out aabbMin1, out aabbMax1 ); } if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) ) { //algo.~btCollisionAlgorithm(); m_dispatcher.freeCollisionAlgorithm( algo ); m_removePairs.Add( new btSimplePair( pairs[i].m_indexA, pairs[i].m_indexB ) ); } } } for( i = 0; i < m_removePairs.Count; i++ ) { m_childCollisionAlgorithmCache.removeOverlappingPair( m_removePairs[i].m_indexA, m_removePairs[i].m_indexB ); } m_removePairs.Clear(); } }
internal void Initialize( btCollisionObjectWrapper compoundObjWrap, btCollisionObjectWrapper otherObjWrap, btDispatcher dispatcher, btDispatcherInfo dispatchInfo, btManifoldResult resultOut, btCollisionAlgorithm[] childCollisionAlgorithms, btPersistentManifold sharedManifold ) { m_compoundColObjWrap = ( compoundObjWrap ); m_otherObjWrap = ( otherObjWrap ); m_dispatcher = ( dispatcher ); m_dispatchInfo = ( dispatchInfo ); m_resultOut = ( resultOut ); m_childCollisionAlgorithms = ( childCollisionAlgorithms ); m_sharedManifold = ( sharedManifold ); }
internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { Debug.Assert( false ); return 0; }
internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { Debug.Assert( false ); //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures btCollisionObject colObj = m_isSwapped ? body1 : body0; btCollisionObject otherObj = m_isSwapped ? body0 : body1; Debug.Assert( colObj.getCollisionShape().isCompound() ); btCompoundShape compoundShape = (btCompoundShape)( colObj.getCollisionShape() ); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 //then use each overlapping node AABB against Tree0 //and vise versa. double hitFraction = btScalar.BT_ONE; int numChildren = m_childCollisionAlgorithms.Count; int i; //btTransform orgTrans; double frac; for( i = 0; i < numChildren; i++ ) { //btCollisionShape childShape = compoundShape.getChildShape(i); //backup //orgTrans = colObj.m_worldTransform; btTransform childTrans = compoundShape.getChildTransform( i ); //btTransform newChildWorldTrans = orgTrans*childTrans ; //colObj.setWorldTransform( orgTrans * childTrans ); btTransform tmp; colObj.m_worldTransform.Apply( ref childTrans, out tmp ); colObj.m_worldTransform = tmp; //btCollisionShape tmpShape = colObj.getCollisionShape(); //colObj.internalSetTemporaryCollisionShape( childShape ); frac = m_childCollisionAlgorithms[i].calculateTimeOfImpact( colObj, otherObj, dispatchInfo, resultOut ); if( frac < hitFraction ) { hitFraction = frac; } //revert back //colObj.internalSetTemporaryCollisionShape( tmpShape); colObj.setWorldTransform( ref colObj.m_worldTransform ); } return hitFraction; }
internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut) { //not yet return 1; }