Пример #1
0
		/// Compute the collision manifold between two circles.
		public static void CollideCircles(out Manifold manifold,
							  CircleShape circleA, Transform xfA,
							  CircleShape circleB, Transform xfB) {
			throw new NotImplementedException();
			//manifold.pointCount = 0;

			//Vec2 pA = Utilities.Mul(xfA, circleA.m_p);
			//Vec2 pB = Utilities.Mul(xfB, circleB.m_p);

			//Vec2 d = pB - pA;
			//float distSqr = Utilities.Dot(d, d);
			//float rA = circleA.m_radius, rB = circleB.m_radius;
			//float radius = rA + rB;
			//if (distSqr > radius * radius)
			//{
			//    return;
			//}

			//manifold.type = Manifold::e_circles;
			//manifold.localPoint = circleA.m_p;
			//manifold.localNormal.SetZero();
			//manifold.pointCount = 1;

			//manifold.points[0].localPoint = circleB.m_p;
			//manifold.points[0].id.key = 0;
		}
Пример #2
0
		public override void Evaluate(out Manifold manifold, Transform xfA, Transform xfB){
			ChainShape chain = (ChainShape)m_fixtureA.GetShape();
			EdgeShape edge;
			chain.GetChildEdge(out edge, m_indexA);
			Collision.CollideEdgeAndCircle(out manifold, edge, xfA,
									(CircleShape)m_fixtureB.GetShape(), xfB);
		}
Пример #3
0
		public override void PreSolve(Contact contact, Manifold oldManifold)
		{
			base.PreSolve(contact, oldManifold);

			Fixture fixtureA = contact.FixtureA;
			Fixture fixtureB = contact.FixtureB;

			if (fixtureA != m_platform && fixtureA != m_character)
			{
				return;
			}

			if (fixtureB != m_platform && fixtureB != m_character)
			{
				return;
			}

	#if true
			Vec2 position = m_character.GetBody().GetPosition();

			if (position.Y < m_top + m_radius - 3.0f *Settings._linearSlop)
			{
				contact.SetEnabled(false);
			}
	#else
			Vec2 v = m_character.GetBody().GetLinearVelocity();
			if (v.Y > 0.0f)
			{
				contact.SetEnabled(false);
			}
	#endif
		}
Пример #4
0
		public override void PreSolve(Contact contact, Manifold oldManifold)
		{
			base.PreSolve(contact, oldManifold);

			Fixture fixtureA = contact.FixtureA;
			Fixture fixtureB = contact.FixtureB;

			if (fixtureA == m_platform)
			{
				contact.SetTangentSpeed(5.0f);
			}

			if (fixtureB == m_platform)
			{
				contact.SetTangentSpeed(-5.0f);
			}
		}
Пример #5
0
		/// Compute the collision manifold between an edge and a circle.
		public static void CollideEdgeAndCircle(out Manifold manifold,
									   EdgeShape edgeA, Transform xfA,
									   CircleShape circleB, Transform xfB){
			manifold = new Manifold();
	
			// Compute circle in frame of edge
			Vec2 Q = Utilities.MulT(xfA, Utilities.Mul(xfB, circleB.m_p));
	
			Vec2 A = edgeA.m_vertex1, B = edgeA.m_vertex2;
			Vec2 e = B - A;
	
			// Barycentric coordinates
			float u = Utilities.Dot(e, B - Q);
			float v = Utilities.Dot(e, Q - A);
	
			float radius = edgeA.m_radius + circleB.m_radius;
	
			ContactFeature cf;
			cf.indexB = 0;
			cf.typeB = ContactFeature.FeatureType.e_vertex;
	
			// Region A
			if (v <= 0.0f)
			{
				Vec2 P = A;
				Vec2 d = Q - P;
				float dd = Utilities.Dot(d, d);
				if (dd > radius * radius)
				{
					return;
				}
		
				// Is there an edge connected to A?
				if (edgeA.m_hasVertex0)
				{
					Vec2 A1 = edgeA.m_vertex0;
					Vec2 B1 = A;
					Vec2 e1 = B1 - A1;
					float u1 = Utilities.Dot(e1, B1 - Q);
			
					// Is the circle in Region AB of the previous edge?
					if (u1 > 0.0f)
					{
						return;
					}
				}
		
				cf.indexA = 0;
				cf.typeA = ContactFeature.FeatureType.e_vertex;
				manifold.points.Clear();
				manifold.points.Add(new ManifoldPoint());
				manifold.type = Manifold.ManifoldType.e_circles;
				manifold.localNormal.SetZero();
				manifold.localPoint = P;
				manifold.points[0].id.key = 0;
				manifold.points[0].id.cf = cf;
				manifold.points[0].localPoint = circleB.m_p;
				return;
			}
	
			// Region B
			if (u <= 0.0f)
			{
				Vec2 P = B;
				Vec2 d = Q - P;
				float dd = Utilities.Dot(d, d);
				if (dd > radius * radius)
				{
					return;
				}
		
				// Is there an edge connected to B?
				if (edgeA.m_hasVertex3)
				{
					Vec2 B2 = edgeA.m_vertex3;
					Vec2 A2 = B;
					Vec2 e2 = B2 - A2;
					float v2 = Utilities.Dot(e2, Q - A2);
			
					// Is the circle in Region AB of the next edge?
					if (v2 > 0.0f)
					{
						return;
					}
				}
		
				cf.indexA = 1;
				cf.typeA = ContactFeature.FeatureType.e_vertex;
				manifold.points.Clear();
				manifold.points.Add(new ManifoldPoint());
				manifold.type = Manifold.ManifoldType.e_circles;
				manifold.localNormal.SetZero();
				manifold.localPoint = P;
				manifold.points[0].id.key = 0;
				manifold.points[0].id.cf = cf;
				manifold.points[0].localPoint = circleB.m_p;
				return;
			}
	
			// Region AB
			float den = Utilities.Dot(e, e);
			Utilities.Assert(den > 0.0f);
			Vec2 Pb = (1.0f / den) * (u * A + v * B);
			Vec2 db = Q - Pb;
			float ddb = Utilities.Dot(db, db);
			if (ddb > radius * radius)
			{
				return;
			}
	
			Vec2 n = new Vec2(-e.Y, e.X);
			if (Utilities.Dot(n, Q - A) < 0.0f)
			{
				n.Set(-n.X, -n.Y);
			}
			n.Normalize();
	
			cf.indexA = 0;
			cf.typeA = ContactFeature.FeatureType.e_face;
			manifold.points.Clear();
			manifold.points.Add(new ManifoldPoint());
			manifold.type = Manifold.ManifoldType.e_faceA;
			manifold.localNormal = n;
			manifold.localPoint = A;
			manifold.points[0].id.key = 0;
			manifold.points[0].id.cf = cf;
			manifold.points[0].localPoint = circleB.m_p;
		}
Пример #6
0
		/// Compute the collision manifold between a polygon and a circle.
		public static void CollidePolygonAndCircle(out Manifold manifold,
									   PolygonShape polygonA, Transform xfA,
									   CircleShape circleB, Transform xfB) {
			manifold = new Manifold();
			manifold.points.Clear();

			// Compute circle position in the frame of the polygon.
			Vec2 c = Utilities.Mul(xfB, circleB.m_p);
			Vec2 cLocal = Utilities.MulT(xfA, c);

			// Find the min separating edge.
			int normalIndex = 0;
			float separation = -Single.MaxValue;
			float radius = polygonA.m_radius + circleB.m_radius;
			int vertexCount = polygonA.m_count;
			List<Vec2> vertices = new List<Vec2>(polygonA.m_vertices);
			List<Vec2> normals = new List<Vec2>(polygonA.m_normals);

			for (int i = 0; i < vertexCount; ++i)
			{
			    float s = Utilities.Dot(normals[i], cLocal - vertices[i]);

			    if (s > radius)
			    {
			        // Early out.
			        return;
			    }

			    if (s > separation)
			    {
			        separation = s;
			        normalIndex = i;
			    }
			}

			// Vertices that subtend the incident face.
			int vertIndex1 = normalIndex;
			int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
			Vec2 v1 = vertices[vertIndex1];
			Vec2 v2 = vertices[vertIndex2];

			// If the center is inside the polygon ...
			if (separation < Single.Epsilon)
			{
				manifold.points.Clear();
				manifold.points.Add(new ManifoldPoint());
			    manifold.type = Manifold.ManifoldType.e_faceA;
			    manifold.localNormal = normals[normalIndex];
			    manifold.localPoint = 0.5f * (v1 + v2);
			    manifold.points[0].localPoint = circleB.m_p;
			    manifold.points[0].id.key = 0;
			    return;
			}

			// Compute barycentric coordinates
			float u1 = Utilities.Dot(cLocal - v1, v2 - v1);
			float u2 = Utilities.Dot(cLocal - v2, v1 - v2);
			if (u1 <= 0.0f)
			{
			    if (Utilities.DistanceSquared(cLocal, v1) > radius * radius)
			    {
			        return;
			    }

				manifold.points.Clear();
				manifold.points.Add(new ManifoldPoint());
			    manifold.type = Manifold.ManifoldType.e_faceA;
			    manifold.localNormal = cLocal - v1;
			    manifold.localNormal.Normalize();
			    manifold.localPoint = v1;
			    manifold.points[0].localPoint = circleB.m_p;
			    manifold.points[0].id.key = 0;
			}
			else if (u2 <= 0.0f)
			{
				if (Utilities.DistanceSquared(cLocal, v2) > radius * radius)
			    {
			        return;
			    }

				manifold.points = new List<ManifoldPoint>();
				manifold.points.Add(new ManifoldPoint());
			    manifold.type = Manifold.ManifoldType.e_faceA;
			    manifold.localNormal = cLocal - v2;
			    manifold.localNormal.Normalize();
			    manifold.localPoint = v2;
			    manifold.points[0].localPoint = circleB.m_p;
			    manifold.points[0].id.key = 0;
			}
			else
			{
			    Vec2 faceCenter = 0.5f * (v1 + v2);
			    float separation2 = Utilities.Dot(cLocal - faceCenter, normals[vertIndex1]);
			    if (separation2 > radius)
			    {
			        return;
			    }

				manifold.points = new List<ManifoldPoint>();
				manifold.points.Add(new ManifoldPoint());
			    manifold.type = Manifold.ManifoldType.e_faceA;
			    manifold.localNormal = normals[vertIndex1];
			    manifold.localPoint = faceCenter;
			    manifold.points[0].localPoint = circleB.m_p;
			    manifold.points[0].id.key = 0;
			}
		}
Пример #7
0
		// Find edge normal of max separation on A - return if separating axis is found
		// Find edge normal of max separation on B - return if separation axis is found
		// Choose reference edge as min(minA, minB)
		// Find incident edge
		// Clip

		// The normal points from 1 to 2
		/// Compute the collision manifold between two polygons.
		public static void CollidePolygons(out Manifold manifold,
							   PolygonShape polyA, Transform xfA,
							   PolygonShape polyB, Transform xfB) {
			manifold = new Manifold();
			float totalRadius = polyA.m_radius + polyB.m_radius;

			int edgeA = 0;
			float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
			if (separationA > totalRadius)
			    return;

			int edgeB = 0;
			float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
			if (separationB > totalRadius)
			    return;

			PolygonShape poly1;	// reference polygon
			PolygonShape poly2;	// incident polygon
			Transform xf1, xf2;
			int edge1;		// reference edge
			bool flip;
			const float k_relativeTol = 0.98f;
			const float k_absoluteTol = 0.001f;

			if (separationB > k_relativeTol * separationA + k_absoluteTol)
			{
			    poly1 = polyB;
			    poly2 = polyA;
			    xf1 = xfB;
			    xf2 = xfA;
			    edge1 = edgeB;
			    manifold.type = Manifold.ManifoldType.e_faceB;
			    flip = true;
			}
			else
			{
			    poly1 = polyA;
			    poly2 = polyB;
			    xf1 = xfA;
			    xf2 = xfB;
			    edge1 = edgeA;
			    manifold.type = Manifold.ManifoldType.e_faceA;
			    flip = false;
			}

			ClipVertex[] incidentEdge = new ClipVertex[2];
			FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);

			int count1 = poly1.m_count;
			Vec2[] vertices1 = poly1.m_vertices;

			int iv1 = edge1;
			int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

			Vec2 v11 = vertices1[iv1];
			Vec2 v12 = vertices1[iv2];

			Vec2 localTangent = v12 - v11;
			localTangent.Normalize();
	
			Vec2 localNormal = Utilities.Cross(localTangent, 1.0f);
			Vec2 planePoint = 0.5f * (v11 + v12);

			Vec2 tangent = Utilities.Mul(xf1.q, localTangent);
			Vec2 normal = Utilities.Cross(tangent, 1.0f);
	
			v11 = Utilities.Mul(xf1, v11);
			v12 = Utilities.Mul(xf1, v12);

			// Face offset.
			float frontOffset = Utilities.Dot(normal, v11);

			// Side offsets, extended by polytope skin thickness.
			float sideOffset1 = -Utilities.Dot(tangent, v11) + totalRadius;
			float sideOffset2 = Utilities.Dot(tangent, v12) + totalRadius;

			// Clip incident edge against extruded edge1 side edges.
			ClipVertex[] clipPoints1 = new ClipVertex[2];
			ClipVertex[] clipPoints2 = new ClipVertex[2];
			int np;

			// Clip to box side 1
			np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);

			if (np < 2)
			    return;

			// Clip to negative box side 1
			np = ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2, iv2);

			if (np < 2)
			{
			    return;
			}

			// Now clipPoints2 contains the clipped points.
			manifold.localNormal = localNormal;
			manifold.localPoint = planePoint;

			manifold.points.Clear();
			for (int i = 0; i < Settings._maxManifoldPoints; ++i)
			{
			    float separation = Utilities.Dot(normal, clipPoints2[i].v) - frontOffset;

			    if (separation <= totalRadius)
			    {
					ManifoldPoint cp = new ManifoldPoint();
			        cp.localPoint = Utilities.MulT(xf2, clipPoints2[i].v);
			        cp.id = clipPoints2[i].id;
			        if (flip)
			        {
			            // Swap features
			            ContactFeature cf = cp.id.cf;
			            cp.id.cf.indexA = cf.indexB;
			            cp.id.cf.indexB = cf.indexA;
			            cp.id.cf.typeA = cf.typeB;
			            cp.id.cf.typeB = cf.typeA;
			        }
					manifold.points.Add(cp);
			    }
			}
		}
Пример #8
0
 public void PreSolve(Contact contact, ref Manifold oldManifold)
 {
 }
Пример #9
0
		/// Evaluate the manifold with supplied transforms. This assumes
		/// modest motion from the original state. This does not change the
		/// point count, impulses, etc. The radii must come from the shapes
		/// that generated the manifold.
		public void Initialize(Manifold manifold,
								Transform xfA, float radiusA,
								Transform xfB, float radiusB){
			if (manifold.points.Count() == 0)
			{
				return;
			}

			switch (manifold.type)
			{
			case Manifold.ManifoldType.e_circles:
				{
					normal.Set(1.0f, 0.0f);
					Vec2 pointA = Utilities.Mul(xfA, manifold.localPoint);
					Vec2 pointB = Utilities.Mul(xfB, manifold.points[0].localPoint);
					if (Utilities.DistanceSquared(pointA, pointB) > Single.Epsilon * Single.Epsilon)
					{
						normal = pointB - pointA;
						normal.Normalize();
					}

					Vec2 cA = pointA + radiusA * normal;
					Vec2 cB = pointB - radiusB * normal;
					points[0] = 0.5f * (cA + cB);
				}
				break;

			case Manifold.ManifoldType.e_faceA:
				{
					normal = Utilities.Mul(xfA.q, manifold.localNormal);
					Vec2 planePoint = Utilities.Mul(xfA, manifold.localPoint);

					points.Clear();
					for (int i = 0; i < manifold.points.Count(); ++i)
					{
						Vec2 clipPoint = Utilities.Mul(xfB, manifold.points[i].localPoint);
						Vec2 cA = clipPoint + (radiusA - Utilities.Dot(clipPoint - planePoint, normal)) * normal;
						Vec2 cB = clipPoint - radiusB * normal;
						points.Add(0.5f * (cA + cB));
					}
				}
				break;

			case Manifold.ManifoldType.e_faceB:
				{
					normal = Utilities.Mul(xfB.q, manifold.localNormal);
					Vec2 planePoint = Utilities.Mul(xfB, manifold.localPoint);

					points.Clear();
					for (int i = 0; i < manifold.points.Count(); ++i)
					{
						Vec2 clipPoint = Utilities.Mul(xfA, manifold.points[i].localPoint);
						Vec2 cB = clipPoint + (radiusB - Utilities.Dot(clipPoint - planePoint, normal)) * normal;
						Vec2 cA = clipPoint - radiusA * normal;
						points.Add(0.5f * (cA + cB));
					}

					// Ensure normal points from A to B.
					normal = -normal;
				}
				break;
			}
		}
Пример #10
0
		/// Evaluate this contact with your own manifold and transforms.
		public abstract void Evaluate(out Manifold manifold, Transform xfA, Transform xfB); //manifold was pointer
Пример #11
0
		public virtual void PreSolve(Contact contact, Manifold oldManifold) {
			Manifold manifold = contact.GetManifold();

			if (manifold.points.Count() == 0)
			{
				return;
			}

			Fixture fixtureA = contact.FixtureA;
			Fixture fixtureB = contact.FixtureB;

			PointState[] state1 = new PointState[Settings._maxManifoldPoints];
			PointState[] state2 = new PointState[Settings._maxManifoldPoints];
			Collision.GetPointStates(state1, state2, oldManifold, manifold);

			WorldManifold worldManifold;
			contact.GetWorldManifold(out worldManifold);

			for (int i = 0; i < manifold.points.Count() && m_pointCount < Program.k_maxContactPoints; ++i)
			{
				ContactPoint cp = m_points[m_pointCount];
				cp.fixtureA = fixtureA;
				cp.fixtureB = fixtureB;
				cp.position = worldManifold.points[i];
				cp.normal = worldManifold.normal;
				cp.state = state2[i];
				cp.normalImpulse = manifold.points[i].normalImpulse;
				cp.tangentImpulse = manifold.points[i].tangentImpulse;
				++m_pointCount;
			}
		}
Пример #12
0
        public void InitializeVelocityConstraints()
        {
            for (int i = 0; i < m_contacts.Count(); ++i)
            {
                ContactVelocityConstraint vc = m_velocityConstraints[i];
                ContactPositionConstraint pc = m_positionConstraints[i];

                float    radiusA  = pc.radiusA;
                float    radiusB  = pc.radiusB;
                Manifold manifold = m_contacts[vc.contactIndex].GetManifold();

                int indexA = vc.indexA;
                int indexB = vc.indexB;

                float mA           = vc.invMassA;
                float mB           = vc.invMassB;
                float iA           = vc.invIA;
                float iB           = vc.invIB;
                Vec2  localCenterA = pc.localCenterA;
                Vec2  localCenterB = pc.localCenterB;

                Vec2  cA = m_positions[indexA].c;
                float aA = m_positions[indexA].a;
                Vec2  vA = m_velocities[indexA].v;
                float wA = m_velocities[indexA].w;

                Vec2  cB = m_positions[indexB].c;
                float aB = m_positions[indexB].a;
                Vec2  vB = m_velocities[indexB].v;
                float wB = m_velocities[indexB].w;

                Utilities.Assert(manifold.points.Count() > 0);

                Transform xfA = new Transform();
                Transform xfB = new Transform();
                xfA.q.Set(aA);
                xfB.q.Set(aB);
                xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
                xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);

                WorldManifold worldManifold = new WorldManifold();
                worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);

                vc.normal = worldManifold.normal;

                int pointCount = vc.points.Count;
                for (int j = 0; j < pointCount; ++j)
                {
                    VelocityConstraintPoint vcp = vc.points[j];

                    vcp.rA = worldManifold.points[j] - cA;
                    vcp.rB = worldManifold.points[j] - cB;

                    float rnA = Utilities.Cross(vcp.rA, vc.normal);
                    float rnB = Utilities.Cross(vcp.rB, vc.normal);

                    float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

                    vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;

                    Vec2 tangent = Utilities.Cross(vc.normal, 1.0f);

                    float rtA = Utilities.Cross(vcp.rA, tangent);
                    float rtB = Utilities.Cross(vcp.rB, tangent);

                    float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;

                    vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;

                    // Setup a velocity bias for restitution.
                    vcp.velocityBias = 0.0f;
                    float vRel = Utilities.Dot(vc.normal, vB + Utilities.Cross(wB, vcp.rB) - vA - Utilities.Cross(wA, vcp.rA));
                    if (vRel < -Settings._velocityThreshold)
                    {
                        vcp.velocityBias = -vc.restitution * vRel;
                    }
                }

                // If we have two points, then prepare the block solver.
                if (vc.points.Count() == 2)
                {
                    VelocityConstraintPoint vcp1 = vc.points[0];
                    VelocityConstraintPoint vcp2 = vc.points[1];

                    float rn1A = Utilities.Cross(vcp1.rA, vc.normal);
                    float rn1B = Utilities.Cross(vcp1.rB, vc.normal);
                    float rn2A = Utilities.Cross(vcp2.rA, vc.normal);
                    float rn2B = Utilities.Cross(vcp2.rB, vc.normal);

                    float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
                    float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
                    float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;

                    // Ensure a reasonable condition number.
                    const float k_maxConditionNumber = 1000.0f;
                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
                    {
                        // K is safe to invert.
                        vc.K.ex.Set(k11, k12);
                        vc.K.ey.Set(k12, k22);
                        vc.normalMass = vc.K.GetInverse();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        vc.points.Clear();
                        vc.points.Add(new VelocityConstraintPoint());
                    }
                }
            }
        }
Пример #13
0
        public ContactSolver(ContactSolverDef def)
        {
            m_step = def.step;
            m_positionConstraints = new List <ContactPositionConstraint>();
            m_velocityConstraints = new List <ContactVelocityConstraint>();
            m_positions           = def.positions;
            m_velocities          = def.velocities;
            m_contacts            = def.contacts;

            // Initialize position independent portions of the constraints.
            for (int i = 0; i < def.contacts.Count(); ++i)
            {
                Contact contact = m_contacts[i];

                Fixture  fixtureA = contact.m_fixtureA;
                Fixture  fixtureB = contact.m_fixtureB;
                Shape    shapeA   = fixtureA.GetShape();
                Shape    shapeB   = fixtureB.GetShape();
                float    radiusA  = shapeA.m_radius;
                float    radiusB  = shapeB.m_radius;
                Body     bodyA    = fixtureA.GetBody();
                Body     bodyB    = fixtureB.GetBody();
                Manifold manifold = contact.GetManifold();

                int pointCount = manifold.points.Count();
                Utilities.Assert(pointCount > 0);

                ContactVelocityConstraint vc = new ContactVelocityConstraint();
                vc.friction     = contact.m_friction;
                vc.restitution  = contact.m_restitution;
                vc.tangentSpeed = contact.m_tangentSpeed;
                vc.indexA       = bodyA.m_islandIndex;
                vc.indexB       = bodyB.m_islandIndex;
                vc.invMassA     = bodyA.m_invMass;
                vc.invMassB     = bodyB.m_invMass;
                vc.invIA        = bodyA.m_invI;
                vc.invIB        = bodyB.m_invI;
                vc.contactIndex = i;
                //vc.points.Count() = pointCount;
                vc.K.SetZero();
                vc.normalMass.SetZero();

                ContactPositionConstraint pc = new ContactPositionConstraint();
                pc.indexA       = bodyA.m_islandIndex;
                pc.indexB       = bodyB.m_islandIndex;
                pc.invMassA     = bodyA.m_invMass;
                pc.invMassB     = bodyB.m_invMass;
                pc.localCenterA = bodyA.m_sweep.localCenter;
                pc.localCenterB = bodyB.m_sweep.localCenter;
                pc.invIA        = bodyA.m_invI;
                pc.invIB        = bodyB.m_invI;
                pc.localNormal  = manifold.localNormal;
                pc.localPoint   = manifold.localPoint;
                pc.pointCount   = pointCount;
                pc.radiusA      = radiusA;
                pc.radiusB      = radiusB;
                pc.type         = manifold.type;

                for (int j = 0; j < pointCount; ++j)
                {
                    ManifoldPoint           cp  = manifold.points[j];
                    VelocityConstraintPoint vcp = new VelocityConstraintPoint();

                    if (m_step.warmStarting)
                    {
                        vcp.normalImpulse  = m_step.dtRatio * cp.normalImpulse;
                        vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse;
                    }
                    else
                    {
                        vcp.normalImpulse  = 0.0f;
                        vcp.tangentImpulse = 0.0f;
                    }

                    vcp.rA.SetZero();
                    vcp.rB.SetZero();
                    vcp.normalMass   = 0.0f;
                    vcp.tangentMass  = 0.0f;
                    vcp.velocityBias = 0.0f;
                    vc.points.Add(vcp);

                    pc.localPoints[j] = cp.localPoint;
                }
                m_velocityConstraints.Add(vc);
                m_positionConstraints.Add(pc);
            }
        }
Пример #14
0
 public override void Evaluate(out Manifold manifold, Transform xfA, Transform xfB)
 {
     Collision.CollidePolygonAndCircle(out manifold,
                                       (PolygonShape)m_fixtureA.GetShape(), xfA,
                                       (CircleShape)m_fixtureB.GetShape(), xfB);
 }
Пример #15
0
		/// Compute the collision manifold between an edge and a circle.
		public static void CollideEdgeAndPolygon(out Manifold manifold,
									   EdgeShape edgeA, Transform xfA,
									   PolygonShape polygonB, Transform xfB) {
			EPCollider collider = new EPCollider();
			collider.Collide(out manifold, edgeA, xfA, polygonB, xfB);
		}
Пример #16
0
		/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
		/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
		public static void GetPointStates(PointState[/*Settings._maxManifoldPoints*/] state1, PointState[/*Settings._maxManifoldPoints*/] state2,
							  Manifold manifold1, Manifold manifold2) {
			throw new NotImplementedException();
			//for (int i = 0; i < Settings._maxManifoldPoints; ++i)
			//{
			//    state1[i] = _nullState;
			//    state2[i] = _nullState;
			//}

			//// Detect persists and removes.
			//for (int i = 0; i < manifold1.pointCount; ++i)
			//{
			//    ContactID id = manifold1.points[i].id;

			//    state1[i] = _removeState;

			//    for (int j = 0; j < manifold2.pointCount; ++j)
			//    {
			//        if (manifold2.points[j].id.key == id.key)
			//        {
			//            state1[i] = _persistState;
			//            break;
			//        }
			//    }
			//}

			//// Detect persists and adds.
			//for (int i = 0; i < manifold2.pointCount; ++i)
			//{
			//    ContactID id = manifold2.points[i].id;

			//    state2[i] = _addState;

			//    for (int j = 0; j < manifold1.pointCount; ++j)
			//    {
			//        if (manifold1.points[j].id.key == id.key)
			//        {
			//            state2[i] = _persistState;
			//            break;
			//        }
			//    }
			//}
		}
Пример #17
0
		protected Contact(Fixture fA, int indexA, Fixture fB, int indexB) {
			m_flags = ContactFlags.e_enabledFlag;

			m_fixtureA = fA;
			m_fixtureB = fB;

			m_indexA = indexA;
			m_indexB = indexB;

			m_manifold = new Manifold();
			m_manifold.points.Clear();

			m_nodeA.contact = null;
			m_nodeA.other = null;

			m_nodeB.contact = null;
			m_nodeB.other = null;

			m_toiCount = 0;

			m_friction = MixFriction(m_fixtureA.m_friction, m_fixtureB.m_friction);
			m_restitution = MixRestitution(m_fixtureA.m_restitution, m_fixtureB.m_restitution);

			m_tangentSpeed = 0.0f;
		}
Пример #18
0
		// Algorithm:
		// 1. Classify v1 and v2
		// 2. Classify polygon centroid as front or back
		// 3. Flip normal if necessary
		// 4. Initialize normal range to [-pi, pi] about face normal
		// 5. Adjust normal range according to adjacent edges
		// 6. Visit each separating axes, only accept axes within the range
		// 7. Return if _any_ axis indicates separation
		// 8. Clip
		public void Collide(out Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB){
			manifold = new Manifold();
			m_xf = Utilities.MulT(xfA, xfB);
	
			m_centroidB = Utilities.Mul(m_xf, polygonB.m_centroid);
	
			m_v0 = edgeA.m_vertex0;
			m_v1 = edgeA.m_vertex1;
			m_v2 = edgeA.m_vertex2;
			m_v3 = edgeA.m_vertex3;
	
			bool hasVertex0 = edgeA.m_hasVertex0;
			bool hasVertex3 = edgeA.m_hasVertex3;
	
			Vec2 edge1 = m_v2 - m_v1;
			edge1.Normalize();
			m_normal1.Set(edge1.Y, -edge1.X);
			float offset1 = Utilities.Dot(m_normal1, m_centroidB - m_v1);
			float offset0 = 0.0f, offset2 = 0.0f;
			bool convex1 = false, convex2 = false;
	
			// Is there a preceding edge?
			if (hasVertex0)
			{
				Vec2 edge0 = m_v1 - m_v0;
				edge0.Normalize();
				m_normal0.Set(edge0.Y, -edge0.X);
				convex1 = Utilities.Cross(edge0, edge1) >= 0.0f;
				offset0 = Utilities.Dot(m_normal0, m_centroidB - m_v0);
			}
	
			// Is there a following edge?
			if (hasVertex3)
			{
				Vec2 edge2 = m_v3 - m_v2;
				edge2.Normalize();
				m_normal2.Set(edge2.Y, -edge2.X);
				convex2 = Utilities.Cross(edge1, edge2) > 0.0f;
				offset2 = Utilities.Dot(m_normal2, m_centroidB - m_v2);
			}
	
			// Determine front or back collision. Determine collision normal limits.
			if (hasVertex0 && hasVertex3)
			{
				if (convex1 && convex2)
				{
					m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal0;
						m_upperLimit = m_normal2;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = -m_normal1;
					}
				}
				else if (convex1)
				{
					m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal0;
						m_upperLimit = m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal2;
						m_upperLimit = -m_normal1;
					}
				}
				else if (convex2)
				{
					m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = m_normal2;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = -m_normal0;
					}
				}
				else
				{
					m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal2;
						m_upperLimit = -m_normal0;
					}
				}
			}
			else if (hasVertex0)
			{
				if (convex1)
				{
					m_front = offset0 >= 0.0f || offset1 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal0;
						m_upperLimit = -m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = -m_normal1;
					}
				}
				else
				{
					m_front = offset0 >= 0.0f && offset1 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = -m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = -m_normal0;
					}
				}
			}
			else if (hasVertex3)
			{
				if (convex2)
				{
					m_front = offset1 >= 0.0f || offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = m_normal2;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = m_normal1;
					}
				}
				else
				{
					m_front = offset1 >= 0.0f && offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal2;
						m_upperLimit = m_normal1;
					}
				}		
			}
			else
			{
				m_front = offset1 >= 0.0f;
				if (m_front)
				{
					m_normal = m_normal1;
					m_lowerLimit = -m_normal1;
					m_upperLimit = -m_normal1;
				}
				else
				{
					m_normal = -m_normal1;
					m_lowerLimit = m_normal1;
					m_upperLimit = m_normal1;
				}
			}
	
			// Get polygonB in frameA
			m_polygonB.count = polygonB.m_count;
			for (int i = 0; i < polygonB.m_count; ++i)
			{
				m_polygonB.vertices[i] = Utilities.Mul(m_xf, polygonB.m_vertices[i]);
				m_polygonB.normals[i] = Utilities.Mul(m_xf.q, polygonB.m_normals[i]);
			}
	
			m_radius = 2.0f * Settings._polygonRadius;

			manifold.points.Clear();
	
			EPAxis edgeAxis = ComputeEdgeSeparation();
	
			// If no valid normal can be found than this edge should not collide.
			if (edgeAxis.type == EPAxisType.e_unknown)
			{
				return;
			}
	
			if (edgeAxis.separation > m_radius)
			{
				return;
			}
	
			EPAxis polygonAxis = ComputePolygonSeparation();
			if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > m_radius)
			{
				return;
			}
	
			// Use hysteresis for jitter reduction.
			const float k_relativeTol = 0.98f;
			const float k_absoluteTol = 0.001f;
	
			EPAxis primaryAxis;
			if (polygonAxis.type == EPAxisType.e_unknown)
			{
				primaryAxis = edgeAxis;
			}
			else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
			{
				primaryAxis = polygonAxis;
			}
			else
			{
				primaryAxis = edgeAxis;
			}
	
			ClipVertex[] ie = new ClipVertex[2];
			ReferenceFace rf = new ReferenceFace();
			if (primaryAxis.type == EPAxisType.e_edgeA)
			{
				manifold.type = Manifold.ManifoldType.e_faceA;
		
				// Search for the polygon normal that is most anti-parallel to the edge normal.
				int bestIndex = 0;
				float bestValue = Utilities.Dot(m_normal, m_polygonB.normals[0]);
				for (int i = 1; i < m_polygonB.count; ++i)
				{
					float value = Utilities.Dot(m_normal, m_polygonB.normals[i]);
					if (value < bestValue)
					{
						bestValue = value;
						bestIndex = i;
					}
				}
		
				int i1 = bestIndex;
				int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
		
				ie[0].v = m_polygonB.vertices[i1];
				ie[0].id.cf.indexA = 0;
				ie[0].id.cf.indexB = (byte)i1;
				ie[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
				ie[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
		
				ie[1].v = m_polygonB.vertices[i2];
				ie[1].id.cf.indexA = 0;
				ie[1].id.cf.indexB = (byte)i2;
				ie[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
				ie[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
		
				if (m_front)
				{
					rf.i1 = 0;
					rf.i2 = 1;
					rf.v1 = m_v1;
					rf.v2 = m_v2;
					rf.normal = m_normal1;
				}
				else
				{
					rf.i1 = 1;
					rf.i2 = 0;
					rf.v1 = m_v2;
					rf.v2 = m_v1;
					rf.normal = -m_normal1;
				}		
			}
			else
			{
				manifold.type = Manifold.ManifoldType.e_faceB;
		
				ie[0].v = m_v1;
				ie[0].id.cf.indexA = 0;
				ie[0].id.cf.indexB = (byte)primaryAxis.index;
				ie[0].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
				ie[0].id.cf.typeB = ContactFeature.FeatureType.e_face;
		
				ie[1].v = m_v2;
				ie[1].id.cf.indexA = 0;
				ie[1].id.cf.indexB = (byte)primaryAxis.index;		
				ie[1].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
				ie[1].id.cf.typeB = ContactFeature.FeatureType.e_face;
		
				rf.i1 = primaryAxis.index;
				rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
				rf.v1 = m_polygonB.vertices[rf.i1];
				rf.v2 = m_polygonB.vertices[rf.i2];
				rf.normal = m_polygonB.normals[rf.i1];
			}
	
			rf.sideNormal1.Set(rf.normal.Y, -rf.normal.X);
			rf.sideNormal2 = -rf.sideNormal1;
			rf.sideOffset1 = Utilities.Dot(rf.sideNormal1, rf.v1);
			rf.sideOffset2 = Utilities.Dot(rf.sideNormal2, rf.v2);
	
			// Clip incident edge against extruded edge1 side edges.
			ClipVertex[] clipPoints1 = new ClipVertex[2];
			ClipVertex[] clipPoints2 = new ClipVertex[2];
			int np;
	
			// Clip to box side 1
			np = Collision.ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
	
			if (np < Settings._maxManifoldPoints)
			{
				return;
			}
	
			// Clip to negative box side 1
			np = Collision.ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
	
			if (np < Settings._maxManifoldPoints)
			{
				return;
			}
	
			// Now clipPoints2 contains the clipped points.
			if (primaryAxis.type == EPAxisType.e_edgeA)
			{
				manifold.localNormal = rf.normal;
				manifold.localPoint = rf.v1;
			}
			else
			{
				manifold.localNormal = polygonB.m_normals[rf.i1];
				manifold.localPoint = polygonB.m_vertices[rf.i1];
			}

			manifold.points.Clear();
			for (int i = 0; i < Settings._maxManifoldPoints; ++i)
			{
				float separation;
		
				separation = Utilities.Dot(rf.normal, clipPoints2[i].v - rf.v1);
		
				if (separation <= m_radius)
				{
					ManifoldPoint cp = new ManifoldPoint();
			
					if (primaryAxis.type == EPAxisType.e_edgeA)
					{
						cp.localPoint = Utilities.MulT(m_xf, clipPoints2[i].v);
						cp.id = clipPoints2[i].id;
					}
					else
					{
						cp.localPoint = clipPoints2[i].v;
						cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
						cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
						cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
						cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
					}

					manifold.points.Add(cp);
				}
			}
		}
Пример #19
0
		public override void Evaluate(out Manifold manifold, Transform xfA, Transform xfB) {
			Collision.CollideEdgeAndCircle(out manifold,
										(EdgeShape)m_fixtureA.GetShape(), xfA,
										(CircleShape)m_fixtureB.GetShape(), xfB);
		}
Пример #20
0
		/// This is called after a contact is updated. This allows you to inspect a
		/// contact before it goes to the solver. If you are careful, you can modify the
		/// contact manifold (e.g. disable contact).
		/// A copy of the old manifold is provided so that you can detect changes.
		/// Note: this is called only for awake bodies.
		/// Note: this is called even when the number of contact points is zero.
		/// Note: this is not called for sensors.
		/// Note: if you set the number of contact points to zero, you will not
		/// get an EndContact callback. However, you may get a BeginContact callback
		/// the next step.
		public virtual void PreSolve(Contact contact, Manifold oldManifold)
		{
		}
Пример #21
0
		public override void Evaluate(out Manifold manifold, Transform xfA, Transform xfB) {
			Collision.CollidePolygons( out manifold,
								(PolygonShape)m_fixtureA.GetShape(), xfA,
								(PolygonShape)m_fixtureB.GetShape(), xfB);
		}