C# IDebugDraw.DrawLine方法代码示例

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

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

        public void DebugDraw(IDebugDraw debugDrawer)
        {
            for (int v = 0; v < NumWheels; v++)
            {
                WheelInfo wheelInfo = GetWheelInfo(v);

                Color wheelColor;
                if (wheelInfo.RaycastInfo.IsInContact)
                {
                    wheelColor = Color.Blue;
                }
                else
                {
                    wheelColor = Color.Magenta;
                }

                Matrix transform = wheelInfo.WorldTransform;
                Vector3 wheelPosWS = transform.Origin;

                Vector3 axle = new Vector3(
                    transform[0, RightAxis],
                    transform[1, RightAxis],
                    transform[2, RightAxis]);

                Vector3 to1 = wheelPosWS + axle;
                Vector3 to2 = GetWheelInfo(v).RaycastInfo.ContactPointWS;

                //debug wheels (cylinders)
                debugDrawer.DrawLine(ref wheelPosWS, ref to1, wheelColor);
                debugDrawer.DrawLine(ref wheelPosWS, ref to2, wheelColor);

            }
        }

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

        public void Draw(IDebugDraw drawer)
        {
            for (int i = 0; i < NumRays; i++)
            {
                drawer.DrawLine(ref _source[i], ref _hitCenterOfMass[i], ref _green);

                Vector3 to = _hitPoint[i] + NormalScale * _normal[i];
                drawer.DrawLine(ref _hitPoint[i], ref to, ref _white);

                Matrix fromTransform = _fromRotation * Matrix.Translation(_source[i]);
                Matrix toTransform = _toRotation * Matrix.Translation(_destination[i]);
                Vector3 linVel, angVel;
                TransformUtil.CalculateVelocity(ref fromTransform, ref toTransform, 1.0f, out linVel, out angVel);
                Matrix transform;
                TransformUtil.IntegrateTransform(ref fromTransform, ref linVel, ref angVel, _hitFraction[i], out transform);
                drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, ref _cyan);
            }
        }

 public void Draw(IDebugDraw drawer)
 {
     int i;
     for (i = 0; i < NumRays; i++)
     {
         drawer.DrawLine(ref _source[i], ref _hitPoint[i], ref green);
     }
     for (i = 0; i < NumRays; i++)
     {
         Vector3 to = _hitPoint[i] + _normal[i];
         drawer.DrawLine(ref _hitPoint[i], ref to, ref white);
     }
 }

		public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw)
		{
			bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0;
			bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0;
			float dbgDrawSize = constraint.GetDbgDrawSize();
			if (dbgDrawSize <= 0f)
			{
				return;
			}

			switch (constraint.GetConstraintType())
			{
				case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE:
					{
						Point2PointConstraint p2pC = constraint as Point2PointConstraint;
						IndexedMatrix tr = IndexedMatrix.Identity;
						IndexedVector3 pivot = p2pC.GetPivotInA();
						pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform()* pivot;
						tr._origin = pivot;
						debugDraw.DrawTransform(ref tr, dbgDrawSize);
						// that ideally should draw the same frame	
						pivot = p2pC.GetPivotInB();
						pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot;
						tr._origin = pivot;
						if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize);
					}
					break;
				case TypedConstraintType.HINGE_CONSTRAINT_TYPE:
					{
						HingeConstraint pHinge = constraint as HingeConstraint;
						IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame();
						if (drawFrames)
						{
							debugDraw.DrawTransform(ref tr, dbgDrawSize);
						}
						tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() *  pHinge.GetBFrame();
						if (drawFrames)
						{
							debugDraw.DrawTransform(ref tr, dbgDrawSize);
						}
						float minAng = pHinge.GetLowerLimit();
						float maxAng = pHinge.GetUpperLimit();
						if (minAng == maxAng)
						{
							break;
						}
						bool drawSect = true;
						if (minAng > maxAng)
						{
							minAng = 0f;
							maxAng = MathUtil.SIMD_2_PI;
							drawSect = false;
						}
						if (drawLimits)
						{
							IndexedVector3 center = tr._origin;
							IndexedVector3 normal = tr._basis.GetColumn(2);
                            IndexedVector3 axis = tr._basis.GetColumn(0);
							IndexedVector3 zero = IndexedVector3.Zero;
							debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect);
						}
					}
					break;
				case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE:
					{
						ConeTwistConstraint pCT = constraint as ConeTwistConstraint;
						IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() *  pCT.GetAFrame();
						if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize);
						tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() *  pCT.GetBFrame();
						if (drawFrames) debugDraw.DrawTransform(ref tr, dbgDrawSize);
						IndexedVector3 zero = IndexedVector3.Zero;

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

								if (i % (nSegments / 8) == 0)
								{
									IndexedVector3 origin = tr._origin;
									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)
							{
//.........这里部分代码省略.........

 public void Draw(IDebugDraw drawer)
 {
     int i;
     for (i = 0; i < NUMRAYS_IN_BAR; i++)
     {
         drawer.DrawLine(ref source[i], ref hit[i], Color.Lime);
     }
     for (i = 0; i < NUMRAYS_IN_BAR; i++)
     {
         Vector3 to = hit[i] + normal[i];
         drawer.DrawLine(ref hit[i], ref to, Color.White);
     }
 }

		public static void DebugDrawObject(ref Matrix worldTransform, CollisionShape shape, ref Vector3 color,
										   IDebugDraw debugDraw)
		{
			// Draw a small simplex at the center of the object
			{
				Vector3 start = worldTransform.Translation;
				float scale = 10f;
				debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Right, worldTransform) * scale), Vector3.Right);
				debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Up, worldTransform) * scale), Vector3.Up);
				debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Backward, worldTransform) * scale),
								   Vector3.Backward);
			}
			//return;
			if (shape.ShapeType == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
			{
				var compoundShape = (CompoundShape)shape;
				for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
				{
					Matrix childTrans = compoundShape.GetChildTransform(i);
					CollisionShape colShape = compoundShape.GetChildShape(i);
					Matrix temp = MathUtil.BulletMatrixMultiply(worldTransform, childTrans);
					DebugDrawObject(ref temp, colShape, ref color, debugDraw);
				}
			}
			else
			{
				switch (shape.ShapeType)
				{
					case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
						{
							var sphereShape = (SphereShape)shape;
							float radius = sphereShape.Margin; //radius doesn't include the margin, so draw with margin
							DebugDrawSphere(radius, ref worldTransform, ref color, debugDraw);
							break;
						}
					case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
						{
							var multiSphereShape = (MultiSphereShape)shape;

							for (int i = multiSphereShape.GetSphereCount() - 1; i >= 0; i--)
							{
								Matrix childTransform = worldTransform;
								childTransform.Translation += multiSphereShape.GetSpherePosition(i);
								DebugDrawSphere(multiSphereShape.GetSphereRadius(i), ref childTransform, ref color, debugDraw);
							}

							break;
						}
					case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
						{
							var capsuleShape = (CapsuleShape)shape;

							float radius = capsuleShape.getRadius();
							float halfHeight = capsuleShape.getHalfHeight();

							int upAxis = capsuleShape.GetUpAxis();

							Vector3 capStart = Vector3.Zero;
							;
							MathUtil.VectorComponent(ref capStart, upAxis, -halfHeight);

							Vector3 capEnd = Vector3.Zero;
							MathUtil.VectorComponent(ref capEnd, upAxis, halfHeight);

							// Draw the ends
							{
								Matrix childTransform = worldTransform;
								childTransform.Translation = Vector3.Transform(capStart, worldTransform);
								DebugDrawSphere(radius, ref childTransform, ref color, debugDraw);
							}

							{
								Matrix childTransform = worldTransform;
								childTransform.Translation = Vector3.Transform(capEnd, worldTransform);
								DebugDrawSphere(radius, ref childTransform, ref color, debugDraw);
							}

							// Draw some additional lines
							Vector3 start = worldTransform.Translation;

							MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, radius);
							MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, radius);
							debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
											   start + Vector3.TransformNormal(capEnd, worldTransform), color);

							MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, -radius);
							MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, -radius);
							debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
											   start + Vector3.TransformNormal(capEnd, worldTransform), color);

							MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, radius);
							MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, radius);

							MathUtil.VectorComponent(ref capStart, (upAxis + 2) % 3, radius);
							MathUtil.VectorComponent(ref capEnd, (upAxis + 2) % 3, radius);
							debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
											   start + Vector3.TransformNormal(capEnd, worldTransform), color);

							MathUtil.VectorComponent(ref capStart, (upAxis + 2) % 3, -radius);
							MathUtil.VectorComponent(ref capEnd, (upAxis + 2) % 3, -radius);
//.........这里部分代码省略.........

		protected static void DebugDrawSphere(float radius, ref Matrix transform, ref Vector3 color, IDebugDraw debugDraw)
		{
			Vector3 start = transform.Translation;

			Vector3 xoffs = Vector3.TransformNormal(new Vector3(radius, 0, 0), transform);
			Vector3 yoffs = Vector3.TransformNormal(new Vector3(0, radius, 0), transform);
			Vector3 zoffs = Vector3.TransformNormal(new Vector3(0, 0, radius), transform);

			// XY 
			debugDraw.DrawLine(start - xoffs, start + yoffs, color);
			debugDraw.DrawLine(start + yoffs, start + xoffs, color);
			debugDraw.DrawLine(start + xoffs, start - yoffs, color);
			debugDraw.DrawLine(start - yoffs, start - xoffs, color);

			// XZ
			debugDraw.DrawLine(start - xoffs, start + zoffs, color);
			debugDraw.DrawLine(start + zoffs, start + xoffs, color);
			debugDraw.DrawLine(start + xoffs, start - zoffs, color);
			debugDraw.DrawLine(start - zoffs, start - xoffs, color);

			// YZ
			debugDraw.DrawLine(start - yoffs, start + zoffs, color);
			debugDraw.DrawLine(start + zoffs, start + yoffs, color);
			debugDraw.DrawLine(start + yoffs, start - zoffs, color);
			debugDraw.DrawLine(start - zoffs, start - yoffs, color);
		}