Esempio n. 1
0
        public void Step(float full_dt)
        {
            var dt         = full_dt / substeps;
            var gravity_dp = gravity * Mathf.Sq(dt);

            var positionDampCoeff = Mathf.Pow(1f - positionDamping, dt);
            var angularDampCoeff  = Mathf.Pow(1f - angularDamping, dt);

            foreach (var body in tree.GetAll())
            {
                body.positionVelocity = body.positionVelocity.Clamp(maxPositionVelocity);
                body.angularVelocity  = Mathf.ClampSymmetric(body.angularVelocity, maxAngularVelocity);
                body.lastPosition     = body.position - body.positionVelocity * dt;
                body.lastAngle        = body.angle - body.angularVelocity * dt;
            }
            for (var substep = 0; substep < substeps; substep++)
            {
                foreach (var body in tree.GetAll())
                {
                    if (body.type == BodyType.Dynamic)
                    {
                        body.position += gravity_dp;
                        var positionDelta = body.position - body.lastPosition;
                        body.lastPosition = body.position;
                        body.position    += positionDelta * positionDampCoeff;
                        var angleDelta = body.angle - body.lastAngle;
                        body.lastAngle = body.angle;
                        body.angle    += angleDelta * angularDampCoeff;
                        var angleWrapper = DiffToMod(body.angle, Mathf.TWO_PI);
                        body.angle     += angleWrapper;
                        body.lastAngle += angleWrapper;
                    }
                    UpdateBodyCache(body);
                }

                foreach (var body in tree.GetAll())
                {
                    tree.Add(body);
                }

                contacts.Clear();
                notables.Clear();

                foreach (var body0 in tree.GetAll())
                {
                    tempList.Clear();
                    tree.TraverseOverlapping(body0, _addToTempList);

                    foreach (var body1 in tempList)
                    {
                        if (body0 == body1)
                        {
                            continue;
                        }
                        ClosestPoints p = CapsuleCache.Collide(
                            body0.fixtureCache,
                            body1.fixtureCache,
                            null
                            );

                        if (!p.degenerate)
                        {
                            StoreContact(body0, body1, p);
                            if (p.distance < 0)
                            {
                                CorrectContact(body0, body1, p);
                                UpdateBodyCache(body0);
                                UpdateBodyCache(body1);
                            }
                        }
                    }
                }
                foreach (var body in tree.GetAll())
                {
                    tree.Add(body);
                }
            }
            foreach (var body in tree.GetAll())
            {
                var invDt = 1f / dt;
                body.positionVelocity = (body.position - body.lastPosition) * invDt;
                body.angularVelocity  = (body.angle - body.lastAngle) * invDt;
            }
        }