C# CollisionObject.GetCollisionShape方法代码示例

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.getPersistentManifold().clearManifold();


            ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;

            IndexedVector3 normalOnB = IndexedVector3.Zero;
            IndexedVector3 pointOnBWorld = IndexedVector3.Zero;

            {
                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
                {
                    gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
                    //TODO: if (dispatchInfo.m_useContinuous)
                    gjkPairDetector.SetMinkowskiA(min0);
                    gjkPairDetector.SetMinkowskiB(min1);

                    {
                        input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();

                    gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
                    if (BulletGlobals.g_streamWriter != null)
                    {
                        BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
                    }
#endif                    
                }
                //BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
                //btVector3 v0,v1;
                //btVector3 sepNormalWorldSpace;

            }

            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }

        }

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            resultOut.SetPersistentManifold(m_manifoldPtr);

            SphereShape sphere0 = body0.GetCollisionShape() as SphereShape;
            SphereShape sphere1 = body1.GetCollisionShape() as SphereShape;

            IndexedVector3 diff = body0.GetWorldTransform()._origin - body1.GetWorldTransform()._origin;
            float len = diff.Length();
            float radius0 = sphere0.GetRadius();
            float 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)
            float dist = len - (radius0 + radius1);

            IndexedVector3 normalOnSurfaceB = new IndexedVector3(1, 0, 0);
            if (len > MathUtil.SIMD_EPSILON)
            {
                normalOnSurfaceB = diff / len;
            }

            ///point on A (worldspace)
            ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
            ///point on B (worldspace)
            IndexedVector3 pos1 = body1.GetWorldTransform()._origin + radius1 * normalOnSurfaceB;

            /// 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


        }

        public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult 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
            ///body0.m_worldTransform,
            float resultFraction = 1.0f;

            float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
            float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();

            if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
                squareMot1 < body1.GetCcdSquareMotionThreshold())
            {
                return resultFraction;
            }
            if (disableCcd)
            {
                return 1f;
            }


            //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
            {
                ConvexShape convex0 = body0.GetCollisionShape() as ConvexShape;

                SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
                sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(convex0, sphere1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                        body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {

                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }
                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }
                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere1);
                    result.Cleanup();
                }
            }

            /// Sphere (for convex0) against Convex1
            {
                ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;

                SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
                sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(sphere0, convex1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                        body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {

                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }
                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }
                        if (resultFraction > result.m_fraction)
                        {
//.........这里部分代码省略.........

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            //resultOut = new ManifoldResult();
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.GetPersistentManifold().ClearManifold();


            ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
            IndexedVector3 normalOnB;
            IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
            if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
            {
                CapsuleShape capsuleA = min0 as CapsuleShape;
                CapsuleShape capsuleB = min1 as CapsuleShape;
                //IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
                //IndexedVector3 localScalingB = capsuleB.GetLocalScaling();

                float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
                    capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
                    body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);

                if (dist < threshold)
                {
                    Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.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.f)
#endif //USE_SEPDISTANCE_UTIL2

            {


                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector 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 = float.MaxValue;
	        } 
            else
#endif //USE_SEPDISTANCE_UTIL2
                    {
                        input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    //input.m_stackAlloc = dispatchInfo.m_stackAllocator;
                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();


                    if (min0.IsPolyhedral() && min1.IsPolyhedral())
                    {


                        DummyResult dummy = new DummyResult();


                        PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
                        PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
                        if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
                        {
                            float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                            float minDist = float.MinValue;
                            IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
                            bool foundSepAxis = true;

//.........这里部分代码省略.........

        public virtual void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
        {
            //check that the object isn't already added
            //btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());

            Debug.Assert(collisionObject != null);
            //Debug.Assert(!m_collisionObjects.Contains(collisionObject));

            if (m_collisionObjects.Contains(collisionObject))
            {
                return;
            }

            m_collisionObjects.Add(collisionObject);

            //calculate new AABB
            IndexedMatrix trans = collisionObject.GetWorldTransform();
            IndexedVector3 minAabb;
            IndexedVector3 maxAabb;

            collisionObject.GetCollisionShape().GetAabb(ref trans, out minAabb, out maxAabb);

            BroadphaseNativeTypes type = collisionObject.GetCollisionShape().GetShapeType();
            collisionObject.SetBroadphaseHandle(GetBroadphase().CreateProxy(
                ref minAabb,
                ref maxAabb,
                type,
                collisionObject,
                collisionFilterGroup,
                collisionFilterMask,
                m_dispatcher1, 0
                ));
        }

        public void UpdateSingleAabb(CollisionObject colObj)
        {
            IndexedVector3 minAabb;
            IndexedVector3 maxAabb;
            IndexedMatrix wt = colObj.GetWorldTransform();
            colObj.GetCollisionShape().GetAabb(ref wt, out minAabb, out maxAabb);
            //need to increase the aabb for contact thresholds
            IndexedVector3 contactThreshold = new IndexedVector3(BulletGlobals.gContactBreakingThreshold);
            minAabb -= contactThreshold;
            maxAabb += contactThreshold;

            if (GetDispatchInfo().m_useContinuous && colObj.GetInternalType() == CollisionObjectTypes.CO_RIGID_BODY && !colObj.IsStaticOrKinematicObject())
	        {
		        IndexedVector3 minAabb2,maxAabb2;
		        colObj.GetCollisionShape().GetAabb(colObj.GetInterpolationWorldTransform(),out minAabb2 ,out maxAabb2);
		        minAabb2 -= contactThreshold;
		        maxAabb2 += contactThreshold;
		        MathUtil.VectorMin(ref minAabb2,ref minAabb);
                MathUtil.VectorMax(ref maxAabb2, ref maxAabb);
            }

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugCollisionWorld)
            {
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb);
            }


            IBroadphaseInterface bp = m_broadphasePairCache as IBroadphaseInterface;

            //moving objects should be moderately sized, probably something wrong if not
            if (colObj.IsStaticObject() || ((maxAabb - minAabb).LengthSquared() < 1e12f))
            {
                bp.SetAabb(colObj.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
            }
            else
            {
                //something went wrong, investigate
                //this assert is unwanted in 3D modelers (danger of loosing work)
                colObj.SetActivationState(ActivationState.DISABLE_SIMULATION);

                //static bool reportMe = true;
                bool reportMe = true;
                if (reportMe && m_debugDrawer != null)
                {
                    reportMe = false;
                    m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation");
                    m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n");
                    m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n");
                    m_debugDrawer.ReportErrorWarning("Thanks.\n");
                }
            }
        }

//.........这里部分代码省略.........
					{
						if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
						{
                            CastResult castResult = BulletGlobals.CastResultPool.Get();
							castResult.m_allowedPenetration = allowedPenetration;
							castResult.m_fraction = resultCallback.m_closestHitFraction;
							StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
							ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);

							if (convexCaster1.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
							{
								//add hit
								if (castResult.m_normal.LengthSquared() > 0.0001f)
								{
									if (castResult.m_fraction < resultCallback.m_closestHitFraction)
									{
										castResult.m_normal.Normalize();
										LocalConvexResult localConvexResult = new LocalConvexResult
											(
											collisionObject,
                                            //null, // updated to allow different ctor on struct
											ref castResult.m_normal,
											ref castResult.m_hitPoint,
											castResult.m_fraction
											);

										bool normalInWorldSpace = true;
										resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
									}
								}
							}
                            castResult.Cleanup();
						}
						else
						{
							BulletGlobals.StartProfile("convexSweepConcave");
							ConcaveShape concaveShape = (ConcaveShape)collisionShape;
                            IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
                            IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
                            IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
                            // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                            IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));

                            using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
                            {
                                tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
                                tccb.m_allowedPenetration = allowedPenetration;
                                IndexedVector3 boxMinLocal;
                                IndexedVector3 boxMaxLocal;
                                castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);

							IndexedVector3 rayAabbMinLocal = convexFromLocal;
							MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
							//rayAabbMinLocal.setMin(convexToLocal);
							IndexedVector3 rayAabbMaxLocal = convexFromLocal;
							//rayAabbMaxLocal.setMax(convexToLocal);
							MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);

                                rayAabbMinLocal += boxMinLocal;
                                rayAabbMaxLocal += boxMaxLocal;
                                concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
                                BulletGlobals.StopProfile();
                            }
						}
					}
				}
				else
				{
					///@todo : use AABB tree or other BVH acceleration structure!
					if (collisionShape.IsCompound())
					{
						BulletGlobals.StartProfile("convexSweepCompound");
						CompoundShape compoundShape = (CompoundShape)collisionShape;
						for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
						{
							IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
							CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
							IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
							// replace collision shape so that callback can determine the triangle
							CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
							collisionObject.InternalSetTemporaryCollisionShape(childCollisionShape);

							LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
							my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;


							ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
								collisionObject,
								childCollisionShape,
								ref childWorldTrans,
								my_cb, allowedPenetration);
							// restore
							collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
						}
						BulletGlobals.StopProfile();
					}
				}
            }
        }

        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            ClearCache();

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ProcessCollision");
            }


            m_resultOut = resultOut;
            m_dispatchInfo = dispatchInfo;
            GImpactShapeInterface gimpactshape0;
            GImpactShapeInterface gimpactshape1;

            if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
            {
                gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;

                if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
                {
                    gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;

                    GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
                }
                else
                {
                    GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
                }

            }
            else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
            {
                gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;

                GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
            }

        }

        protected void ConvexVsConvexCollision(CollisionObject body0,
                          CollisionObject body1,
                          CollisionShape shape0,
                          CollisionShape shape1)
        {
            CollisionShape tmpShape0 = body0.GetCollisionShape();
            CollisionShape tmpShape1 = body1.GetCollisionShape();

            body0.InternalSetTemporaryCollisionShape(shape0);
            body1.InternalSetTemporaryCollisionShape(shape1);

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ConvexVsConvex");
            }


            m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
            m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);

            CheckConvexAlgorithm(body0, body1);
            m_convex_algorithm.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);

            body0.InternalSetTemporaryCollisionShape(tmpShape0);
            body1.InternalSetTemporaryCollisionShape(tmpShape1);

        }

        protected void ShapeVsShapeCollision(
                          CollisionObject body0,
                          CollisionObject body1,
                          CollisionShape shape0,
                          CollisionShape shape1)
        {
            CollisionShape tmpShape0 = body0.GetCollisionShape();
            CollisionShape tmpShape1 = body1.GetCollisionShape();

            body0.InternalSetTemporaryCollisionShape(shape0);
            body1.InternalSetTemporaryCollisionShape(shape1);

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
            {
                BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ShapeVsShape");
            }



            {
                CollisionAlgorithm algor = NewAlgorithm(body0, body1);
                // post :	checkManifold is called

                m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
                m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);

                algor.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);

                m_dispatcher.FreeCollisionAlgorithm(algor);
            }

            body0.InternalSetTemporaryCollisionShape(tmpShape0);
            body1.InternalSetTemporaryCollisionShape(tmpShape1);

        }

        public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
        {
            CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);
            ci.SetManifold(sharedManifold);
            int index1 = (int)body0.GetCollisionShape().GetShapeType();
            int index2 = (int)body1.GetCollisionShape().GetShapeType();

            return m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1);
        }

        public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
        {
            BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
            IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
            float boxMargin = boxShape.GetMargin();
            penetrationDepth = 1.0f;

            // convert the sphere position to the box's local space
            IndexedMatrix m44T = boxObj.GetWorldTransform();
            IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);

            // Determine the closest point to the sphere center in the box
            IndexedVector3 closestPoint = sphereRelPos;
            closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
            closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
            closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
            closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
            closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
            closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));

            float intersectionDist = fRadius + boxMargin;
            float contactDist = intersectionDist + maxContactDistance;
            normal = sphereRelPos - closestPoint;

            //if there is no penetration, we are done
            float dist2 = normal.LengthSquared();
            if (dist2 > contactDist * contactDist)
            {
                return false;
            }

            float distance;

            //special case if the sphere center is inside the box
            if (dist2 == 0.0f)
            {
                distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
            }
            else //compute the penetration details
            {
                distance = normal.Length();
                normal /= distance;
            }

            pointOnBox = closestPoint + normal * boxMargin;
            //	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
            penetrationDepth = distance - intersectionDist;

            // transform back in world space
            IndexedVector3 tmp = m44T * pointOnBox;
            pointOnBox = tmp;
            //	tmp = m44T(v3PointOnSphere);
            //	v3PointOnSphere = tmp;
            tmp = m44T._basis * normal;
            normal = tmp;

            return true;
        }