C# IDebugDraw类代码示例

        public GjkEpaPenetrationDepthSolver() { } // for pool

        public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
        {
            //float radialmargin = 0f;

            IndexedVector3 guessVector = (transA._origin - transB._origin);
            GjkEpaSolver2Results results = new GjkEpaSolver2Results();
            if (GjkEpaSolver2.Penetration(convexA, ref transA,
                                        convexB, ref transB,
                                        ref guessVector, ref results))
            {
                //	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
                //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
                wWitnessOnA = results.witnesses0;
                wWitnessOnB = results.witnesses1;
                v = results.normal;
                return true;
            }
            else
            {
                if (GjkEpaSolver2.Distance(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
                {
                    wWitnessOnA = results.witnesses0;
                    wWitnessOnB = results.witnesses1;
                    v = results.normal;
                    return false;
                }
            }
            return false;
        }

	    ///btActionInterface interface
        public virtual void DebugDraw(IDebugDraw debugDrawer)
        {
	        for (int v=0;v<GetNumWheels();v++)
	        {
		        IndexedVector3 wheelColor = new IndexedVector3(0,1,1);
		        if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
		        {
			        wheelColor = new IndexedVector3(0,0,1);
		        } else
		        {
			        wheelColor= new IndexedVector3(1,0,1);
		        }

		        IndexedVector3 wheelPosWS = GetWheelInfo(v).m_worldTransform._origin;

                IndexedMatrix temp = GetWheelInfo(v).m_worldTransform;
                IndexedVector3 axle = new IndexedVector3(
        GetWheelInfo(v).m_worldTransform._basis._el0[GetRightAxis()],
        GetWheelInfo(v).m_worldTransform._basis._el1[GetRightAxis()],
        GetWheelInfo(v).m_worldTransform._basis._el2[GetRightAxis()]);

		        //debug wheels (cylinders)
		        debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
		        debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);

	        }
        }

 public FrameRateCounter(Game game,Vector3 location,IDebugDraw debugDraw)
     : base(game)
 {
     content = new ContentManager(game.Services);
     m_location = location;
     m_debugDraw = debugDraw;
 }

		public bool CalcPenDepth(VoronoiSimplexSolver simplexSolver, ConvexShape convexA,
			ConvexShape convexB, Matrix transA, Matrix transB, out Vector3 v, out Vector3 pa,
			out Vector3 pb, IDebugDraw debugDraw)
		{
			return btConvexPenetrationDepthSolver_calcPenDepth(_native, simplexSolver._native,
				convexA._native, convexB._native, ref transA, ref transB, out v, out pa,
				out pb, DebugDraw.GetUnmanaged(debugDraw));
		}

		public PerturbedContactResult(ManifoldResult originalResult,ref Matrix transformA,ref Matrix transformB,ref Matrix unPerturbedTransform,bool perturbA,IDebugDraw debugDrawer)
		{
			m_originalManifoldResult = originalResult;
			m_transformA = transformA;
			m_transformB = transformB;
			m_perturbA = perturbA;
			m_unPerturbedTransform = unPerturbedTransform;
			m_debugDrawer = debugDrawer;
		}

		/// <summary>
		/// Draws the specified constraint.
		/// </summary>
		/// <param name="constraint">The constraint.</param>
		/// <param name="debugDraw">The debug draw.</param>
		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var p6DOF = (Generic6DofConstraint)constraint;
			Matrix tr = p6DOF.GetCalculatedTransformA();
			if (DrawFrames)
			{
				debugDraw.DrawTransform(ref tr, DrawSize);
			}
			tr = p6DOF.GetCalculatedTransformB();
			if (DrawFrames)
			{
				debugDraw.DrawTransform(ref tr, DrawSize);
			}
			Vector3 zero = Vector3.Zero;
			if (DrawLimits)
			{
				tr = p6DOF.GetCalculatedTransformA();
				Vector3 center = p6DOF.GetCalculatedTransformB().Translation;
				// up is axis 1 not 2 ?
				Vector3 up = MathUtil.MatrixColumn(ref tr, 1);
				Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
				float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
				float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
				float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
				float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
				debugDraw.DrawSpherePatch(ref center, ref up, ref axis, DrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
				axis = MathUtil.MatrixColumn(ref tr, 1);
				float ay = p6DOF.GetAngle(1);
				float az = p6DOF.GetAngle(2);
				var cy = (float)System.Math.Cos(ay);
				var sy = (float)System.Math.Sin(ay);
				var cz = (float)System.Math.Cos(az);
				var sz = (float)System.Math.Sin(az);
				var ref1 = new Vector3();
				ref1.X = cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z;
				ref1.Y = -sz * axis.X + cz * axis.Y;
				ref1.Z = cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z;
				tr = p6DOF.GetCalculatedTransformB();
				Vector3 normal = -MathUtil.MatrixColumn(ref tr, 0);
				float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
				float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
				if (minFi > maxFi)
				{
					debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI,
					                  ref zero, false);
				}
				else if (minFi < maxFi)
				{
					debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, minFi, maxFi, ref zero, false);
				}
				tr = p6DOF.GetCalculatedTransformA();
				Vector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
				Vector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
				debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
			}
		}

		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var pCT = (ConeTwistConstraint)constraint;
			Matrix tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			Vector3 zero = Vector3.Zero;

			if (DrawLimits)
			{
				//const float length = float(5);
				float length = DrawSize;
				int nSegments = 8 * 4;
				float fAngleInRadians = MathUtil.SIMD_2_PI * (nSegments - 1) / nSegments;
				Vector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
				pPrev = Vector3.Transform(pPrev, tr);
				for (int i = 0; i < nSegments; i++)
				{
					fAngleInRadians = MathUtil.SIMD_2_PI * i / nSegments;
					Vector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
					pCur = Vector3.Transform(pCur, tr);
					debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);

					if (i % (nSegments / 8) == 0)
					{
						Vector3 origin = tr.Translation;
						debugDraw.DrawLine(ref origin, ref pCur, ref zero);
					}

					pPrev = pCur;
				}
				float tws = pCT.GetTwistSpan();
				float twa = pCT.GetTwistAngle();
				bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
				if (useFrameB)
				{
					tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
				}
				else
				{
					tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
				}
				Vector3 pivot = tr.Translation;
				Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
				Vector3 axis1 = MathUtil.MatrixColumn(ref tr, 1);

				debugDraw.DrawArc(ref pivot, ref normal, ref axis1, DrawSize, DrawSize, -twa - tws, -twa + tws, ref zero, true);
			}
		}

		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var p2pC = (Point2PointConstraint)constraint;
			Matrix tr = Matrix.Identity;
			Vector3 pivot = p2pC.GetPivotInA();
			pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyA().GetCenterOfMassTransform());
			tr.Translation = pivot;
			debugDraw.DrawTransform(ref tr, DrawSize);
			// that ideally should draw the same frame	
			pivot = p2pC.GetPivotInB();
			pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyB().GetCenterOfMassTransform());
			tr.Translation = pivot;

			if (DrawFrames)
				debugDraw.DrawTransform(ref tr, DrawSize);
		}

		public InplaceSolverIslandCallback(
			ContactSolverInfo solverInfo,
			IConstraintSolver solver,
			ObjectArray<TypedConstraint> sortedConstraints,
			int numConstraints,
			IDebugDraw debugDrawer,
			IDispatcher dispatcher)
		{
			m_solverInfo = solverInfo;
			m_solver = solver;
			m_sortedConstraints = sortedConstraints;
			m_numConstraints = numConstraints;
			m_debugDrawer = debugDrawer;
			m_dispatcher = dispatcher;
			m_bodies = new ObjectArray<CollisionObject>();
			m_manifolds = new ObjectArray<PersistentManifold>();
			m_constraints = new ObjectArray<TypedConstraint>();
		}

		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var pSlider = (SliderConstraint)constraint;
			Matrix tr = pSlider.GetCalculatedTransformA();
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			tr = pSlider.GetCalculatedTransformB();
			if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
			Vector3 zero = Vector3.Zero;
			if (DrawLimits)
			{
				Matrix tr2 = pSlider.GetCalculatedTransformA();
				Vector3 li_min = Vector3.Transform(new Vector3(pSlider.GetLowerLinLimit(), 0f, 0f), tr2);
				Vector3 li_max = Vector3.Transform(new Vector3(pSlider.GetUpperLinLimit(), 0f, 0f), tr2);
				debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
				Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
				Vector3 axis = MathUtil.MatrixColumn(ref tr, 1);
				float a_min = pSlider.GetLowerAngLimit();
				float a_max = pSlider.GetUpperAngLimit();
				Vector3 center = pSlider.GetCalculatedTransformB().Translation;
				debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, a_min, a_max, ref zero, true);
			}
		}

        internal static IntPtr CreateWrapper(IDebugDraw value, bool weakReference)
        {
            DrawAabbUnmanagedDelegate a = new DrawAabbUnmanagedDelegate(value.DrawAabb);
            /*
            _drawArc = new DrawArcUnmanagedDelegate(DrawArc);
            _drawBox = new DrawBoxUnmanagedDelegate(DrawBox);
            _drawCapsule = new DrawCapsuleUnmanagedDelegate(DrawCapsule);
            _drawCone = new DrawConeUnmanagedDelegate(DrawCone);
            _drawContactPoint = new DrawContactPointUnmanagedDelegate(DrawContactPoint);
            _drawCylinder = new DrawCylinderUnmanagedDelegate(DrawCylinder);
            _drawLine = new DrawLineUnmanagedDelegate(DrawLine);
            _drawPlane = new DrawPlaneUnmanagedDelegate(DrawPlane);
            _drawSphere = new DrawSphereUnmanagedDelegate(DrawSphere);
            _drawSpherePatch = new DrawSpherePatchUnmanagedDelegate(DrawSpherePatch);
            _drawTransform = new DrawTransformUnmanagedDelegate(DrawTransform);
            _drawTriangle = new DrawTriangleUnmanagedDelegate(DrawTriangle);
            _getDebugMode = new GetDebugModeUnmanagedDelegate(GetDebugModeUnmanaged);
            _cb = new SimpleCallback(SimpleCallbackUnmanaged);

            _native = btIDebugDrawWrapper_new(
                GCHandle.ToIntPtr(GCHandle.Alloc(this)),
                Marshal.GetFunctionPointerForDelegate(_drawAabb),
                Marshal.GetFunctionPointerForDelegate(_drawArc),
                Marshal.GetFunctionPointerForDelegate(_drawBox),
                Marshal.GetFunctionPointerForDelegate(_drawCapsule),
                Marshal.GetFunctionPointerForDelegate(_drawCone),
                Marshal.GetFunctionPointerForDelegate(_drawContactPoint),
                Marshal.GetFunctionPointerForDelegate(_drawCylinder),
                Marshal.GetFunctionPointerForDelegate(_drawLine),
                Marshal.GetFunctionPointerForDelegate(_drawPlane),
                Marshal.GetFunctionPointerForDelegate(_drawSphere),
                Marshal.GetFunctionPointerForDelegate(_drawSpherePatch),
                Marshal.GetFunctionPointerForDelegate(_drawTransform),
                Marshal.GetFunctionPointerForDelegate(_drawTriangle),
                Marshal.GetFunctionPointerForDelegate(_getDebugMode),
                Marshal.GetFunctionPointerForDelegate(_cb));
            */
            return IntPtr.Zero;
        }

		public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			var pHinge = (HingeConstraint)constraint;
			Matrix tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyA().GetCenterOfMassTransform(), pHinge.GetAFrame());

			if (DrawFrames)
				debugDraw.DrawTransform(ref tr, DrawSize);

			tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyB().GetCenterOfMassTransform(), pHinge.GetBFrame());

			if (DrawFrames)
				debugDraw.DrawTransform(ref tr, DrawSize);

			float minAng = pHinge.GetLowerLimit();
			float maxAng = pHinge.GetUpperLimit();

			if (minAng == maxAng)
				return;

			bool drawSect = true;

			if (minAng > maxAng)
			{
				minAng = 0f;
				maxAng = MathUtil.SIMD_2_PI;
				drawSect = false;
			}

			if (DrawLimits)
			{
				Vector3 center = tr.Translation;
				Vector3 normal = MathUtil.MatrixColumn(ref tr, 2);
				Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
				Vector3 zero = Vector3.Zero;
				debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, minAng, maxAng, ref zero, drawSect);
			}
		}

	    ///btActionInterface interface
        public virtual void DebugDraw(IDebugDraw debugDrawer)
        {
	        for (int v=0;v<GetNumWheels();v++)
	        {
		        Vector3 wheelColor = new Vector3(0,1,1);
		        if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
		        {
			        wheelColor = new Vector3(0,0,1);
		        } else
		        {
			        wheelColor= new Vector3(1,0,1);
		        }

		        Vector3 wheelPosWS = GetWheelInfo(v).m_worldTransform.Translation;

                Matrix temp = GetWheelInfo(v).m_worldTransform;
                Vector3 axle = MathUtil.MatrixColumn(ref temp, GetRightAxis());

		        //debug wheels (cylinders)
		        debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
		        debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);

	        }
        }

        public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
                ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
        {
            bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();


            float minProj = float.MaxValue;
            Vector3 minNorm = Vector3.Zero;
            Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
            Vector3 seperatingAxisInA, seperatingAxisInB;
            Vector3 pInA, qInB, pWorld, qWorld, w;

#if USE_BATCHED_SUPPORT


            IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
            IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
            IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
            IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);


            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            for (int i = 0; i < numSampleDirections; i++)
            {
                Vector3 norm = sPenetrationDirections[i];
				seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
				seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
            }

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        Vector3 norm = Vector3.Up;
                        convexA.GetPreferredPenetrationDirection(i, ref norm);
                        norm = Vector3.TransformNormal(norm, transA);
                        sPenetrationDirections[numSampleDirections] = norm;
                        seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
                        seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        Vector3 norm = Vector3.Up;
                        convexB.GetPreferredPenetrationDirection(i, ref norm);
                        norm = Vector3.TransformNormal(norm, transB);
                        sPenetrationDirections[numSampleDirections] = norm;
                        seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
                        seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
                        numSampleDirections++;
                    }
                }
            }

            convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
            convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);

            for (int i = 0; i < numSampleDirections; i++)
            {
                Vector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    // shouldn't this be Y ?
                    norm.Z = 0;
                }
                seperatingAxisInA = seperatingAxisInABatch[i];
                seperatingAxisInB = seperatingAxisInBBatch[i];

                pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
                qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);

                pWorld = Vector3.Transform(pInA, transA);
                qWorld = Vector3.Transform(qInB, transB);
                if (check2d)
                {
                    // shouldn't this be Y ?
                    pWorld.Z = 0f;
                    qWorld.Z = 0f;
                }


                w = qWorld - pWorld;
                float delta = Vector3.Dot(norm, w);
                //find smallest delta
                if (delta < minProj)
                {
                    minProj = delta;
                    minNorm = norm;
                    minA = pWorld;
                    minB = qWorld;
//.........这里部分代码省略.........

        public void Draw(IDebugDraw drawer)
        {
            int i;
            for (i = 0; i < NUMRAYS_IN_BAR; i++)
            {
                drawer.DrawLine(ref source[i], ref hit_com[i], ref green);
            }
            const float normalScale = 10.0f; // easier to see if this is big
            for (i = 0; i < NUMRAYS_IN_BAR; i++)
            {
                Vector3 to = hit_surface[i] + normalScale * normal[i];
                drawer.DrawLine(ref hit_surface[i], ref to, ref white);
            }
            Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f);
            Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f);
            for (i = 0; i < NUMRAYS_IN_BAR; i++)
            {
                Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]);
                Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]);
                Vector3 linVel, angVel;

                TransformUtil.CalculateVelocity(ref from, ref to, 1.0f, out linVel, out angVel);
                Matrix T;
                TransformUtil.IntegrateTransform(ref from, ref linVel, ref angVel, hit_fraction[i], out T);
                Vector3 box1 = boxShapeHalfExtents;
                Vector3 box2 = -boxShapeHalfExtents;
                drawer.DrawBox(ref box1, ref box2, ref T, ref cyan);
            }
        }