Пример #1
0
        public override void Step(Settings settings)
        {
            base.Step(settings);

            DistanceInput input = new DistanceInput();

            input.TransformA = _transformA;
            input.TransformB = _transformB;
            input.UseRadii   = true;
            SimplexCache cache = new SimplexCache();

            cache.Count = 0;
            DistanceOutput output;

            Collision.Distance(out output, ref cache, ref input, _polygonA, _polygonB);

            StringBuilder strBld = new StringBuilder();

            strBld.AppendFormat("distance = {0}", new object[] { output.Distance });
            OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
            _textLine += 15;

            strBld = new StringBuilder();
            strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations });
            OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
            _textLine += 15;

            {
                Color color = new Color(0.9f, 0.9f, 0.9f);
                int   i;
                for (i = 0; i < _polygonA.VertexCount; ++i)
                {
                    _dv[i] = Math.Mul(_transformA, _polygonA.Vertices[i]);
                }
                _debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color);

                for (i = 0; i < _polygonB.VertexCount; ++i)
                {
                    _dv[i] = Math.Mul(_transformB, _polygonB.Vertices[i]);
                }
                _debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color);
            }

            Vec2 x1 = output.PointA;
            Vec2 x2 = output.PointB;

            OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0));
            OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0));
            OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0));
        }
        public override void Step(TimeStep step)
        {
            //B2_NOT_USED(step);
            if (InvSqr)
            {
                for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
                {
                    Body body1 = i.body;
                    for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
                    {
                        Body    body2 = j.body;
                        Vector2 d     = body2.GetWorldCenter() - body1.GetWorldCenter();
                        float   r2    = d.LengthSquared();
                        if (r2 < Settings.FLT_EPSILON)
                        {
                            continue;
                        }

                        Vector2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d;
                        body1.ApplyForce(f, body1.GetWorldCenter());
                        body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
                    }
                }
            }
            else
            {
                for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
                {
                    Body body1 = i.body;
                    for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
                    {
                        Body    body2 = j.body;
                        Vector2 d     = body2.GetWorldCenter() - body1.GetWorldCenter();
                        float   r2    = d.LengthSquared();
                        if (r2 < Settings.FLT_EPSILON)
                        {
                            continue;
                        }
                        Vector2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
                        body1.ApplyForce(f, body1.GetWorldCenter());
                        body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
                    }
                }
            }
        }