C# Box2D.Vec2类代码示例

		public RopeTest()
		{
			const int N = 40;
			Vec2[] vertices = new Vec2[N];
			float [] masses = new float [N];

			for (int i = 0; i < N; ++i)
			{
				vertices[i].Set(0.0f, 20.0f - 0.25f * i);
				masses[i] = 1.0f;
			}
			masses[0] = 0.0f;
			masses[1] = 0.0f;

			RopeDef def = new RopeDef();
			def.vertices = new List<Vec2>(vertices);
			def.count = N;
			def.gravity.Set(0.0f, -10.0f);
			def.masses = new List<float>(masses);
			def.damping = 0.1f;
			def.k2 = 1.0f;
			def.k3 = 0.5f;

			m_rope.Initialize(def);

			m_angle = 0.0f;
			m_rope.SetAngle(m_angle);
		}

		public Test(){
			Vec2 gravity = new Vec2();
			gravity.Set(0.0f, -10.0f);
			m_world = new World(gravity);
			m_bomb = null;
			m_textLine = 30;
			m_mouseJoint = null;
			m_pointCount = 0;
			m_debugDraw = new DebugDraw();

			m_destructionListener = new TestDestructionListener();
			m_destructionListener.test = this;
			m_world.SetDestructionListener(m_destructionListener);
			m_world.SetContactListener(this);
			m_world.SetDebugDraw(m_debugDraw);
	
			m_bombSpawning = false;

			m_stepCount = 0;

			BodyDef bodyDef = new BodyDef();
			m_groundBody = m_world.CreateBody(bodyDef);

			m_maxProfile = new Profile();
			m_totalProfile = new Profile();
		}

		/// Initialize the bodies, anchors, and reference angle using a world
		/// anchor point.
		// Point-to-point constraint
		// C = p2 - p1
		// Cdot = v2 - v1
		//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
		// J = [-I -r1_skew I r2_skew ]
		// Identity used:
		// w k % (rx i + ry j) = w * (-ry i + rx j)

		// Angle constraint
		// C = angle2 - angle1 - referenceAngle
		// Cdot = w2 - w1
		// J = [0 0 -1 0 0 1]
		// K = invI1 + invI2
		public void Initialize(Body bA, Body bB, Vec2 anchor) {
			bodyA = bA;
			bodyB = bB;
			localAnchorA = bodyA.GetLocalPoint(anchor);
			localAnchorB = bodyB.GetLocalPoint(anchor);
			referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
		}

		public override void DrawSolidCircle(Vec2 center, float radius, Vec2 axis, Color color) {
			float k_segments = 16.0f;
			float k_increment = 2.0f * (float)System.Math.PI / k_segments;
			float theta = 0.0f;
			GL.Color3(0.5f * color.R, 0.5f * color.G, 0.5f * color.B);
			GL.Disable(EnableCap.Texture2D);
			GL.Begin(BeginMode.TriangleFan);
			for (int i = 0; i < k_segments; ++i) {
				Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
				GL.Vertex2(v.X, v.Y);
				theta += k_increment;
			}
			GL.End();

			theta = 0.0f;
			GL.Color4(color.R, color.G, color.B, 1.0f);
			GL.Begin(BeginMode.LineLoop);
			for (int i = 0; i < k_segments; ++i) {
				Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
				GL.Vertex2(v.X, v.Y);
				theta += k_increment;
			}
			GL.End();

			Vec2 p = center + radius * axis;
			GL.Begin(BeginMode.Lines);
			GL.Vertex2(center.X, center.Y);
			GL.Vertex2(p.X, p.Y);
			GL.End();
			GL.Enable(EnableCap.Texture2D);
		}

		/// Use this to update the target point.
		public void SetTarget(Vec2 target){
			if (m_bodyB.IsAwake() == false)
			{
				m_bodyB.SetAwake(true);
			}
			m_targetA = target;
		}

		/// Initialize the bodies, anchors, axis, and reference angle using the world
		/// anchor and world axis.
		// Linear constraint (point-to-line)
		// d = pB - pA = xB + rB - xA - rA
		// C = dot(ay, d)
		// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
		//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
		// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]

		// Spring linear constraint
		// C = dot(ax, d)
		// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
		// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]

		// Motor rotational constraint
		// Cdot = wB - wA
		// J = [0 0 -1 0 0 1]
		public void Initialize(Body bA, Body bB, Vec2 anchor, Vec2 axis) {
			bodyA = bA;
			bodyB = bB;
			localAnchorA = bodyA.GetLocalPoint(anchor);
			localAnchorB = bodyB.GetLocalPoint(anchor);
			localAxisA = bodyA.GetLocalVector(axis);
		}

		public override void Step(TestSettings settings)
		{
			Manifold manifold;
			Collision.CollidePolygons(out manifold, m_polygonA, m_transformA, m_polygonB, m_transformB);

			WorldManifold worldManifold = new WorldManifold();
			worldManifold.Initialize(manifold, m_transformA, m_polygonA.m_radius, m_transformB, m_polygonB.m_radius);

			m_debugDraw.DrawString("point count = {0}", manifold.points.Count());
			

			{
				Color color = Color.FromArgb(225, 225, 225);
				Vec2[] v = new Vec2[Settings._maxPolygonVertices];
				for (int i = 0; i < m_polygonA.m_count; ++i)
				{
					v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
				}
				m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);

				for (int i = 0; i < m_polygonB.m_count; ++i)
				{
					v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
				}
				m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
			}

			for (int i = 0; i < manifold.points.Count(); ++i)
			{
				m_debugDraw.DrawPoint(worldManifold.points[i], 4.0f, Color.FromArgb(225, 75, 75));
			}
		}

		/// Set/get the target linear offset, in frame A, in meters.
		public void SetLinearOffset(Vec2 linearOffset){
			if (linearOffset.X != m_linearOffset.X || linearOffset.Y != m_linearOffset.Y)
			{
				m_bodyA.SetAwake(true);
				m_bodyB.SetAwake(true);
				m_linearOffset = linearOffset;
			}
		}

		/// Initialize the bodies, anchors, and length using the world
		/// anchors.
		// 1-D constrained system
		// m (v2 - v1) = lambda
		// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
		// x2 = x1 + h * v2

		// 1-D mass-damper-spring system
		// m (v2 - v1) + h * d * v2 + h * k * 

		// C = norm(p2 - p1) - L
		// u = (p2 - p1) / norm(p2 - p1)
		// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
		// J = [-u -cross(r1, u) u cross(r2, u)]
		// K = J * invM * JT
		//   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
		public void Initialize(Body b1, Body b2,
						Vec2 anchor1, Vec2 anchor2) {
			bodyA = b1;
			bodyB = b2;
			localAnchorA = bodyA.GetLocalPoint(anchor1);
			localAnchorB = bodyB.GetLocalPoint(anchor2);
			Vec2 d = anchor2 - anchor1;
			length = d.Length();
		}

	public class RopeDef { //was struct
		public RopeDef() {
			vertices = new List<Vec2>();
			count = 0;
			masses = new List<float>();
			gravity = new Vec2(0, 0);
			damping = 0.1f;
			k2 = 0.9f;
			k3 = 0.1f;
		}

		public override float ReportFixture(Fixture fixture, Vec2 point,
			Vec2 normal, float fraction)
		{
			m_fixture = fixture;
			m_point = point;
			m_normal = normal;

			return fraction;
		}

		public override void DrawPolygon(Vec2[] vertices, int vertexCount, Color color) {
			GL.Color3(color.R, color.G, color.B);
			GL.Disable(EnableCap.Texture2D);
			GL.Begin(BeginMode.LineLoop);
			for (int i = 0; i < vertexCount; ++i) {
				GL.Vertex2(vertices[i].X, vertices[i].Y);
			}
			GL.End();
			GL.Enable(EnableCap.Texture2D);
		}

		public override void DrawSegment(Vec2 p1, Vec2 p2, Color color) {
			GL.Color3(color.R, color.G, color.B);
			GL.Disable(EnableCap.Texture2D);
			GL.Begin(BeginMode.Lines);
			{
				GL.Vertex2(p1.X, p1.Y);
				GL.Vertex2(p2.X, p2.Y);
			}
			GL.End();
			GL.Enable(EnableCap.Texture2D);
		}

		/// Solve A * x = b, where b is a column vector. This is more efficient
		/// than computing the inverse in one-shot cases. Solve only the upper
		/// 2-by-2 matrix equation.
		public Vec2 Solve22(Vec2 b) {
			float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y;
			float det = a11 * a22 - a12 * a21;
			if (det != 0.0f) {
				det = 1.0f / det;
			}
			Vec2 x;
			x.X = det * (a22 * b.X - a12 * b.Y);
			x.Y = det * (a11 * b.Y - a21 * b.X);
			return x;
		}

		/// Get the supporting vertex index in the given direction.
		public int GetSupport(Vec2 d){
			int bestIndex = 0;
			float bestValue = Utilities.Dot(m_vertices[0], d);
			for (int i = 1; i < m_vertices.Count(); ++i) {
				float value = Utilities.Dot(m_vertices[i], d);
				if (value > bestValue) {
					bestIndex = i;
					bestValue = value;
				}
			}

			return bestIndex;
		}