Пример #1
0
        public void Solve(ref TimeStep step, ref TSVector2 gravity)
        {
            FP h = step.dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                TSVector2 c = b._sweep.C;
                FP        a = b._sweep.A;
                TSVector2 v = b._linearVelocity;
                FP        w = b._angularVelocity;

                // Store positions for continuous collision.
                b._sweep.C0 = b._sweep.C;
                b._sweep.A0 = b._sweep.A;

                if (b.BodyType == BodyType.Dynamic)
                {
                    // Integrate velocities.

                    // FPE: Only apply gravity if the body wants it.
                    if (b.IgnoreGravity)
                    {
                        v += h * (b._invMass * b._force);
                    }
                    else
                    {
                        v += h * (b.GravityScale * gravity + b._invMass * b._force);
                    }

                    w += h * b._invI * b._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 *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
                    //w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);

                    v *= 1 / (1 + h * b.LinearDamping);
                    w *= 1 / (1 + h * b.AngularDamping);
                }

                _positions[i].c  = c;
                _positions[i].a  = a;
                _velocities[i].v = v;
                _velocities[i].w = w;
            }

            // Solver data
            SolverData solverData = new SolverData();

            solverData.step       = step;
            solverData.positions  = _positions;
            solverData.velocities = _velocities;

            _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
            _contactSolver.InitializeVelocityConstraints();

            if (Settings.EnableWarmstarting)
            {
                _contactSolver.WarmStart();
            }

            for (int i = 0; i < JointCount; ++i)
            {
                if (_joints[i].Enabled)
                {
                    _joints[i].InitVelocityConstraints(ref solverData);
                }
            }

            // Solve velocity constraints.
            for (int i = 0; i < Settings.VelocityIterations; ++i)
            {
                for (int j = 0; j < JointCount; ++j)
                {
                    Joint2D joint = _joints[j];

                    if (!joint.Enabled)
                    {
                        continue;
                    }

                    joint.SolveVelocityConstraints(ref solverData);
                    joint.Validate(step.inv_dt);
                }

                _contactSolver.SolveVelocityConstraints();
            }

            // Store impulses for warm starting.
            _contactSolver.StoreImpulses();

            // Integrate positions
            for (int i = 0; i < BodyCount; ++i)
            {
                TSVector2 c = _positions[i].c;
                FP        a = _positions[i].a;
                TSVector2 v = _velocities[i].v;
                FP        w = _velocities[i].w;

                // Check for large velocities
                TSVector2 translation = h * v;
                if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
                {
                    FP ratio = Settings.MaxTranslation / translation.magnitude;
                    v *= ratio;
                }

                FP rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    FP ratio = Settings.MaxRotation / FP.Abs(rotation);
                    w *= ratio;
                }

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

                _positions[i].c  = c;
                _positions[i].a  = a;
                _velocities[i].v = v;
                _velocities[i].w = w;
            }


            // Solve position constraints
            bool positionSolved = false;

            for (int i = 0; i < Settings.PositionIterations; ++i)
            {
                bool contactsOkay = _contactSolver.SolvePositionConstraints();

                bool jointsOkay = true;
                for (int j = 0; j < JointCount; ++j)
                {
                    Joint2D joint = _joints[j];

                    if (!joint.Enabled)
                    {
                        continue;
                    }

                    bool jointOkay = joint.SolvePositionConstraints(ref 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 < BodyCount; ++i)
            {
                Body body = Bodies[i];
                body._sweep.C         = _positions[i].c;
                body._sweep.A         = _positions[i].a;
                body._linearVelocity  = _velocities[i].v;
                body._angularVelocity = _velocities[i].w;
                body.SynchronizeTransform();
            }

            Report(_contactSolver._velocityConstraints);

            if (Settings.AllowSleep)
            {
                FP minSleepTime = Settings.MaxFP;

                for (int i = 0; i < BodyCount; ++i)
                {
                    Body b = Bodies[i];

                    if (b.BodyType == BodyType.Static)
                    {
                        continue;
                    }

                    if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || TSVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += h;
                        minSleepTime  = SyncFrame.TSMath.Min(minSleepTime, b._sleepTime);
                    }
                }

                if (minSleepTime >= Settings.TimeToSleep && positionSolved)
                {
                    for (int i = 0; i < BodyCount; ++i)
                    {
                        Body b = Bodies[i];
                        b.Awake = false;
                    }
                }
            }
        }
Пример #2
0
        /// <summary>
        /// Attaches the bodies with revolute joints.
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="bodies">The bodies.</param>
        /// <param name="localAnchorA">The local anchor A.</param>
        /// <param name="localAnchorB">The local anchor B.</param>
        /// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
        /// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
        public static List <RevoluteJoint> AttachBodiesWithRevoluteJoint(World world, List <Body> bodies, TSVector2 localAnchorA, TSVector2 localAnchorB, bool connectFirstAndLast, bool collideConnected)
        {
            List <RevoluteJoint> joints = new List <RevoluteJoint>(bodies.Count + 1);

            for (int i = 1; i < bodies.Count; i++)
            {
                RevoluteJoint joint = new RevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB);
                joint.CollideConnected = collideConnected;
                world.AddJoint(joint);
                joints.Add(joint);
            }

            if (connectFirstAndLast)
            {
                RevoluteJoint lastjoint = new RevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB);
                lastjoint.CollideConnected = collideConnected;
                world.AddJoint(lastjoint);
                joints.Add(lastjoint);
            }

            return(joints);
        }
Пример #3
0
 public override bool TestPoint(ref Transform transform, ref TSVector2 point)
 {
     return(false);
 }
Пример #4
0
 /// <summary>
 /// Draw a line segment.
 /// </summary>
 /// <param name="start">The start.</param>
 /// <param name="end">The end.</param>
 /// <param name="red">The red value.</param>
 /// <param name="blue">The blue value.</param>
 /// <param name="green">The green value.</param>
 public abstract void DrawSegment(TSVector2 start, TSVector2 end, FP red, FP blue, FP green);
Пример #5
0
        internal override void InitVelocityConstraints(ref SolverData data)
        {
            _indexA       = BodyA.IslandIndex;
            _indexB       = BodyB.IslandIndex;
            _localCenterA = BodyA._sweep.LocalCenter;
            _localCenterB = BodyB._sweep.LocalCenter;
            _invMassA     = BodyA._invMass;
            _invMassB     = BodyB._invMass;
            _invIA        = BodyA._invI;
            _invIB        = BodyB._invI;

            FP        aA = data.positions[_indexA].a;
            TSVector2 vA = data.velocities[_indexA].v;
            FP        wA = data.velocities[_indexA].w;

            FP        aB = data.positions[_indexB].a;
            TSVector2 vB = data.velocities[_indexB].v;
            FP        wB = data.velocities[_indexB].w;

            Rot qA = new Rot(aA), qB = new Rot(aB);

            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            FP mA = _invMassA, mB = _invMassB;
            FP iA = _invIA, iB = _invIB;

            Mat33 K = new Mat33();

            K.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
            K.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
            K.ez.x = -_rA.y * iA - _rB.y * iB;
            K.ex.y = K.ey.x;
            K.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
            K.ez.y = _rA.x * iA + _rB.x * iB;
            K.ex.z = K.ez.x;
            K.ey.z = K.ez.y;
            K.ez.z = iA + iB;

            if (FrequencyHz > 0.0f)
            {
                K.GetInverse22(ref _mass);

                FP invM = iA + iB;
                FP m    = invM > 0.0f ? 1.0f / invM : 0.0f;

                FP C = aB - aA - ReferenceAngle;

                // Frequency
                FP omega = 2.0f * Settings.Pi * FrequencyHz;

                // Damping coefficient
                FP d = 2.0f * m * DampingRatio * omega;

                // Spring stiffness
                FP k = m * omega * omega;

                // magic formulas
                FP h = data.step.dt;
                _gamma = h * (d + h * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * h * k * _gamma;

                invM      += _gamma;
                _mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
            }
            else
            {
                K.GetSymInverse33(ref _mass);
                _gamma = 0.0f;
                _bias  = 0.0f;
            }

            if (Settings.EnableWarmstarting)
            {
                // Scale impulses to support a variable time step.
                _impulse *= data.step.dtRatio;

                TSVector2 P = new TSVector2(_impulse.x, _impulse.y);

                vA -= mA * P;
                wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z);

                vB += mB * P;
                wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z);
            }
            else
            {
                _impulse = TSVector.zero;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
Пример #6
0
 public BuoyancyController(AABB container, FP density, FP linearDragCoefficient, FP rotationalDragCoefficient, TSVector2 gravity) : base(ControllerType.BuoyancyController)
 {
     this.Container              = container;
     this._normal                = new TSVector2(0, 1);
     this.Density                = density;
     this.LinearDragCoefficient  = linearDragCoefficient;
     this.AngularDragCoefficient = rotationalDragCoefficient;
     this._gravity               = gravity;
 }
Пример #7
0
        internal override void InitVelocityConstraints(ref SolverData data)
        {
            this._indexA       = base.BodyA.IslandIndex;
            this._indexB       = base.BodyB.IslandIndex;
            this._localCenterA = base.BodyA._sweep.LocalCenter;
            this._localCenterB = base.BodyB._sweep.LocalCenter;
            this._invMassA     = base.BodyA._invMass;
            this._invMassB     = base.BodyB._invMass;
            this._invIA        = base.BodyA._invI;
            this._invIB        = base.BodyB._invI;
            TSVector2 c         = data.positions[this._indexA].c;
            FP        a         = data.positions[this._indexA].a;
            TSVector2 tSVector  = data.velocities[this._indexA].v;
            FP        fP        = data.velocities[this._indexA].w;
            TSVector2 c2        = data.positions[this._indexB].c;
            FP        a2        = data.positions[this._indexB].a;
            TSVector2 tSVector2 = data.velocities[this._indexB].v;
            FP        fP2       = data.velocities[this._indexB].w;
            Rot       q         = new Rot(a);
            Rot       q2        = new Rot(a2);

            this._rA     = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
            this._rB     = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
            this._u      = c2 + this._rB - c - this._rA;
            this._length = this._u.magnitude;
            FP   x    = this._length - this.MaxLength;
            bool flag = x > 0f;

            if (flag)
            {
                this.State = LimitState.AtUpper;
            }
            else
            {
                this.State = LimitState.Inactive;
            }
            bool flag2 = this._length > Settings.LinearSlop;

            if (flag2)
            {
                this._u *= 1f / this._length;
                FP y   = MathUtils.Cross(this._rA, this._u);
                FP y2  = MathUtils.Cross(this._rB, this._u);
                FP fP3 = this._invMassA + this._invIA * y * y + this._invMassB + this._invIB * y2 * y2;
                this._mass     = ((fP3 != 0f) ? (1f / fP3) : 0f);
                this._impulse *= data.step.dtRatio;
                TSVector2 tSVector3 = this._impulse * this._u;
                tSVector  -= this._invMassA * tSVector3;
                fP        -= this._invIA * MathUtils.Cross(this._rA, tSVector3);
                tSVector2 += this._invMassB * tSVector3;
                fP2       += this._invIB * MathUtils.Cross(this._rB, tSVector3);
                data.velocities[this._indexA].v = tSVector;
                data.velocities[this._indexA].w = fP;
                data.velocities[this._indexB].v = tSVector2;
                data.velocities[this._indexB].w = fP2;
            }
            else
            {
                this._u       = TSVector2.zero;
                this._mass    = 0f;
                this._impulse = 0f;
            }
        }
Пример #8
0
        public static BreakableBody CreateBreakableBody(World world, IEnumerable <Shape> shapes, TSVector2 position)
        {
            BreakableBody breakableBody = new BreakableBody(shapes, world);

            breakableBody.MainBody.Position = position;
            world.AddBreakableBody(breakableBody);

            return(breakableBody);
        }
Пример #9
0
        public static Body CreateLineArc(World world, FP radians, int sides, FP radius, TSVector2 position, FP angle, bool closed)
        {
            Body body = CreateBody(world);

            FixtureFactory.AttachLineArc(radians, sides, radius, position, angle, closed, body);
            return(body);
        }
Пример #10
0
        /// <summary>
        /// Creates a rounded rectangle.
        /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="width">The width.</param>
        /// <param name="height">The height.</param>
        /// <param name="xRadius">The x radius.</param>
        /// <param name="yRadius">The y radius.</param>
        /// <param name="segments">The segments.</param>
        /// <param name="density">The density.</param>
        /// <param name="position">The position.</param>
        /// <returns></returns>
        public static Body CreateRoundedRectangle(World world, FP width, FP height, FP xRadius, FP yRadius, int segments, FP density, TSVector2 position, object userData = null)
        {
            Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments);

            //There are too many vertices in the capsule. We decompose it.
            if (verts.Count >= Settings.MaxPolygonVertices)
            {
                List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, FP.EN3);
                Body            body     = CreateCompoundPolygon(world, vertList, density, userData);
                body.Position = position;
                return(body);
            }

            return(CreatePolygon(world, verts, density, null));
        }
Пример #11
0
        /// <summary>
        /// Creates a breakable body. You would want to remove collinear points before using this.
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="vertices">The vertices.</param>
        /// <param name="density">The density.</param>
        /// <param name="position">The position.</param>
        /// <returns></returns>
        public static BreakableBody CreateBreakableBody(World world, Vertices vertices, FP density, TSVector2 position)
        {
            List <Vertices> triangles = Triangulate.ConvexPartition(vertices, TriangulationAlgorithm.Earclip, true, FP.EN3);

            BreakableBody breakableBody = new BreakableBody(triangles, world, density);

            breakableBody.MainBody.Position = position;
            world.AddBreakableBody(breakableBody);

            return(breakableBody);
        }
Пример #12
0
        /// <summary>
        /// Creates a capsule.
        /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
        /// </summary>
        /// <returns></returns>
        public static Body CreateCapsule(World world, FP height, FP topRadius, int topEdges, FP bottomRadius, int bottomEdges, FP density, TSVector2 position, object userData = null)
        {
            Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges);

            Body body;

            //There are too many vertices in the capsule. We decompose it.
            if (verts.Count >= Settings.MaxPolygonVertices)
            {
                List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, FP.EN3);
                body          = CreateCompoundPolygon(world, vertList, density, userData);
                body.Position = position;

                return(body);
            }

            body          = CreatePolygon(world, verts, density, userData);
            body.Position = position;

            return(body);
        }
Пример #13
0
        // TS - public static Body CreateBody(World world, Vector2 position, FP rotation = 0, object userData = null)
        public static Body CreateBody(World world, TSVector2 position, FP rotation, object userData = null)
        {
            Body body = new Body(world, position, rotation, userData);

            return(body);
        }
Пример #14
0
        public static Body CreateCompoundPolygon(World world, List <Vertices> list, FP density, TSVector2 position, object userData = null)
        {
            //We create a single body
            Body polygonBody = CreateBody(world, position);

            FixtureFactory.AttachCompoundPolygon(list, density, polygonBody, userData);
            return(polygonBody);
        }
Пример #15
0
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            TSVector2 cA = data.positions[_indexA].c;
            FP        aA = data.positions[_indexA].a;
            TSVector2 cB = data.positions[_indexB].c;
            FP        aB = data.positions[_indexB].a;

            Rot qA = new Rot(aA), qB = new Rot(aB);

            FP angularError = 0.0f;
            FP positionError;

            bool fixedRotation = (_invIA + _invIB == 0.0f);

            // Solve angular limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
            {
                FP angle        = aB - aA - ReferenceAngle;
                FP limitImpulse = 0.0f;

                if (_limitState == LimitState.Equal)
                {
                    // Prevent large angular corrections
                    FP C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = FP.Abs(C);
                }
                else if (_limitState == LimitState.AtLower)
                {
                    FP C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C            = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    FP C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C            = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                }

                aA -= _invIA * limitImpulse;
                aB += _invIB * limitImpulse;
            }

            // Solve point-to-point constraint.
            {
                qA.Set(aA);
                qB.Set(aB);
                TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
                TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);

                TSVector2 C = cB + rB - cA - rA;
                positionError = C.magnitude;

                FP mA = _invMassA, mB = _invMassB;
                FP iA = _invIA, iB = _invIB;

                Mat22 K = new Mat22();
                K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
                K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
                K.ey.x = K.ex.y;
                K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;

                TSVector2 impulse = -K.Solve(C);

                cA -= mA * impulse;
                aA -= iA * MathUtils.Cross(rA, impulse);

                cB += mB * impulse;
                aB += iB * MathUtils.Cross(rB, impulse);
            }

            data.positions[_indexA].c = cA;
            data.positions[_indexA].a = aA;
            data.positions[_indexB].c = cB;
            data.positions[_indexB].a = aB;

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Пример #16
0
        public static Body CreateSolidArc(World world, FP density, FP radians, int sides, FP radius, TSVector2 position, FP angle)
        {
            Body body = CreateBody(world);

            FixtureFactory.AttachSolidArc(density, radians, sides, radius, position, angle, body);
            return(body);
        }
Пример #17
0
 Vector3 ToXZ(TSVector2 target)
 {
     return(new Vector3(target.x.AsFloat(), 0, target.y.AsFloat()));
 }
Пример #18
0
        public static Body CreateRectangle(World world, FP width, FP height, FP density, TSVector2 position, object userData = null)
        {
            if (width <= 0)
            {
                throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters");
            }

            if (height <= 0)
            {
                throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters");
            }

            Body newBody = CreateBody(world, position);

            newBody.UserData = userData;

            Vertices     rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
            PolygonShape rectangleShape    = new PolygonShape(rectangleVertices, density);

            newBody.CreateFixture(rectangleShape);

            return(newBody);
        }
Пример #19
0
 public override void Update(FP dt)
 {
     this._uniqueBodies.Clear();
     this.World.QueryAABB(delegate(Fixture fixture)
     {
         bool flag3 = fixture.Body.IsStatic || !fixture.Body.Awake;
         bool result;
         if (flag3)
         {
             result = true;
         }
         else
         {
             bool flag4 = !this._uniqueBodies.ContainsKey(fixture.Body.BodyId);
             if (flag4)
             {
                 this._uniqueBodies.Add(fixture.Body.BodyId, fixture.Body);
             }
             result = true;
         }
         return(result);
     }, ref this._container);
     foreach (KeyValuePair <int, Body> current in this._uniqueBodies)
     {
         Body      value = current.Value;
         TSVector2 zero  = TSVector2.zero;
         TSVector2 zero2 = TSVector2.zero;
         FP        fP    = 0;
         FP        fP2   = 0;
         for (int i = 0; i < value.FixtureList.Count; i++)
         {
             Fixture fixture2 = value.FixtureList[i];
             bool    flag     = fixture2.Shape.ShapeType != ShapeType.Polygon && fixture2.Shape.ShapeType > ShapeType.Circle;
             if (!flag)
             {
                 Shape     shape = fixture2.Shape;
                 TSVector2 tSVector;
                 FP        fP3 = shape.ComputeSubmergedArea(ref this._normal, this._offset, ref value._xf, out tSVector);
                 fP      += fP3;
                 zero.x  += fP3 * tSVector.x;
                 zero.y  += fP3 * tSVector.y;
                 fP2     += fP3 * shape.Density;
                 zero2.x += fP3 * tSVector.x * shape.Density;
                 zero2.y += fP3 * tSVector.y * shape.Density;
             }
         }
         zero.x  /= fP;
         zero.y  /= fP;
         zero2.x /= fP2;
         zero2.y /= fP2;
         bool flag2 = fP < Settings.Epsilon;
         if (!flag2)
         {
             TSVector2 force = -this.Density * fP * this._gravity;
             value.ApplyForce(force, zero2);
             TSVector2 tSVector2 = value.GetLinearVelocityFromWorldPoint(zero) - this.Velocity;
             tSVector2 *= -this.LinearDragCoefficient * fP;
             value.ApplyForce(tSVector2, zero);
             value.ApplyTorque(-value.Inertia / value.Mass * fP * value.AngularVelocity * this.AngularDragCoefficient);
         }
     }
 }
Пример #20
0
        /// <summary>
        /// Ray-cast against the proxies in the tree. This relies on the callback
        /// to perform a exact ray-cast in the case were the proxy contains a Shape.
        /// The callback also performs the any collision filtering. This has performance
        /// roughly equal to k * log(n), where k is the number of collisions and n is the
        /// number of proxies in the tree.
        /// </summary>
        /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
        /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
        public void RayCast(Func <RayCastInput, int, FP> callback, ref RayCastInput input)
        {
            TSVector2 p1 = input.Point1;
            TSVector2 p2 = input.Point2;
            TSVector2 r  = p2 - p1;

            Debug.Assert(r.LengthSquared() > 0.0f);
            r.Normalize();

            // v is perpendicular to the segment.
            TSVector2 absV = MathUtils.Abs(new TSVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable

            // Separating axis for segment (Gino, p80).
            // |dot(v, p1 - c)| > dot(|v|, h)

            FP maxFraction = input.MaxFraction;

            // Build a bounding box for the segment.
            AABB segmentAABB = new AABB();

            {
                TSVector2 t = p1 + maxFraction * (p2 - p1);
                TSVector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
                TSVector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
            }

            _raycastStack.Clear();
            _raycastStack.Push(_root);

            while (_raycastStack.Count > 0)
            {
                int nodeId = _raycastStack.Pop();
                if (nodeId == NullNode)
                {
                    continue;
                }

                TreeNode <T> node = _nodes[nodeId];

                if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c)| > dot(|v|, h)
                TSVector2 c          = node.AABB.Center;
                TSVector2 h          = node.AABB.Extents;
                FP        separation = FP.Abs(TSVector2.Dot(new TSVector2(-r.y, r.x), p1 - c)) - TSVector2.Dot(absV, h);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.IsLeaf())
                {
                    RayCastInput subInput;
                    subInput.Point1      = input.Point1;
                    subInput.Point2      = input.Point2;
                    subInput.MaxFraction = maxFraction;

                    FP value = callback(subInput, nodeId);

                    if (value == 0.0f)
                    {
                        // the client has terminated the raycast.
                        return;
                    }

                    if (value > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value;
                        TSVector2 t = p1 + maxFraction * (p2 - p1);
                        segmentAABB.LowerBound = TSVector2.Min(p1, t);
                        segmentAABB.UpperBound = TSVector2.Max(p1, t);
                    }
                }
                else
                {
                    _raycastStack.Push(node.Child1);
                    _raycastStack.Push(node.Child2);
                }
            }
        }
Пример #21
0
 /// <summary>
 /// Draw a solid circle.
 /// </summary>
 /// <param name="center">The center.</param>
 /// <param name="radius">The radius.</param>
 /// <param name="axis">The axis.</param>
 /// <param name="red">The red value.</param>
 /// <param name="blue">The blue value.</param>
 /// <param name="green">The green value.</param>
 public abstract void DrawSolidCircle(TSVector2 center, FP radius, TSVector2 axis, FP red, FP blue,
                                      FP green);
Пример #22
0
 /// <summary>
 /// Constructor of RevoluteJoint.
 /// </summary>
 /// <param name="bodyA">The first body.</param>
 /// <param name="bodyB">The second body.</param>
 /// <param name="anchor">The shared anchor.</param>
 /// <param name="useWorldCoordinates"></param>
 public RevoluteJoint(Body bodyA, Body bodyB, TSVector2 anchor, bool useWorldCoordinates = false)
     : this(bodyA, bodyB, anchor, anchor, useWorldCoordinates)
 {
 }
Пример #23
0
 public FP SDValue(TSVector2 pos)
 {
     return(SDFUtils.SDCircle(pos, Center, Radius));
 }
Пример #24
0
        public override TSVector2 GetReactionForce(FP invDt)
        {
            TSVector2 p = new TSVector2(_impulse.x, _impulse.y);

            return(invDt * p);
        }
Пример #25
0
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            TSVector2 cA = data.positions[_indexA].c;
            FP        aA = data.positions[_indexA].a;
            TSVector2 cB = data.positions[_indexB].c;
            FP        aB = data.positions[_indexB].a;

            Rot qA = new Rot(aA), qB = new Rot(aB);

            FP mA = _invMassA, mB = _invMassB;
            FP iA = _invIA, iB = _invIB;

            TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);

            FP positionError, angularError;

            Mat33 K = new Mat33();

            K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
            K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
            K.ez.x = -rA.y * iA - rB.y * iB;
            K.ex.y = K.ey.x;
            K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
            K.ez.y = rA.x * iA + rB.x * iB;
            K.ex.z = K.ez.x;
            K.ey.z = K.ez.y;
            K.ez.z = iA + iB;

            if (FrequencyHz > 0.0f)
            {
                TSVector2 C1 = cB + rB - cA - rA;

                positionError = C1.magnitude;
                angularError  = 0.0f;

                TSVector2 P = -K.Solve22(C1);

                cA -= mA * P;
                aA -= iA * MathUtils.Cross(rA, P);

                cB += mB * P;
                aB += iB * MathUtils.Cross(rB, P);
            }
            else
            {
                TSVector2 C1 = cB + rB - cA - rA;
                FP        C2 = aB - aA - ReferenceAngle;

                positionError = C1.magnitude;
                angularError  = FP.Abs(C2);

                TSVector C = new TSVector(C1.x, C1.y, C2);

                TSVector  impulse = K.Solve33(C) * -1;
                TSVector2 P       = new TSVector2(impulse.x, impulse.y);

                cA -= mA * P;
                aA -= iA * (MathUtils.Cross(rA, P) + impulse.z);

                cB += mB * P;
                aB += iB * (MathUtils.Cross(rB, P) + impulse.z);
            }

            data.positions[_indexA].c = cA;
            data.positions[_indexA].a = aA;
            data.positions[_indexB].c = cB;
            data.positions[_indexB].a = aB;

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Пример #26
0
        internal override void InitVelocityConstraints(ref SolverData data)
        {
            _indexA       = BodyA.IslandIndex;
            _indexB       = BodyB.IslandIndex;
            _localCenterA = BodyA._sweep.LocalCenter;
            _localCenterB = BodyB._sweep.LocalCenter;
            _invMassA     = BodyA._invMass;
            _invMassB     = BodyB._invMass;
            _invIA        = BodyA._invI;
            _invIB        = BodyB._invI;

            FP        aA = data.positions[_indexA].a;
            TSVector2 vA = data.velocities[_indexA].v;
            FP        wA = data.velocities[_indexA].w;

            FP        aB = data.positions[_indexB].a;
            TSVector2 vB = data.velocities[_indexB].v;
            FP        wB = data.velocities[_indexB].w;

            Rot qA = new Rot(aA), qB = new Rot(aB);

            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            FP mA = _invMassA, mB = _invMassB;
            FP iA = _invIA, iB = _invIB;

            bool fixedRotation = (iA + iB == 0.0f);

            _mass.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
            _mass.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
            _mass.ez.x = -_rA.y * iA - _rB.y * iB;
            _mass.ex.y = _mass.ey.x;
            _mass.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
            _mass.ez.y = _rA.x * iA + _rB.x * iB;
            _mass.ex.z = _mass.ez.x;
            _mass.ey.z = _mass.ez.y;
            _mass.ez.z = iA + iB;

            _motorMass = iA + iB;
            if (_motorMass > 0.0f)
            {
                _motorMass = 1.0f / _motorMass;
            }

            if (_enableMotor == false || fixedRotation)
            {
                _motorImpulse = 0.0f;
            }

            if (_enableLimit && fixedRotation == false)
            {
                FP jointAngle = aB - aA - ReferenceAngle;
                if (FP.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _impulse.z = 0.0f;
                    }
                    _limitState = LimitState.AtLower;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _impulse.z = 0.0f;
                    }
                    _limitState = LimitState.AtUpper;
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (Settings.EnableWarmstarting)
            {
                // Scale impulses to support a variable time step.
                _impulse      *= data.step.dtRatio;
                _motorImpulse *= data.step.dtRatio;

                TSVector2 P = new TSVector2(_impulse.x, _impulse.y);

                vA -= mA * P;
                wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.z);

                vB += mB * P;
                wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.z);
            }
            else
            {
                _impulse      = TSVector.zero;
                _motorImpulse = 0.0f;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
Пример #27
0
        private static List <Vertices> TriangulatePolygon(Vertices vertices, FP tolerance)
        {
            bool            flag = vertices.Count < 3;
            List <Vertices> result;

            if (flag)
            {
                result = new List <Vertices>();
            }
            else
            {
                List <Vertices> list = new List <Vertices>();
                Vertices        pin  = new Vertices(vertices);
                Vertices        vertices2;
                Vertices        vertices3;
                bool            flag2 = EarclipDecomposer.ResolvePinchPoint(pin, out vertices2, out vertices3, tolerance);
                if (flag2)
                {
                    List <Vertices> list2 = EarclipDecomposer.TriangulatePolygon(vertices2, tolerance);
                    List <Vertices> list3 = EarclipDecomposer.TriangulatePolygon(vertices3, tolerance);
                    bool            flag3 = list2.Count == -1 || list3.Count == -1;
                    if (flag3)
                    {
                        throw new Exception("Can't triangulate your polygon.");
                    }
                    for (int i = 0; i < list2.Count; i++)
                    {
                        list.Add(new Vertices(list2[i]));
                    }
                    for (int j = 0; j < list3.Count; j++)
                    {
                        list.Add(new Vertices(list3[j]));
                    }
                    result = list;
                }
                else
                {
                    Vertices[] array  = new Vertices[vertices.Count - 2];
                    int        num    = 0;
                    FP[]       array2 = new FP[vertices.Count];
                    FP[]       array3 = new FP[vertices.Count];
                    for (int k = 0; k < vertices.Count; k++)
                    {
                        array2[k] = vertices[k].x;
                        array3[k] = vertices[k].y;
                    }
                    int l = vertices.Count;
                    while (l > 3)
                    {
                        int num2 = -1;
                        FP  y    = -10f;
                        for (int m = 0; m < l; m++)
                        {
                            bool flag4 = EarclipDecomposer.IsEar(m, array2, array3, l);
                            if (flag4)
                            {
                                int       num3      = EarclipDecomposer.Remainder(m - 1, l);
                                int       num4      = EarclipDecomposer.Remainder(m + 1, l);
                                TSVector2 tSVector  = new TSVector2(array2[num4] - array2[m], array3[num4] - array3[m]);
                                TSVector2 tSVector2 = new TSVector2(array2[m] - array2[num3], array3[m] - array3[num3]);
                                TSVector2 tSVector3 = new TSVector2(array2[num3] - array2[num4], array3[num3] - array3[num4]);
                                tSVector.Normalize();
                                tSVector2.Normalize();
                                tSVector3.Normalize();
                                FP fP;
                                MathUtils.Cross(ref tSVector, ref tSVector2, out fP);
                                fP = FP.Abs(fP);
                                FP fP2;
                                MathUtils.Cross(ref tSVector2, ref tSVector3, out fP2);
                                fP2 = FP.Abs(fP2);
                                FP fP3;
                                MathUtils.Cross(ref tSVector3, ref tSVector, out fP3);
                                fP3 = FP.Abs(fP3);
                                FP   fP4   = TSMath.Min(fP, TSMath.Min(fP2, fP3));
                                bool flag5 = fP4 > y;
                                if (flag5)
                                {
                                    num2 = m;
                                    y    = fP4;
                                }
                            }
                        }
                        bool flag6 = num2 == -1;
                        if (flag6)
                        {
                            for (int n = 0; n < num; n++)
                            {
                                list.Add(array[n]);
                            }
                            result = list;
                            return(result);
                        }
                        l--;
                        FP[] array4 = new FP[l];
                        FP[] array5 = new FP[l];
                        int  num5   = 0;
                        for (int num6 = 0; num6 < l; num6++)
                        {
                            bool flag7 = num5 == num2;
                            if (flag7)
                            {
                                num5++;
                            }
                            array4[num6] = array2[num5];
                            array5[num6] = array3[num5];
                            num5++;
                        }
                        int num7 = (num2 == 0) ? l : (num2 - 1);
                        int num8 = (num2 == l) ? 0 : (num2 + 1);
                        EarclipDecomposer.Triangle triangle = new EarclipDecomposer.Triangle(array2[num2], array3[num2], array2[num8], array3[num8], array2[num7], array3[num7]);
                        array[num] = triangle;
                        num++;
                        array2 = array4;
                        array3 = array5;
                    }
                    EarclipDecomposer.Triangle triangle2 = new EarclipDecomposer.Triangle(array2[1], array3[1], array2[2], array3[2], array2[0], array3[0]);
                    array[num] = triangle2;
                    num++;
                    for (int num9 = 0; num9 < num; num9++)
                    {
                        list.Add(new Vertices(array[num9]));
                    }
                    result = list;
                }
            }
            return(result);
        }
Пример #28
0
        internal override void SolveVelocityConstraints(ref SolverData data)
        {
            TSVector2 vA = data.velocities[_indexA].v;
            FP        wA = data.velocities[_indexA].w;
            TSVector2 vB = data.velocities[_indexB].v;
            FP        wB = data.velocities[_indexB].w;

            FP mA = _invMassA, mB = _invMassB;
            FP iA = _invIA, iB = _invIB;

            bool fixedRotation = (iA + iB == 0.0f);

            // Solve motor constraint.
            if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
            {
                FP Cdot       = wB - wA - _motorSpeed;
                FP impulse    = _motorMass * (-Cdot);
                FP oldImpulse = _motorImpulse;
                FP maxImpulse = data.step.dt * _maxMotorTorque;
                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
            {
                TSVector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
                FP        Cdot2 = wB - wA;
                TSVector  Cdot  = new TSVector(Cdot1.x, Cdot1.y, Cdot2);

                TSVector impulse = _mass.Solve33(Cdot) * -1;

                if (_limitState == LimitState.Equal)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLower)
                {
                    FP newImpulse = _impulse.z + impulse.z;
                    if (newImpulse < 0.0f)
                    {
                        TSVector2 rhs     = -Cdot1 + _impulse.z * new TSVector2(_mass.ez.x, _mass.ez.y);
                        TSVector2 reduced = _mass.Solve22(rhs);
                        impulse.x   = reduced.x;
                        impulse.y   = reduced.y;
                        impulse.z   = -_impulse.z;
                        _impulse.x += reduced.x;
                        _impulse.y += reduced.y;
                        _impulse.z  = 0.0f;
                    }
                    else
                    {
                        _impulse += impulse;
                    }
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    FP newImpulse = _impulse.z + impulse.z;
                    if (newImpulse > 0.0f)
                    {
                        TSVector2 rhs     = -Cdot1 + _impulse.z * new TSVector2(_mass.ez.x, _mass.ez.y);
                        TSVector2 reduced = _mass.Solve22(rhs);
                        impulse.x   = reduced.x;
                        impulse.y   = reduced.y;
                        impulse.z   = -_impulse.z;
                        _impulse.x += reduced.x;
                        _impulse.y += reduced.y;
                        _impulse.z  = 0.0f;
                    }
                    else
                    {
                        _impulse += impulse;
                    }
                }

                TSVector2 P = new TSVector2(impulse.x, impulse.y);

                vA -= mA * P;
                wA -= iA * (MathUtils.Cross(_rA, P) + impulse.z);

                vB += mB * P;
                wB += iB * (MathUtils.Cross(_rB, P) + impulse.z);
            }
            else
            {
                // Solve point-to-point constraint
                TSVector2 Cdot    = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
                TSVector2 impulse = _mass.Solve22(-Cdot);

                _impulse.x += impulse.x;
                _impulse.y += impulse.y;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(_rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(_rB, impulse);
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
Пример #29
0
 public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
 {
     sc = TSVector2.zero;
     return(0);
 }
Пример #30
0
        internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
        {
            Debug.Assert(toiIndexA < BodyCount);
            Debug.Assert(toiIndexB < BodyCount);

            // Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];
                _positions[i].c  = b._sweep.C;
                _positions[i].a  = b._sweep.A;
                _velocities[i].v = b._linearVelocity;
                _velocities[i].w = b._angularVelocity;
            }

            _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);

            // Solve position constraints.
            for (int i = 0; i < Settings.TOIPositionIterations; ++i)
            {
                bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
                if (contactsOkay)
                {
                    break;
                }
            }

            // Leap of faith to new safe state.
            Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
            Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
            Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
            Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a;

            // No warm starting is needed for TOI events because warm
            // starting impulses were applied in the discrete solver.
            _contactSolver.InitializeVelocityConstraints();

            // Solve velocity constraints.
            for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
            {
                _contactSolver.SolveVelocityConstraints();
            }

            // Don't store the TOI contact forces for warm starting
            // because they can be quite large.

            FP h = subStep.dt;

            // Integrate positions.
            for (int i = 0; i < BodyCount; ++i)
            {
                TSVector2 c = _positions[i].c;
                FP        a = _positions[i].a;
                TSVector2 v = _velocities[i].v;
                FP        w = _velocities[i].w;

                // Check for large velocities
                TSVector2 translation = h * v;
                if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
                {
                    FP ratio = Settings.MaxTranslation / translation.magnitude;
                    v *= ratio;
                }

                FP rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    FP ratio = Settings.MaxRotation / FP.Abs(rotation);
                    w *= ratio;
                }

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

                _positions[i].c  = c;
                _positions[i].a  = a;
                _velocities[i].v = v;
                _velocities[i].w = w;

                // Sync bodies
                Body body = Bodies[i];
                body._sweep.C         = c;
                body._sweep.A         = a;
                body._linearVelocity  = v;
                body._angularVelocity = w;
                body.SynchronizeTransform();
            }

            Report(_contactSolver._velocityConstraints);
        }