Esempio n. 1
0
		private void SolveTOI(TimeStep step){
			Island island = new Island(m_contactManager.m_contactListener);

			if (m_stepComplete)
			{
			    foreach (Body b in m_bodyList)
			    {
			        b.m_flags &= ~Body.BodyFlags.e_islandFlag;
			        b.m_sweep.alpha0 = 0.0f;
			    }

			    foreach (Contact c in m_contactManager.m_contactList)
			    {
			        // Invalidate TOI
			        c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
			        c.m_toiCount = 0;
			        c.m_toi = 1.0f;
			    }
			}

			Fixture fA = null;
			Fixture fB = null;
			Body bA = null;
			Body bB = null;

			// Find TOI events and solve them.
			for (;;)
			{
			    // Find the first TOI.
			    Contact minContact = null;
			    float minAlpha = 1.0f;

			    foreach (Contact c in m_contactManager.m_contactList)
			    {
			        // Is this contact disabled?
			        if (c.IsEnabled() == false)
			        {
			            continue;
			        }

			        // Prevent excessive sub-stepping.
			        if (c.m_toiCount > Settings._maxSubSteps)
			        {
			            continue;
			        }

					

			        float alpha = 1.0f;
			        if (c.m_flags.HasFlag(ContactFlags.e_toiFlag))
			        {
			            // This contact has a valid cached TOI.
			            alpha = c.m_toi;
			        }
			        else
			        {
			            fA = c.FixtureA;
			            fB = c.FixtureB;

			            // Is there a sensor?
			            if (fA.IsSensor || fB.IsSensor)
			            {
			                continue;
			            }

			            bA = fA.GetBody();
			            bB = fB.GetBody();

			            BodyType typeA = bA.m_type;
			            BodyType typeB = bB.m_type;
			            Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody);

			            bool activeA = bA.IsAwake() && typeA != BodyType._staticBody;
			            bool activeB = bB.IsAwake() && typeB != BodyType._staticBody;

			            // Is at least one body active (awake and dynamic or kinematic)?
			            if (activeA == false && activeB == false)
			            {
			                continue;
			            }

			            bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody;
			            bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody;

			            // Are these two non-bullet dynamic bodies?
			            if (collideA == false && collideB == false)
			            {
			                continue;
			            }

			            // Compute the TOI for this contact.
			            // Put the sweeps onto the same time interval.
			            float alpha0 = bA.m_sweep.alpha0;

			            if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
			            {
			                alpha0 = bB.m_sweep.alpha0;
			                bA.m_sweep.Advance(alpha0);
			            }
			            else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
			            {
			                alpha0 = bA.m_sweep.alpha0;
			                bB.m_sweep.Advance(alpha0);
			            }

			            Utilities.Assert(alpha0 < 1.0f);

			            int indexA = c.GetChildIndexA();
			            int indexB = c.GetChildIndexB();

			            // Compute the time of impact in interval [0, minTOI]
			            TOIInput input = new TOIInput();
			            input.proxyA.Set(fA.GetShape(), indexA);
			            input.proxyB.Set(fB.GetShape(), indexB);
			            input.sweepA = bA.m_sweep;
			            input.sweepB = bB.m_sweep;
			            input.tMax = 1.0f;

			            TOIOutput output;
			            Utilities.TimeOfImpact(out output, input);

			            // Beta is the fraction of the remaining portion of the .
			            float beta = output.t;
			            if (output.state == TOIOutput.State.e_touching)
			            {
			                alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
			            }
			            else
			            {
			                alpha = 1.0f;
			            }

			            c.m_toi = alpha;
			            c.m_flags |= ContactFlags.e_toiFlag;
			        }

			        if (alpha < minAlpha)
			        {
			            // This is the minimum TOI found so far.
			            minContact = c;
			            minAlpha = alpha;
			        }
			    }

			    if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha)
			    {
			        // No more TOI events. Done!
			        m_stepComplete = true;
			        break;
			    }

			    // Advance the bodies to the TOI.
			    fA = minContact.FixtureA;
			    fB = minContact.FixtureB;
			    bA = fA.GetBody();
			    bB = fB.GetBody();

			    Sweep backup1 = bA.m_sweep;
			    Sweep backup2 = bB.m_sweep;

			    bA.Advance(minAlpha);
			    bB.Advance(minAlpha);

			    // The TOI contact likely has some new contact points.
			    minContact.Update(m_contactManager.m_contactListener);
			    minContact.m_flags &= ~ContactFlags.e_toiFlag;
			    ++minContact.m_toiCount;

			    // Is the contact solid?
			    if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
			    {
			        // Restore the sweeps.
			        minContact.SetEnabled(false);
			        bA.m_sweep = backup1;
			        bB.m_sweep = backup2;
			        bA.SynchronizeTransform();
			        bB.SynchronizeTransform();
			        continue;
			    }

			    bA.SetAwake(true);
			    bB.SetAwake(true);

			    // Build the island
			    island.Clear();
			    island.Add(bA);
			    island.Add(bB);
			    island.Add(minContact);

			    bA.m_flags |= Body.BodyFlags.e_islandFlag;
			    bB.m_flags |= Body.BodyFlags.e_islandFlag;
			    minContact.m_flags |= ContactFlags.e_islandFlag;

			    // Get contacts on bodyA and bodyB.
			    Body[] bodies = {bA, bB};
			    for (int i = 0; i < 2; ++i)
			    {
			        Body body = bodies[i];
			        if (body.m_type == BodyType._dynamicBody)
			        {
						foreach (ContactEdge ce in body.m_contactList)
			            {
							throw new NotImplementedException();

							//if (island.m_bodies.Count() == island.m_bodyCapacity)
							//{
							//    break;
							//}

							//if (island.m_bodies.Count() == island.m_contactCapacity)
							//{
							//    break;
							//}

							//Contact* contact = ce.contact;

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

							//// Only add static, kinematic, or bullet bodies.
							//Body* other = ce.other;
							//if (other.m_type == _dynamicBody &&
							//    body.IsBullet() == false && other.IsBullet() == false)
							//{
							//    continue;
							//}

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

							//// Tentatively advance the body to the TOI.
							//Sweep backup = other.m_sweep;
							//if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0)
							//{
							//    other.Advance(minAlpha);
							//}

							//// Update the contact points
							//contact.Update(m_contactManager.m_contactListener);

							//// Was the contact disabled by the user?
							//if (contact.IsEnabled() == false)
							//{
							//    other.m_sweep = backup;
							//    other.SynchronizeTransform();
							//    continue;
							//}

							//// Are there contact points?
							//if (contact.IsTouching() == false)
							//{
							//    other.m_sweep = backup;
							//    other.SynchronizeTransform();
							//    continue;
							//}

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

							//// Has the other body already been added to the island?
							//if (other.m_flags & Body.BodyFlags.e_islandFlag)
							//{
							//    continue;
							//}
					
							//// Add the other body to the island.
							//other.m_flags |= Body.BodyFlags.e_islandFlag;

							//if (other.m_type != _staticBody)
							//{
							//    other.SetAwake(true);
							//}

							//island.Add(other);
			            }
			        }
			    }

			    TimeStep subStep;
			    subStep.dt = (1.0f - minAlpha) * step.dt;
			    subStep.inv_dt = 1.0f / subStep.dt;
			    subStep.dtRatio = 1.0f;
			    subStep.positionIterations = 20;
			    subStep.velocityIterations = step.velocityIterations;
			    subStep.warmStarting = false;
			    island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);

			    // Reset island flags and synchronize broad-phase proxies.
			    for (int i = 0; i < island.m_bodies.Count(); ++i)
			    {
					throw new NotImplementedException();
					//Body* body = island.m_bodies[i];
					//body.m_flags &= ~Body.BodyFlags.e_islandFlag;

					//if (body.m_type != _dynamicBody)
					//{
					//    continue;
					//}

					//body.SynchronizeFixtures();

					//// Invalidate all contact TOIs on this displaced body.
					//for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next)
					//{
					//    ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
					//}
			    }

			    // Commit fixture proxy movements to the broad-phase so that new contacts are created.
			    // Also, some contacts can be destroyed.
			    m_contactManager.FindNewContacts();

			    if (m_subStepping)
			    {
			        m_stepComplete = false;
			        break;
			    }
			}
		}
Esempio n. 2
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();
            }
        }
Esempio n. 3
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();
			}
		}
Esempio n. 4
0
        private void SolveTOI(TimeStep step)
        {
            Island island = new Island(m_contactManager.m_contactListener);

            if (m_stepComplete)
            {
                foreach (Body b in m_bodyList)
                {
                    b.m_flags       &= ~Body.BodyFlags.e_islandFlag;
                    b.m_sweep.alpha0 = 0.0f;
                }

                foreach (Contact c in m_contactManager.m_contactList)
                {
                    // Invalidate TOI
                    c.m_flags   &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
                    c.m_toiCount = 0;
                    c.m_toi      = 1.0f;
                }
            }

            Fixture fA = null;
            Fixture fB = null;
            Body    bA = null;
            Body    bB = null;

            // Find TOI events and solve them.
            for (;;)
            {
                // Find the first TOI.
                Contact minContact = null;
                float   minAlpha   = 1.0f;

                foreach (Contact c in m_contactManager.m_contactList)
                {
                    // Is this contact disabled?
                    if (c.IsEnabled() == false)
                    {
                        continue;
                    }

                    // Prevent excessive sub-stepping.
                    if (c.m_toiCount > Settings._maxSubSteps)
                    {
                        continue;
                    }



                    float alpha = 1.0f;
                    if (c.m_flags.HasFlag(ContactFlags.e_toiFlag))
                    {
                        // This contact has a valid cached TOI.
                        alpha = c.m_toi;
                    }
                    else
                    {
                        fA = c.FixtureA;
                        fB = c.FixtureB;

                        // Is there a sensor?
                        if (fA.IsSensor || fB.IsSensor)
                        {
                            continue;
                        }

                        bA = fA.GetBody();
                        bB = fB.GetBody();

                        BodyType typeA = bA.m_type;
                        BodyType typeB = bB.m_type;
                        Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody);

                        bool activeA = bA.IsAwake() && typeA != BodyType._staticBody;
                        bool activeB = bB.IsAwake() && typeB != BodyType._staticBody;

                        // Is at least one body active (awake and dynamic or kinematic)?
                        if (activeA == false && activeB == false)
                        {
                            continue;
                        }

                        bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody;
                        bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody;

                        // Are these two non-bullet dynamic bodies?
                        if (collideA == false && collideB == false)
                        {
                            continue;
                        }

                        // Compute the TOI for this contact.
                        // Put the sweeps onto the same time interval.
                        float alpha0 = bA.m_sweep.alpha0;

                        if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
                        {
                            alpha0 = bB.m_sweep.alpha0;
                            bA.m_sweep.Advance(alpha0);
                        }
                        else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
                        {
                            alpha0 = bA.m_sweep.alpha0;
                            bB.m_sweep.Advance(alpha0);
                        }

                        Utilities.Assert(alpha0 < 1.0f);

                        int indexA = c.GetChildIndexA();
                        int indexB = c.GetChildIndexB();

                        // Compute the time of impact in interval [0, minTOI]
                        TOIInput input = new TOIInput();
                        input.proxyA.Set(fA.GetShape(), indexA);
                        input.proxyB.Set(fB.GetShape(), indexB);
                        input.sweepA = bA.m_sweep;
                        input.sweepB = bB.m_sweep;
                        input.tMax   = 1.0f;

                        TOIOutput output;
                        Utilities.TimeOfImpact(out output, input);

                        // Beta is the fraction of the remaining portion of the .
                        float beta = output.t;
                        if (output.state == TOIOutput.State.e_touching)
                        {
                            alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                        }
                        else
                        {
                            alpha = 1.0f;
                        }

                        c.m_toi    = alpha;
                        c.m_flags |= ContactFlags.e_toiFlag;
                    }

                    if (alpha < minAlpha)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minAlpha   = alpha;
                    }
                }

                if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha)
                {
                    // No more TOI events. Done!
                    m_stepComplete = true;
                    break;
                }

                // Advance the bodies to the TOI.
                fA = minContact.FixtureA;
                fB = minContact.FixtureB;
                bA = fA.GetBody();
                bB = fB.GetBody();

                Sweep backup1 = bA.m_sweep;
                Sweep backup2 = bB.m_sweep;

                bA.Advance(minAlpha);
                bB.Advance(minAlpha);

                // The TOI contact likely has some new contact points.
                minContact.Update(m_contactManager.m_contactListener);
                minContact.m_flags &= ~ContactFlags.e_toiFlag;
                ++minContact.m_toiCount;

                // Is the contact solid?
                if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
                {
                    // Restore the sweeps.
                    minContact.SetEnabled(false);
                    bA.m_sweep = backup1;
                    bB.m_sweep = backup2;
                    bA.SynchronizeTransform();
                    bB.SynchronizeTransform();
                    continue;
                }

                bA.SetAwake(true);
                bB.SetAwake(true);

                // Build the island
                island.Clear();
                island.Add(bA);
                island.Add(bB);
                island.Add(minContact);

                bA.m_flags         |= Body.BodyFlags.e_islandFlag;
                bB.m_flags         |= Body.BodyFlags.e_islandFlag;
                minContact.m_flags |= ContactFlags.e_islandFlag;

                // Get contacts on bodyA and bodyB.
                Body[] bodies = { bA, bB };
                for (int i = 0; i < 2; ++i)
                {
                    Body body = bodies[i];
                    if (body.m_type == BodyType._dynamicBody)
                    {
                        foreach (ContactEdge ce in body.m_contactList)
                        {
                            throw new NotImplementedException();

                            //if (island.m_bodies.Count() == island.m_bodyCapacity)
                            //{
                            //    break;
                            //}

                            //if (island.m_bodies.Count() == island.m_contactCapacity)
                            //{
                            //    break;
                            //}

                            //Contact* contact = ce.contact;

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

                            //// Only add static, kinematic, or bullet bodies.
                            //Body* other = ce.other;
                            //if (other.m_type == _dynamicBody &&
                            //    body.IsBullet() == false && other.IsBullet() == false)
                            //{
                            //    continue;
                            //}

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

                            //// Tentatively advance the body to the TOI.
                            //Sweep backup = other.m_sweep;
                            //if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0)
                            //{
                            //    other.Advance(minAlpha);
                            //}

                            //// Update the contact points
                            //contact.Update(m_contactManager.m_contactListener);

                            //// Was the contact disabled by the user?
                            //if (contact.IsEnabled() == false)
                            //{
                            //    other.m_sweep = backup;
                            //    other.SynchronizeTransform();
                            //    continue;
                            //}

                            //// Are there contact points?
                            //if (contact.IsTouching() == false)
                            //{
                            //    other.m_sweep = backup;
                            //    other.SynchronizeTransform();
                            //    continue;
                            //}

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

                            //// Has the other body already been added to the island?
                            //if (other.m_flags & Body.BodyFlags.e_islandFlag)
                            //{
                            //    continue;
                            //}

                            //// Add the other body to the island.
                            //other.m_flags |= Body.BodyFlags.e_islandFlag;

                            //if (other.m_type != _staticBody)
                            //{
                            //    other.SetAwake(true);
                            //}

                            //island.Add(other);
                        }
                    }
                }

                TimeStep subStep;
                subStep.dt                 = (1.0f - minAlpha) * step.dt;
                subStep.inv_dt             = 1.0f / subStep.dt;
                subStep.dtRatio            = 1.0f;
                subStep.positionIterations = 20;
                subStep.velocityIterations = step.velocityIterations;
                subStep.warmStarting       = false;
                island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);

                // Reset island flags and synchronize broad-phase proxies.
                for (int i = 0; i < island.m_bodies.Count(); ++i)
                {
                    throw new NotImplementedException();
                    //Body* body = island.m_bodies[i];
                    //body.m_flags &= ~Body.BodyFlags.e_islandFlag;

                    //if (body.m_type != _dynamicBody)
                    //{
                    //    continue;
                    //}

                    //body.SynchronizeFixtures();

                    //// Invalidate all contact TOIs on this displaced body.
                    //for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next)
                    //{
                    //    ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
                    //}
                }

                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
                // Also, some contacts can be destroyed.
                m_contactManager.FindNewContacts();

                if (m_subStepping)
                {
                    m_stepComplete = false;
                    break;
                }
            }
        }