C# Vector2f.Length方法代码示例

		public virtual int Collide(PShape s1, PShape s2, PContact[] cs) {
			if (s1._type != PShapeType.CIRCLE_SHAPE
					|| s2._type != PShapeType.CONVEX_SHAPE
					&& s2._type != PShapeType.BOX_SHAPE) {
				return 0;
			}
			PCircleShape c1 = (PCircleShape) s1;
			PConvexPolygonShape p1 = (PConvexPolygonShape) s2;
			float distance = -1F;
			int edgeNumber = -1;
			Vector2f[] vers = p1.vers;
			int numVers = p1.numVertices;
			Vector2f normal = new Vector2f();
			Vector2f edgeNormal = new Vector2f();
			Vector2f a = new Vector2f();
			Vector2f b = new Vector2f();
			int num = 0;
			for (int i = 0; i < numVers; i++) {
				a.Set(c1._pos.x - vers[i].x, c1._pos.y - vers[i].y);
				distance = a.Length();
				distance -= c1.rad;
				if (distance <= 0.0F) {
					PContact c = new PContact();
					c.overlap = distance;
					a.Normalize();
					c.normal.Set(a.x, a.y);
					c.pos.Set(vers[i].x, vers[i].y);
					cs[num] = c;
					if (++num == 2) {
						return num;
					}
				}
			}
	
			if (num > 0) {
				return num;
			}
			for (int i_0 = 0; i_0 < numVers; i_0++) {
				Vector2f ver = vers[i_0];
				Vector2f nextVer = vers[(i_0 + 1) % numVers];
				float edgeX = nextVer.x - ver.x;
				float edgeY = nextVer.y - ver.y;
				edgeNormal.Set(edgeY, -edgeX);
				edgeNormal.Normalize();
				a.Set(c1._pos.x - ver.x, c1._pos.y - ver.y);
				b.Set(c1._pos.x - nextVer.x, c1._pos.y - nextVer.y);
				if ((a.x * edgeX + a.y * edgeY) * (b.x * edgeX + b.y * edgeY) <= 0.0F) {
					float edgeLen = (float) System.Math.Sqrt(edgeX * edgeX + edgeY * edgeY);
					float distanceToEdge = System.Math.Abs(a.x * edgeY - a.y * edgeX)
							/ edgeLen;
					if (distanceToEdge <= c1.rad) {
						distanceToEdge -= c1.rad;
						if (distance > distanceToEdge || distance == -1F) {
							edgeNumber = i_0;
							distance = distanceToEdge;
							normal.Set(edgeNormal.x, edgeNormal.y);
						}
					}
				}
			}
	
			if (edgeNumber > -1) {
				PContact c_1 = new PContact();
				c_1.overlap = distance;
				c_1.normal = normal;
				c_1.pos = c1._pos.Sub(normal.Mul(c1.rad));
				cs[0] = c_1;
				return 1;
			}
			bool hit = true;
			for (int i_2 = 0; i_2 < numVers; i_2++) {
				Vector2f ver = vers[i_2];
				Vector2f nextVer = vers[(i_2 + 1) % numVers];
				float v1x = nextVer.x - ver.x;
				float v1y = nextVer.y - ver.y;
				float v2x = c1._pos.x - ver.x;
				float v2y = c1._pos.y - ver.y;
				if (v1x * v2y - v1y * v2x >= 0.0F) {
					continue;
				}
				hit = false;
				break;
			}
	
			if (hit) {
				distance = 1.0F;
				normal = new Vector2f();
				for (int i = 0; i < numVers; i++) {
					Vector2f ver = vers[i];
					Vector2f nextVer = vers[(i + 1) % numVers];
					a.Set(nextVer.x - ver.x, nextVer.y - ver.y);
					a.Normalize();
					float d = c1._pos.Sub(ver).Cross(a);
					if (d < 0.0F && (distance == 1.0F || distance < d)) {
						distance = d;
						normal.Set(a.y, -a.x);
					}
				}
	
				if (distance != 1.0F) {
//.........这里部分代码省略.........

		internal override void PreSolve(float dt) {
			relAnchor1 = b1.mAng.Mul(localAnchor1);
			relAnchor2 = b2.mAng.Mul(localAnchor2);
			anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
			anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
			normal = anchor2.Sub(anchor1);
			length = normal.Length();
			normal.Normalize();
			mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
					normal);
			b1.ApplyImpulse(normal.x * norI, normal.y * norI, anchor1.x, anchor1.y);
			b2.ApplyImpulse(normal.x * -norI, normal.y * -norI, anchor2.x,
					anchor2.y);
		}

		internal override void PreSolve(float i_0) {
			relAnchor1 = b1.mAng.Mul(localAnchor1);
			relAnchor2 = b2.mAng.Mul(localAnchor2);
			anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
			anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
			normal = anchor2.Sub(anchor1);
			float over = dist - normal.Length();
			normal.Normalize();
			mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
					normal);
			float k = mass * 1000F * str;
			force = -over * k;
			force += PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1,
					relAnchor2).Dot(normal)
					* damp * -(float) System.Math.Sqrt(k * mass) * 2.0F;
			force *= i_0;
			b1.ApplyImpulse(normal.x * force, normal.y * force, anchor1.x,
					anchor1.y);
			b2.ApplyImpulse(normal.x * -force, normal.y * -force, anchor2.x,
					anchor2.y);
		}