Пример #1
0
		/// Compute the upper bound on time before two shapes penetrate. Time is represented as
		/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
		/// non-tunneling collision. If you change the time interval, you should call this function
		/// again.
		/// Note: use Distance to compute the contact point and normal at the time of impact.
		// CCD via the local separating axis method. This seeks progression
		// by computing the largest time at which separation is maintained.
		public static void TimeOfImpact(out TOIOutput output, TOIInput input){
		    Timer timer = new Timer();

		    ++_toiCalls;

		    output.state = TOIOutput.State.e_unknown;
		    output.t = input.tMax;

		    DistanceProxy proxyA = input.proxyA;
		    DistanceProxy proxyB = input.proxyB;

		    Sweep sweepA = input.sweepA;
		    Sweep sweepB = input.sweepB;

		    // Large rotations can make the root finder fail, so we normalize the
		    // sweep angles.
		    sweepA.Normalize();
		    sweepB.Normalize();

		    float tMax = input.tMax;

		    float totalRadius = proxyA.m_radius + proxyB.m_radius;
			float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop);
			float tolerance = 0.25f * Settings._linearSlop;
		    Utilities.Assert(target > tolerance);

		    float t1 = 0.0f;
		    const int k_maxIterations = 20;	// TODO_ERIN Settings
		    int iter = 0;

		    // Prepare input for distance query.
		    SimplexCache cache = new SimplexCache();
		    cache.count = 0;
		    DistanceInput distanceInput;
		    distanceInput.proxyA = input.proxyA;
		    distanceInput.proxyB = input.proxyB;
		    distanceInput.useRadii = false;

		    // The outer loop progressively attempts to compute new separating axes.
		    // This loop terminates when an axis is repeated (no progress is made).
		    for(;;)
		    {
		        Transform xfA, xfB;
		        sweepA.GetTransform(out xfA, t1);
		        sweepB.GetTransform(out xfB, t1);

		        // Get the distance between shapes. We can also use the results
		        // to get a separating axis.
		        distanceInput.transformA = xfA;
		        distanceInput.transformB = xfB;
		        DistanceOutput distanceOutput;
				Utilities.Distance(out distanceOutput, cache, distanceInput);

		        // If the shapes are overlapped, we give up on continuous collision.
		        if (distanceOutput.distance <= 0.0f)
		        {
		            // Failure!
		            output.state = TOIOutput.State.e_overlapped;
		            output.t = 0.0f;
		            break;
		        }

		        if (distanceOutput.distance < target + tolerance)
		        {
		            // Victory!
		            output.state = TOIOutput.State.e_touching;
		            output.t = t1;
		            break;
		        }

		        // Initialize the separating axis.
				throw new NotImplementedException();
		//        SeparationFunction fcn;
		//        fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
		//#if ZERO
		//        // Dump the curve seen by the root finder
		//        {
		//            const int N = 100;
		//            float dx = 1.0f / N;
		//            float xs[N+1];
		//            float fs[N+1];

		//            float x = 0.0f;

		//            for (int i = 0; i <= N; ++i)
		//            {
		//                sweepA.GetTransform(out xfA, x);
		//                sweepB.GetTransform(out xfB, x);
		//                float f = fcn.Evaluate(xfA, xfB) - target;

		//                printf("%g %g\n", x, f);

		//                xs[i] = x;
		//                fs[i] = f;

		//                x += dx;
		//            }
		//        }
		//#endif

		//        // Compute the TOI on the separating axis. We do this by successively
		//        // resolving the deepest point. This loop is bounded by the number of vertices.
		//        bool done = false;
		//        float t2 = tMax;
		//        int pushBackIter = 0;
		//        for (;;)
		//        {
		//            // Find the deepest point at t2. Store the witness point indices.
		//            int indexA, indexB;
		//            float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);

		//            // Is the final configuration separated?
		//            if (s2 > target + tolerance)
		//            {
		//                // Victory!
		//                output.state = TOIOutput.State.e_separated;
		//                output.t = tMax;
		//                done = true;
		//                break;
		//            }

		//            // Has the separation reached tolerance?
		//            if (s2 > target - tolerance)
		//            {
		//                // Advance the sweeps
		//                t1 = t2;
		//                break;
		//            }

		//            // Compute the initial separation of the witness points.
		//            float s1 = fcn.Evaluate(indexA, indexB, t1);

		//            // Check for initial overlap. This might happen if the root finder
		//            // runs out of iterations.
		//            if (s1 < target - tolerance)
		//            {
		//                output.state = TOIOutput.State.e_failed;
		//                output.t = t1;
		//                done = true;
		//                break;
		//            }

		//            // Check for touching
		//            if (s1 <= target + tolerance)
		//            {
		//                // Victory! t1 should hold the TOI (could be 0.0).
		//                output.state = TOIOutput.State.e_touching;
		//                output.t = t1;
		//                done = true;
		//                break;
		//            }

		//            // Compute 1D root of: f(x) - target = 0
		//            int rootIterCount = 0;
		//            float a1 = t1, a2 = t2;
		//            for (;;)
		//            {
		//                // Use a mix of the secant rule and bisection.
		//                float t;
		//                if (rootIterCount & 1)
		//                {
		//                    // Secant rule to improve convergence.
		//                    t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
		//                }
		//                else
		//                {
		//                    // Bisection to guarantee progress.
		//                    t = 0.5f * (a1 + a2);
		//                }

		//                ++rootIterCount;
		//                ++_toiRootIters;

		//                float s = fcn.Evaluate(indexA, indexB, t);

		//                if (Math.Abs(s - target) < tolerance)
		//                {
		//                    // t2 holds a tentative value for t1
		//                    t2 = t;
		//                    break;
		//                }

		//                // Ensure we continue to bracket the root.
		//                if (s > target)
		//                {
		//                    a1 = t;
		//                    s1 = s;
		//                }
		//                else
		//                {
		//                    a2 = t;
		//                    s2 = s;
		//                }
				
		//                if (rootIterCount == 50)
		//                {
		//                    break;
		//                }
		//            }

		//            _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount);

		//            ++pushBackIter;

		//            if (pushBackIter == Settings._maxPolygonVertices)
		//            {
		//                break;
		//            }
		//        }

		//        ++iter;
		//        ++_toiIters;

		//        if (done)
		//        {
		//            break;
		//        }

		//        if (iter == k_maxIterations)
		//        {
		//            // Root finder got stuck. Semi-victory.
		//            output.state = TOIOutput.State.e_failed;
		//            output.t = t1;
		//            break;
		//        }
		    }

		    _toiMaxIters = Math.Max(_toiMaxIters, iter);

		    float time = timer.GetMilliseconds();
		    _toiMaxTime = Math.Max(_toiMaxTime, time);
		    _toiTime += time;
		}
Пример #2
0
		public Tiles()
		{
			m_fixtureCount = 0;
			Timer timer = new Timer();

			{
				float a = 0.5f;
				BodyDef bd = new BodyDef();
				bd.Position.Y = -a;
				Body ground = m_world.CreateBody(bd);

	#if true
				int N = 200;
				int M = 10;
				Vec2 position;
				position.Y = 0.0f;
				for (int j = 0; j < M; ++j)
				{
					position.X = -N * a;
					for (int i = 0; i < N; ++i)
					{
						PolygonShape shape = new PolygonShape();
						shape.SetAsBox(a, a, position, 0.0f);
						shape.Density = 0;
						ground.CreateFixture(shape);
						++m_fixtureCount;
						position.X += 2.0f * a;
					}
					position.Y -= 2.0f * a;
				}
	#else
				int N = 200;
				int M = 10;
				Vec2 position;
				position.X = -N * a;
				for (int i = 0; i < N; ++i)
				{
					position.Y = 0.0f;
					for (int j = 0; j < M; ++j)
					{
						PolygonShape shape = new PolygonShape();
						shape.SetAsBox(a, a, position, 0.0f);
						ground.CreateFixture(shape, 0.0f);
						position.Y -= 2.0f * a;
					}
					position.X += 2.0f * a;
				}
	#endif
			}

			{
				float a = 0.5f;
				PolygonShape shape = new PolygonShape();
				shape.SetAsBox(a, a);

				Vec2 x = new Vec2(-7.0f, 0.75f);
				Vec2 y;
				Vec2 deltaX = new Vec2(0.5625f, 1.25f);
				Vec2 deltaY = new Vec2(1.125f, 0.0f);

				for (int i = 0; i < e_count; ++i)
				{
					y = x;

					for (int j = i; j < e_count; ++j)
					{
						BodyDef bd = new BodyDef();
						bd.type = BodyType._dynamicBody;
						bd.Position = y;

						//if (i == 0 && j == 0)
						//{
						//	bd.allowSleep = false;
						//}
						//else
						//{
						//	bd.allowSleep = true;
						//}

						Body body = m_world.CreateBody(bd);
						shape.Density = 5;
						body.CreateFixture(shape);
						++m_fixtureCount;
						y += deltaY;
					}

					x += deltaX;
				}
			}

			m_createTime = timer.GetMilliseconds();
		}
Пример #3
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);
			        }
			    }
			}
		}