Exemplo n.º 1
0
		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();
		}
Exemplo n.º 2
0
		/// Construct a world object.
		/// @param gravity the world gravity vector.
		public World(Vec2 gravity){
			m_destructionListener = null;
			m_debugDraw = null;

			m_bodyList = new List<Body>();
			m_jointList = new List<Joint>();

			m_warmStarting = true;
			m_continuousPhysics = true;
			m_subStepping = false;

			m_stepComplete = true;

			m_allowSleep = true;
			m_gravity = gravity;

			m_flags = WorldFlags.e_clearForces;

			m_inv_dt0 = 0.0f;

			m_profile = new Profile();
			m_contactManager = new ContactManager();
		}
Exemplo n.º 3
0
		public virtual void Step(TestSettings settings) {
			float timeStep = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;

			if (settings.pause)
			{
				if (settings.singleStep)
				{
					settings.singleStep = false;
				}
				else
				{
					timeStep = 0.0f;
				}

				m_debugDraw.DrawString("****PAUSED****");
			}

			Draw.DrawFlags flags = 0;
			flags |= settings.drawShapes ? Draw.DrawFlags.e_shapeBit : 0;
			flags |= settings.drawJoints ? Draw.DrawFlags.e_jointBit : 0;
			flags |= settings.drawAABBs  ? Draw.DrawFlags.e_aabbBit  : 0;
			flags |= settings.drawCOMs   ? Draw.DrawFlags.e_centerOfMassBit : 0;
			m_debugDraw.SetFlags(flags);

			m_world.SetAllowSleeping(settings.enableSleep);
			m_world.SetWarmStarting(settings.enableWarmStarting);
			m_world.SetContinuousPhysics(settings.enableContinuous);
			m_world.SetSubStepping(settings.enableSubStepping);

			m_pointCount = 0;

			m_world.Step(timeStep, settings.velocityIterations, settings.positionIterations);

			m_world.DrawDebugData();

			if (timeStep > 0.0f)
			{
				++m_stepCount;
			}

			if (settings.drawStats)
			{
				int bodyCount = m_world.GetBodyCount();
				int contactCount = m_world.GetContactCount();
				int jointCount = m_world.GetJointCount();
				m_debugDraw.DrawString("bodies/contacts/joints = {0}/{1}/{2}", bodyCount, contactCount, jointCount);

				int proxyCount = m_world.GetProxyCount();
				int height = m_world.GetTreeHeight();
				int balance = m_world.GetTreeBalance();
				float quality = m_world.GetTreeQuality();
				m_debugDraw.DrawString("proxies/height/balance/quality = {0}/{1}/{2}/{3}", proxyCount, height, balance, quality);
			}

			// Track maximum profile times
			{
				Profile p = m_world.GetProfile();
				m_maxProfile.step = Math.Max(m_maxProfile.step, p.step);
				m_maxProfile.collide = Math.Max(m_maxProfile.collide, p.collide);
				m_maxProfile.solve = Math.Max(m_maxProfile.solve, p.solve);
				m_maxProfile.solveInit = Math.Max(m_maxProfile.solveInit, p.solveInit);
				m_maxProfile.solveVelocity = Math.Max(m_maxProfile.solveVelocity, p.solveVelocity);
				m_maxProfile.solvePosition = Math.Max(m_maxProfile.solvePosition, p.solvePosition);
				m_maxProfile.solveTOI = Math.Max(m_maxProfile.solveTOI, p.solveTOI);
				m_maxProfile.broadphase = Math.Max(m_maxProfile.broadphase, p.broadphase);

				m_totalProfile.step += p.step;
				m_totalProfile.collide += p.collide;
				m_totalProfile.solve += p.solve;
				m_totalProfile.solveInit += p.solveInit;
				m_totalProfile.solveVelocity += p.solveVelocity;
				m_totalProfile.solvePosition += p.solvePosition;
				m_totalProfile.solveTOI += p.solveTOI;
				m_totalProfile.broadphase += p.broadphase;
			}

			if (settings.drawProfile)
			{
				Profile p = m_world.GetProfile();

				Profile aveProfile = new Profile();
				if (m_stepCount > 0)
				{
					float scale = 1.0f / m_stepCount;
					aveProfile.step = scale * m_totalProfile.step;
					aveProfile.collide = scale * m_totalProfile.collide;
					aveProfile.solve = scale * m_totalProfile.solve;
					aveProfile.solveInit = scale * m_totalProfile.solveInit;
					aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
					aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
					aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
					aveProfile.broadphase = scale * m_totalProfile.broadphase;
				}

				m_debugDraw.DrawString("step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step);
				m_debugDraw.DrawString("collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide);
				m_debugDraw.DrawString("solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve);
				m_debugDraw.DrawString("solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit);
				m_debugDraw.DrawString("solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity);
				m_debugDraw.DrawString("solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition);
				m_debugDraw.DrawString("solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI);
				m_debugDraw.DrawString("broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase);
			}

			if (m_mouseJoint != null)
			{
				Vec2 p1 = m_mouseJoint.GetAnchorB();
				Vec2 p2 = m_mouseJoint.GetTarget();

				Color c = Color.FromArgb(0, 255, 0);
				m_debugDraw.DrawPoint(p1, 4.0f, c);
				m_debugDraw.DrawPoint(p2, 4.0f, c);

				c = Color.FromArgb(204, 204, 204);
				m_debugDraw.DrawSegment(p1, p2, c);
			}
	
			if (m_bombSpawning)
			{
				Color c = Color.FromArgb(0, 0, 255);
				m_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);

				c = Color.FromArgb(200, 200, 200);
				m_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
			}

			if (settings.drawContactPoints)
			{
				const float k_impulseScale = 0.1f;
				const float k_axisScale = 0.3f;

				for (int i = 0; i < m_pointCount; ++i)
				{
					ContactPoint point = m_points[i];

					if (point.state == PointState._addState)
					{
						// Add
						m_debugDraw.DrawPoint(point.position, 10.0f, Color.FromArgb(75, 242, 75));
					}
					else if (point.state == PointState._persistState)
					{
						// Persist
						m_debugDraw.DrawPoint(point.position, 5.0f, Color.FromArgb(75, 75, 242));
					}

					if (settings.drawContactNormals)
					{
						Vec2 p1 = point.position;
						Vec2 p2 = p1 + k_axisScale * point.normal;
						m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 242));
					}
					else if (settings.drawContactImpulse)
					{
						Vec2 p1 = point.position;
						Vec2 p2 = p1 + k_impulseScale * point.normalImpulse * point.normal;
						m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 75));
					}

					if (settings.drawFrictionImpulse)
					{
						Vec2 tangent = Utilities.Cross(point.normal, 1.0f);
						Vec2 p1 = point.position;
						Vec2 p2 = p1 + k_impulseScale * point.tangentImpulse * tangent;
						m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 75));
					}
				}
			}
		}
Exemplo n.º 4
0
		private void Solve(TimeStep step){
			m_profile.solveInit = 0.0f;
			m_profile.solveVelocity = 0.0f;
			m_profile.solvePosition = 0.0f;

			// Size the island for the worst case.
			Island island = new Island(m_contactManager.m_contactListener);

			// Clear all the island flags.
			foreach (Body b in m_bodyList)
			{
			    b.m_flags &= ~Body.BodyFlags.e_islandFlag;
			}
			foreach (Contact c in m_contactManager.m_contactList)
			{
			    c.m_flags &= ~ContactFlags.e_islandFlag;
			}
			foreach (Joint j in m_jointList)
			{
			    j.m_islandFlag = false;
			}

			// Build and simulate all awake islands.
			List<Body> stack = new List<Body>(m_bodyList.Count());
			foreach (Body seed in m_bodyList)
			{
			    if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
			    {
			        continue;
			    }

			    if (seed.IsAwake() == false || seed.IsActive() == false)
			    {
			        continue;
			    }

			    // The seed can be dynamic or kinematic.
			    if (seed.GetBodyType() == BodyType._staticBody)
			    {
			        continue;
			    }

			    // Reset island and stack.
			    island.Clear();
			    int stackCount = 0;
				stack.Add(seed); stackCount++;
			    seed.m_flags |= Body.BodyFlags.e_islandFlag;

			    // Perform a depth first search (DFS) on the constraint graph.
			    while (stackCount > 0)
			    {
			        // Grab the next body off the stack and add it to the island.
			        Body b = stack[--stackCount];
			        Utilities.Assert(b.IsActive() == true);
			        island.Add(b);

			        // Make sure the body is awake.
			        b.SetAwake(true);

			        // To keep islands as small as possible, we don't
			        // propagate islands across static bodies.
			        if (b.GetBodyType() == BodyType._staticBody)
			        {
			            continue;
			        }

			        // Search all contacts connected to this body.
			        foreach (ContactEdge ce in b.m_contactList)
			        {
			            Contact contact = ce.contact;

			            // Has this contact already been added to an island?
			            if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
			            {
			                continue;
			            }

			            // Is this contact solid and touching?
			            if (contact.IsEnabled() == false ||
			                contact.IsTouching() == false)
			            {
			                continue;
			            }

			            // Skip sensors.
			            bool sensorA = contact.m_fixtureA.m_isSensor;
			            bool sensorB = contact.m_fixtureB.m_isSensor;
			            if (sensorA || sensorB)
			            {
			                continue;
			            }

			            island.Add(contact);
			            contact.m_flags |= ContactFlags.e_islandFlag;

			            Body other = ce.other;

			            // Was the other body already added to this island?
			            if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
			            {
			                continue;
			            }

			            Utilities.Assert(stackCount < m_bodyList.Count());
						stack.Add(other); stackCount++;
			            other.m_flags |= Body.BodyFlags.e_islandFlag;
			        }

					// Search all joints connect to this body.
					foreach (JointEdge je in b.m_jointList){
						if (je.joint.m_islandFlag == true) {
							continue;
						}

						Body other = je.other;

						// Don't simulate joints connected to inactive bodies.
						if (other.IsActive() == false) {
							continue;
						}

						island.Add(je.joint);
						je.joint.m_islandFlag = true;

						if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) {
							continue;
						}

						stack.Add(other); stackCount++;
						other.m_flags |= Body.BodyFlags.e_islandFlag;
					}
			    }

			    Profile profile = new Profile();
				island.Solve(profile, step, m_gravity, m_allowSleep);
				m_profile.solveInit += profile.solveInit;
				m_profile.solveVelocity += profile.solveVelocity;
				m_profile.solvePosition += profile.solvePosition;

				// Post solve cleanup.
				for (int i = 0; i < island.m_bodies.Count(); ++i) {
					// Allow static bodies to participate in other islands.
					Body b = island.m_bodies[i];
					if (b.GetBodyType() == BodyType._staticBody) {
						b.m_flags &= ~Body.BodyFlags.e_islandFlag;
					}
				}
			}

			{
			    Timer timer = new Timer();
			    // Synchronize fixtures, check for out of range bodies.
			    foreach (Body b in m_bodyList)
			    {
			        // If a body was not in an island then it did not move.
			        if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
			        {
			            continue;
			        }

			        if (b.GetBodyType() == BodyType._staticBody)
			        {
			            continue;
			        }

			        // Update fixtures (for broad-phase).
			        b.SynchronizeFixtures();
			    }

			    // Look for new contacts.
			    m_contactManager.FindNewContacts();
			    m_profile.broadphase = timer.GetMilliseconds();
			}
		}
Exemplo n.º 5
0
		public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) {
			Timer timer = new Timer();

			float h = step.dt;

			// Integrate velocities and apply damping. Initialize the body state.
			for (int i = 0; i < m_bodies.Count(); i++)
			{
				Body b = m_bodies[i];
			    Vec2 c = b.m_sweep.c;
			    float a = b.m_sweep.a;
			    Vec2 v = b.m_linearVelocity;
			    float w = b.m_angularVelocity;

			    // Store positions for continuous collision.
			    b.m_sweep.c0 = b.m_sweep.c;
			    b.m_sweep.a0 = b.m_sweep.a;

			    if (b.m_type == BodyType._dynamicBody)
			    {
			        // Integrate velocities.
			        v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
			        w += h * b.m_invI * b.m_torque;

			        // Apply damping.
			        // ODE: dv/dt + c * v = 0
			        // Solution: v(t) = v0 * exp(-c * t)
			        // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
			        // v2 = exp(-c * dt) * v1
			        // Taylor expansion:
			        // v2 = (1.0f - c * dt) * v1
			        v *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
					w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
			    }

				Position pos = new Position();
				pos.c = c;
				pos.a = a;
				m_positions.Add(pos);

				Velocity vel = new Velocity();
				vel.v = v;
				vel.w = w;
				m_velocities.Add(vel);
			}

			timer.Reset();

			// Solver data
			SolverData solverData;
			solverData.step = step;
			solverData.positions = m_positions;
			solverData.velocities = m_velocities;

			// Initialize velocity constraints.
			ContactSolverDef contactSolverDef;
			contactSolverDef.step = step;
			contactSolverDef.contacts = m_contacts;
			contactSolverDef.positions = m_positions;
			contactSolverDef.velocities = m_velocities;

			ContactSolver contactSolver = new ContactSolver(contactSolverDef);
			contactSolver.InitializeVelocityConstraints();

			if (step.warmStarting)
			{
			    contactSolver.WarmStart();
			}
	
			for (int i = 0; i < m_joints.Count(); ++i)
			{
			    m_joints[i].InitVelocityConstraints(solverData);
			}

			profile.solveInit = timer.GetMilliseconds();

			// Solve velocity constraints
			timer.Reset();
			for (int i = 0; i < step.velocityIterations; ++i)
			{
			    for (int j = 0; j < m_joints.Count(); ++j)
			    {
			        m_joints[j].SolveVelocityConstraints(solverData);
			    }

			    contactSolver.SolveVelocityConstraints();
			}

			// Store impulses for warm starting
			contactSolver.StoreImpulses();
			profile.solveVelocity = timer.GetMilliseconds();

			// Integrate positions
			for (int i = 0; i < m_bodies.Count(); ++i)
			{
			    Vec2 c = m_positions[i].c;
			    float a = m_positions[i].a;
			    Vec2 v = m_velocities[i].v;
			    float w = m_velocities[i].w;

			    // Check for large velocities
			    Vec2 translation = h * v;
			    if (Utilities.Dot(translation, translation) > Settings._maxTranslationSquared)
			    {
					float ratio = Settings._maxTranslation / translation.Length();
			        v *= ratio;
			    }

			    float rotation = h * w;
				if (rotation * rotation > Settings._maxRotationSquared)
			    {
					float ratio = Settings._maxRotation / Math.Abs(rotation);
			        w *= ratio;
			    }

			    // Integrate
			    c += h * v;
			    a += h * w;

				m_positions[i].c = c;
				m_positions[i].a = a;
				m_velocities[i].v = v;
				m_velocities[i].w = w;
			}

			// Solve position constraints
			timer.Reset();
			bool positionSolved = false;
			for (int i = 0; i < step.positionIterations; ++i)
			{
			    bool contactsOkay = contactSolver.SolvePositionConstraints();

			    bool jointsOkay = true;
			    for (int j = 0; j < m_joints.Count; ++j)
			    {
			        bool jointOkay = m_joints[j].SolvePositionConstraints(solverData);
			        jointsOkay = jointsOkay && jointOkay;
			    }

			    if (contactsOkay && jointsOkay)
			    {
			        // Exit early if the position errors are small.
			        positionSolved = true;
			        break;
			    }
			}

			// Copy state buffers back to the bodies
			for (int i = 0; i < m_bodies.Count(); ++i)
			{
			    Body body = m_bodies[i];
			    body.m_sweep.c = m_positions[i].c;
			    body.m_sweep.a = m_positions[i].a;
			    body.m_linearVelocity = m_velocities[i].v;
			    body.m_angularVelocity = m_velocities[i].w;
			    body.SynchronizeTransform();
			}

			profile.solvePosition = timer.GetMilliseconds();

			Report(contactSolver.m_velocityConstraints);

			if (allowSleep)
			{
			    float minSleepTime = Single.MaxValue;

				const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
				const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;

			    for (int i = 0; i < m_bodies.Count(); ++i)
			    {
			        Body b = m_bodies[i];
			        if (b.GetBodyType() == BodyType._staticBody)
			        {
			            continue;
			        }

			        if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
			            b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
			            Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
			        {
			            b.m_sleepTime = 0.0f;
			            minSleepTime = 0.0f;
			        }
			        else
			        {
			            b.m_sleepTime += h;
			            minSleepTime = Math.Min(minSleepTime, b.m_sleepTime);
			        }
			    }

				if (minSleepTime >= Settings._timeToSleep && positionSolved)
			    {
			        for (int i = 0; i < m_bodies.Count(); ++i)
			        {
			            Body b = m_bodies[i];
			            b.SetAwake(false);
			        }
			    }
			}
		}