source: 2013/30/MiskaK/MW2(My Warfare 2)/Paranneltu Jypeli/Physics2DDotNet/Solvers/SequentialImpulses.cs @ 4507

Revision 4507, 33.8 KB checked in by anlakane, 6 years ago (diff)

Talletus.

Line 
1#region MIT License
2/*
3 * Copyright (c) 2005-2008 Jonathan Mark Porter. http://physics2d.googlepages.com/
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights to
8 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
9 * the Software, and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
16 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
17 * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
18 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
20 * OTHER DEALINGS IN THE SOFTWARE.
21 */
22#endregion
23
24
25
26// because this code was basically copied from Box2D
27// Copyright (c) 2006 Erin Catto http://www.gphysics.com
28#if UseDouble
29using Scalar = System.Double;
30#else
31using Scalar = System.Single;
32#endif
33using System;
34using System.Collections.ObjectModel;
35using System.Collections.Generic;
36
37using AdvanceMath;
38using AdvanceMath.Geometry2D;
39using Physics2DDotNet.Shapes;
40using Physics2DDotNet.Joints;
41
42
43
44namespace Physics2DDotNet.Solvers
45{
46#if !(WINDOWS_PHONE || XBOX)
47    [Serializable]
48#endif
49    sealed class SequentialImpulsesTag
50    {
51        public ALVector2D lastAccel;
52        public ALVector2D biasVelocity;
53        public Body body;
54        public SequentialImpulsesTag(Body body)
55        {
56            this.body = body;
57        }
58    }
59#if !(WINDOWS_PHONE || XBOX)
60    [Serializable]
61#endif
62    public sealed class SequentialImpulsesSolver : CollisionSolver
63    {
64        #region sub-classes
65#if !(WINDOWS_PHONE || XBOX)
66        [Serializable]
67#endif
68        sealed class ContactPoint : IContactPointInfo
69        {
70            public int id;
71            public Vector2D position;
72            public Vector2D normal;
73            public Scalar distance;
74            public Scalar Pn;
75            public Scalar Pt;
76            public Scalar Pnb;  // accumulated normal impulse for position bias
77            public Scalar massNormal;
78            public Scalar massTangent;
79            public Scalar bias;
80            public Vector2D r1;
81            public Vector2D r2;
82
83
84            Vector2D IContactPointInfo.Position
85            {
86                get { return position; }
87            }
88            Vector2D IContactPointInfo.Normal
89            {
90                get { return normal; }
91            }
92            Scalar IContactPointInfo.Distance
93            {
94                get { return distance; }
95            }
96        }
97#if !(WINDOWS_PHONE || XBOX)
98        [Serializable]
99#endif
100        sealed class Arbiter : IContact
101        {
102
103
104            static ContactPoint[] Empty = new ContactPoint[0];
105            static Scalar ZeroClamp(Scalar value)
106            {
107                return ((value < 0) ? (0) : (value));
108            }
109
110            public event EventHandler Updated;
111
112            public event EventHandler Ended;
113
114            ContactState state;
115
116
117            int lastUpdate;
118
119
120            CircleShape circle1;
121            CircleShape circle2;
122
123            LinkedList<ContactPoint> contacts;
124            ContactPoint[] contactsArray;
125
126            public Body body1;
127            public Body body2;
128            SequentialImpulsesTag tag1;
129            SequentialImpulsesTag tag2;
130
131            SequentialImpulsesSolver parent;
132            Scalar restitution;
133            bool ignoresCollisionResponse;
134
135
136            Scalar friction;
137            public Arbiter(SequentialImpulsesSolver parent, Body body1, Body body2)
138            {
139                if (body1.ID < body2.ID)
140                {
141                    this.body1 = body1;
142                    this.body2 = body2;
143                }
144                else
145                {
146                    this.body1 = body2;
147                    this.body2 = body1;
148                }
149                this.tag1 = (SequentialImpulsesTag)this.body1.SolverTag;
150                this.tag2 = (SequentialImpulsesTag)this.body2.SolverTag;
151                this.circle1 = this.body1.Shape as CircleShape;
152                this.circle2 = this.body2.Shape as CircleShape;
153                this.friction = MathHelper.Sqrt(
154                        this.body1.Coefficients.DynamicFriction *
155                        this.body2.Coefficients.DynamicFriction);
156                this.restitution = Math.Min(body1.Coefficients.Restitution, body2.Coefficients.Restitution);
157                this.parent = parent;
158                this.contacts = new LinkedList<ContactPoint>();
159                this.lastUpdate = -1;
160                this.state = ContactState.New;
161                this.ignoresCollisionResponse = body1.IgnoresCollisionResponse || body2.IgnoresCollisionResponse;
162            }
163
164
165            public bool IgnoresCollisionResponse
166            {
167                get { return ignoresCollisionResponse; }
168            }
169            public ContactState State
170            {
171                get { return state; }
172            }
173            public int LastUpdate
174            {
175                get { return lastUpdate; }
176            }
177            bool everCollided;
178
179            public void Update(TimeStep step)
180            {
181                if (lastUpdate != -1)
182                {
183                    if (lastUpdate != step.UpdateCount - 1)
184                    {
185                        state = ContactState.New;
186                    }
187                    else
188                    {
189                        state = ContactState.Old;
190                    }
191                }
192
193
194                if ((!body1.IsFrozen || !body2.IsFrozen))
195                {
196                    if (circle1 != null && circle2 != null &&
197                        !body1.IsTransformed && !body2.IsTransformed)
198                    {
199                        CollideCircles();
200                    }
201                    else
202                    {
203                        Collide();
204                    }
205                    UpdateContacts();
206                    if (Collided)
207                    {
208                        body1.IsFrozen = false;
209                        body2.IsFrozen = false;
210                        everCollided = true;
211                    }
212                }
213                if (Collided)
214                {
215                    lastUpdate = step.UpdateCount;
216                }
217
218                if (Updated != null &&
219                    contactsArray.Length != 0)
220                {
221                    Updated(this, EventArgs.Empty);
222                }
223            }
224            void UpdateContacts()
225            {
226                if (contacts.Count == 0)
227                {
228                    contactsArray = Empty;
229                    return;
230                }
231                if (contactsArray == null || contactsArray.Length != contacts.Count)
232                {
233                    contactsArray = new ContactPoint[contacts.Count];
234                }
235                contacts.CopyTo(contactsArray, 0);
236            }
237
238
239            void CollideCircles()
240            {
241                Vector2D center1 = Vector2D.Zero;
242                Vector2D center2 = Vector2D.Zero;
243                Vector2D.Transform(ref  body1.Matrices.ToWorld, ref center1, out center1);
244                Vector2D.Transform(ref  body2.Matrices.ToWorld, ref center2, out center2);
245                Vector2D normal;
246                Vector2D.Subtract(ref  center2, ref center1, out normal);
247                Scalar distance;
248                Vector2D.Normalize(ref normal, out distance, out normal);
249                Scalar depth = distance - (circle1.Radius + circle2.Radius);
250                if (depth > 0)
251                {
252                    contacts.Clear();
253                }
254                else
255                {
256                    ContactPoint contact;
257                    if (contacts.First == null)
258                    {
259                        contact = new ContactPoint();
260                        contacts.AddLast(contact);
261                    }
262                    else
263                    {
264                        contact = contacts.First.Value;
265                    }
266                    contact.distance = depth;
267                    contact.normal = normal;
268                    contact.position.X = center2.X - normal.X * circle2.Radius;
269                    contact.position.Y = center2.Y - normal.Y * circle2.Radius;
270                }
271            }
272            void Collide()
273            {
274                BoundingRectangle bb1 = body1.Rectangle;
275                BoundingRectangle bb2 = body2.Rectangle;
276                BoundingRectangle targetArea;
277                BoundingRectangle.FromIntersection(ref bb1, ref bb2, out targetArea);
278                LinkedListNode<ContactPoint> node = contacts.First;
279                if (!body2.IgnoreVertexes &&
280                    body1.Shape.CanGetIntersection)
281                {
282                    Collide(ref node, this.body1, this.body2, false, ref targetArea);
283                }
284                if (!body1.IgnoreVertexes &&
285                    body2.Shape.CanGetIntersection)
286                {
287                    Collide(ref node, this.body2, this.body1, true, ref targetArea);
288                }
289            }
290            void Collide(ref LinkedListNode<ContactPoint> node, Body b1, Body b2, bool inverse, ref BoundingRectangle targetArea)
291            {
292                Vector2D[] vertexes = b2.Shape.Vertexes;
293                Vector2D[] normals = b2.Shape.VertexNormals;
294
295
296                Matrix2x3 b2ToWorld = b2.Matrices.ToWorld;
297                Matrix2x3 b1ToBody = b1.Matrices.ToBody;
298                Matrix2x2 b1ToWorldNormal = b1.Matrices.ToWorldNormal;
299
300                Matrix2x2 normalM;
301                Matrix2x2.Multiply(ref b1.Matrices.ToBodyNormal, ref b2.Matrices.ToWorldNormal, out normalM);
302
303                IntersectionInfo info = IntersectionInfo.Zero;
304                ContainmentType contains;
305                ContactPoint contact;
306
307                for (int index = 0; index < vertexes.Length; ++index)
308                {
309                    Vector2D worldVertex;
310                    Vector2D.Transform(ref b2ToWorld, ref vertexes[index], out worldVertex);
311                    targetArea.Contains(ref worldVertex, out contains);
312                    bool isBad = (contains != ContainmentType.Contains);
313                    if (!isBad)
314                    {
315                        Vector2D bodyVertex;
316                        Vector2D.Transform(ref b1ToBody, ref worldVertex, out bodyVertex);
317                        isBad = !b1.Shape.TryGetIntersection(bodyVertex, out info);
318                        if (!isBad && normals != null &&
319                            !body1.IgnoresCollisionResponse &&
320                            !body2.IgnoresCollisionResponse)
321                        {
322                            Vector2D normal;
323                            Vector2D.Transform(ref normalM, ref  normals[index], out normal);
324                            Scalar temp;
325                            temp = Vector2D.Dot(info.Normal, normal);
326                            isBad = temp >= 0;
327                        }
328                    }
329
330                    int Id = (inverse) ? (index) : ((-vertexes.Length + index));
331                    while (node != null && node.Value.id < Id) { node = node.Next; }
332
333                    if (isBad)
334                    {
335                        if (node != null && node.Value.id == Id)
336                        {
337                            LinkedListNode<ContactPoint> nextNode = node.Next;
338                            contacts.Remove(node);
339                            node = nextNode;
340                        }
341                    }
342                    else
343                    {
344                        if (node == null)
345                        {
346                            contact = new ContactPoint();
347                            contact.id = Id;
348                            contacts.AddLast(contact);
349                        }
350                        else if (node.Value.id == Id)
351                        {
352                            contact = node.Value;
353                            node = node.Next;
354                            if (!parent.warmStarting)
355                            {
356                                contact.Pn = 0;
357                                contact.Pt = 0;
358                                contact.Pnb = 0;
359                            }
360                        }
361                        else
362                        {
363                            contact = new ContactPoint();
364                            contact.id = Id;
365                            contacts.AddBefore(node, contact);
366                        }
367                        contact.normal = Vector2D.Transform(b1ToWorldNormal, info.Normal);
368                        contact.distance = info.Distance;
369                        contact.position = worldVertex;
370                        if (inverse)
371                        {
372                            Vector2D.Negate(ref contact.normal, out contact.normal);
373                        }
374                        Vector2D.Normalize(ref contact.normal, out contact.normal);
375                    }
376                }
377            }
378            public void PreApply(Scalar dtInv)
379            {
380
381                Scalar mass1Inv = body1.Mass.MassInv;
382                Scalar I1Inv = body1.Mass.MomentOfInertiaInv;
383                Scalar mass2Inv = body2.Mass.MassInv;
384                Scalar I2Inv = body2.Mass.MomentOfInertiaInv;
385
386                for (int index = 0; index < contactsArray.Length; ++index)
387                {
388                    ContactPoint c = contactsArray[index];
389                    Vector2D.Subtract(ref c.position, ref body1.State.Position.Linear, out c.r1);
390                    Vector2D.Subtract(ref c.position, ref body2.State.Position.Linear, out c.r2);
391
392                    // Precompute normal mass, tangent mass, and bias.
393                    PhysicsHelper.GetMassNormal(
394                        ref c.r1, ref c.r2,
395                        ref c.normal,
396                        ref mass1Inv, ref I1Inv,
397                        ref mass2Inv, ref I2Inv,
398                        out c.massNormal);
399
400                    Vector2D tangent;
401                    PhysicsHelper.GetTangent(ref c.normal, out tangent);
402
403                    PhysicsHelper.GetMassNormal(
404                        ref c.r1, ref c.r2,
405                        ref tangent,
406                        ref mass1Inv, ref I1Inv,
407                        ref mass2Inv, ref I2Inv,
408                        out c.massTangent);
409
410                    if (parent.positionCorrection)
411                    {
412                        c.bias = -parent.biasFactor * dtInv * Math.Min(0.0f, c.distance + parent.allowedPenetration);
413                    }
414                    else
415                    {
416                        c.bias = 0;
417                    }
418                    if (parent.accumulateImpulses)
419                    {
420                        // Apply normal + friction impulse
421                        Vector2D vect1, vect2, P;
422
423                        Scalar temp = (1 + this.restitution) * c.Pn;
424                        Vector2D.Multiply(ref c.normal, ref temp, out vect1);
425                        Vector2D.Multiply(ref tangent, ref c.Pt, out vect2);
426                        Vector2D.Add(ref vect1, ref vect2, out P);
427
428                        PhysicsHelper.SubtractImpulse(
429                            ref body1.State.Velocity,
430                            ref P,
431                            ref c.r1,
432                            ref mass1Inv,
433                            ref I1Inv);
434
435                        PhysicsHelper.AddImpulse(
436                            ref body2.State.Velocity,
437                            ref P,
438                            ref c.r2,
439                            ref mass2Inv,
440                            ref I2Inv);
441                    }
442                    // Initialize bias impulse to zero.
443                    c.Pnb = 0;
444                }
445                body1.ApplyProxy();
446                body2.ApplyProxy();
447            }
448            public void Apply()
449            {
450                Body b1 = body1;
451                Body b2 = body2;
452
453                Scalar mass1Inv = b1.Mass.MassInv;
454                Scalar I1Inv = b1.Mass.MomentOfInertiaInv;
455                Scalar mass2Inv = b2.Mass.MassInv;
456                Scalar I2Inv = b2.Mass.MomentOfInertiaInv;
457
458                PhysicsState state1 = b1.State;
459                PhysicsState state2 = b2.State;
460
461                for (int index = 0; index < contactsArray.Length; ++index)
462                {
463                    ContactPoint c = contactsArray[index];
464
465                    // Relative velocity at contact
466                    Vector2D dv;
467                    PhysicsHelper.GetRelativeVelocity(
468                        ref state1.Velocity,
469                        ref state2.Velocity,
470                        ref c.r1, ref c.r2, out dv);
471
472                    // Compute normal impulse
473                    Scalar vn;
474                    Vector2D.Dot(ref dv, ref c.normal, out vn);
475                    //Scalar vn = Vector2D.Dot(dv, c.normal);
476
477                    Scalar dPn;
478                    if (parent.splitImpulse)
479                    {
480                        dPn = c.massNormal * (-vn);
481                    }
482                    else
483                    {
484                        dPn = c.massNormal * (c.bias - vn);
485                    }
486
487
488                    if (parent.accumulateImpulses)
489                    {
490                        // Clamp the accumulated impulse
491                        Scalar Pn0 = c.Pn;
492                        c.Pn = ZeroClamp(Pn0 + dPn);
493                        //c.Pn = Math.Max(Pn0 + dPn, 0.0f);
494                        dPn = c.Pn - Pn0;
495                    }
496                    else
497                    {
498                        //dPn = Math.Max(dPn, 0.0f);
499                        dPn = ZeroClamp(dPn);
500                    }
501
502                    // Apply contact impulse
503                    Vector2D Pn;
504                    Vector2D.Multiply(ref  c.normal, ref dPn, out Pn);
505                    //Vector2D Pn = dPn * c.normal;
506
507                    PhysicsHelper.SubtractImpulse(
508                        ref state1.Velocity,
509                        ref Pn,
510                        ref c.r1,
511                        ref mass1Inv,
512                        ref I1Inv);
513
514                    PhysicsHelper.AddImpulse(
515                        ref state2.Velocity,
516                        ref Pn,
517                        ref c.r2,
518                        ref mass2Inv,
519                        ref I2Inv);
520
521
522                    if (parent.splitImpulse)
523                    {
524                        // Compute bias impulse
525                        PhysicsHelper.GetRelativeVelocity(
526                            ref tag1.biasVelocity,
527                            ref tag2.biasVelocity,
528                            ref c.r1, ref c.r2, out dv);
529
530
531
532                        Scalar vnb;
533                        Vector2D.Dot(ref dv, ref c.normal, out vnb);
534                        //Scalar vnb = Vector2D.Dot(dv, c.normal);
535
536                        Scalar dPnb = c.massNormal * (c.bias - vnb);
537                        Scalar Pnb0 = c.Pnb;
538                        c.Pnb = ZeroClamp(Pnb0 + dPnb);
539                        // c.Pnb = Math.Max(Pnb0 + dPnb, 0.0f);
540                        dPnb = c.Pnb - Pnb0;
541
542                        Vector2D Pb;
543                        Vector2D.Multiply(ref dPnb, ref c.normal, out Pb);
544                        //Vector2D Pb = dPnb * c.normal;
545
546
547                        PhysicsHelper.SubtractImpulse(
548                            ref tag1.biasVelocity,
549                            ref Pb,
550                            ref c.r1,
551                            ref mass1Inv,
552                            ref I1Inv);
553
554                        PhysicsHelper.AddImpulse(
555                            ref tag2.biasVelocity,
556                            ref Pb,
557                            ref c.r2,
558                            ref mass2Inv,
559                            ref I2Inv);
560                    }
561
562                    // Relative velocity at contact
563
564                    PhysicsHelper.GetRelativeVelocity(
565                        ref state1.Velocity,
566                        ref state2.Velocity,
567                        ref c.r1, ref c.r2, out dv);
568
569
570                    Vector2D tangent;
571                    PhysicsHelper.GetTangent(ref c.normal, out tangent);
572
573                    Scalar vt;
574                    Vector2D.Dot(ref dv, ref tangent, out vt);
575                    //Scalar vt = Vector2D.Dot(dv, tangent);
576                    Scalar dPt = c.massTangent * (-vt);
577
578
579
580
581                    if (parent.accumulateImpulses)
582                    {
583                        // Compute friction impulse
584                        Scalar maxPt = friction * c.Pn;
585                        // Clamp friction
586                        Scalar oldTangentImpulse = c.Pt;
587                        c.Pt = MathHelper.Clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
588                        dPt = c.Pt - oldTangentImpulse;
589                    }
590                    else
591                    {
592                        // Compute friction impulse
593                        Scalar maxPt = friction * dPn;
594                        dPt = MathHelper.Clamp(dPt, -maxPt, maxPt);
595                    }
596
597
598                    // Apply contact impulse
599                    Vector2D Pt;
600                    Vector2D.Multiply(ref tangent, ref dPt, out Pt);
601
602                    //Vector2D Pt = dPt * tangent;
603
604                    PhysicsHelper.SubtractImpulse(
605                        ref state1.Velocity,
606                        ref Pt,
607                        ref c.r1,
608                        ref mass1Inv,
609                        ref I1Inv);
610
611                    PhysicsHelper.AddImpulse(
612                        ref state2.Velocity,
613                        ref Pt,
614                        ref c.r2,
615                        ref mass2Inv,
616                        ref I2Inv);
617                }
618                body1.ApplyProxy();
619                body2.ApplyProxy();
620            }
621            public bool Collided
622            {
623                get { return contactsArray != null && contactsArray.Length > 0; }
624            }
625            public void OnRemoved()
626            {
627                if (everCollided)
628                {
629                    body1.IsFrozen = false;
630                    body2.IsFrozen = false;
631                   // body1.idleCount -= 3;
632                   // body2.idleCount -= 3;
633                }
634                this.state = ContactState.Ended;
635                if (Ended != null) { Ended(this, EventArgs.Empty); }
636            }
637
638
639
640            Body IContact.Body1
641            {
642                get { return body1; }
643            }
644
645            Body IContact.Body2
646            {
647                get { return body2; }
648            }
649
650            ReadOnlyCollection<IContactPointInfo> IContact.Points
651            {
652                get
653                {
654                    return new ReadOnlyCollection<IContactPointInfo>(
655                        new Physics2DDotNet.Collections.ImplicitCastCollection<IContactPointInfo, ContactPoint>(contactsArray));
656                }
657            }
658
659
660
661
662
663        }
664        #endregion
665        #region static
666        static bool IsJointRemoved(ISequentialImpulsesJoint joint)
667        {
668            return !joint.IsAdded;
669        }
670        static bool IsTagRemoved(SequentialImpulsesTag tag)
671        {
672            return !tag.body.IsAdded;
673        }
674        #endregion
675        #region fields
676        Dictionary<long, Arbiter> arbiters;
677        List<ISequentialImpulsesJoint> siJoints;
678        List<SequentialImpulsesTag> tags;
679        bool splitImpulse = true;
680        bool accumulateImpulses = true;
681        bool warmStarting = true;
682        bool positionCorrection = true ;
683        bool freezing = false;
684
685
686
687        int freezeTimeout = 50;
688        ALVector2D freezeVelocityTolerance = new ALVector2D(.1f, 5, 5);
689
690        int removeTimout = 5;
691
692
693        Scalar biasFactor = 0.7f;
694        Scalar allowedPenetration = 0.1f;
695        int iterations = 12;
696
697        List<long> empty = new List<long>();
698        List<Arbiter> arbs2 = new List<Arbiter>();
699
700        #endregion
701        #region constructors
702        public SequentialImpulsesSolver()
703        {
704            arbiters = new Dictionary<long, Arbiter>();
705            siJoints = new List<ISequentialImpulsesJoint>();
706            tags = new List<SequentialImpulsesTag>();
707        }
708        #endregion
709        #region properties
710        public bool PositionCorrection
711        {
712            get { return positionCorrection; }
713            set { positionCorrection = value; }
714        }
715        public bool AccumulateImpulses
716        {
717            get { return accumulateImpulses; }
718            set { accumulateImpulses = value; }
719        }
720        public bool SplitImpulse
721        {
722            get { return splitImpulse; }
723            set { splitImpulse = value; }
724        }
725        public bool WarmStarting
726        {
727            get { return warmStarting; }
728            set { warmStarting = value; }
729        }
730        public Scalar BiasFactor
731        {
732            get { return biasFactor; }
733            set { biasFactor = value; }
734        }
735        public Scalar AllowedPenetration
736        {
737            get { return allowedPenetration; }
738            set { allowedPenetration = value; }
739        }
740        public int Iterations
741        {
742            get { return iterations; }
743            set { iterations = value; }
744        }
745
746        public bool Freezing
747        {
748            get { return freezing; }
749            set { freezing = value; }
750        }
751        public int FreezeTimeout
752        {
753            get { return freezeTimeout; }
754            set { freezeTimeout = value; }
755        }
756        public ALVector2D FreezeVelocityTolerance
757        {
758            get { return freezeVelocityTolerance; }
759            set { freezeVelocityTolerance = value; }
760        }
761
762        #endregion
763        #region methods
764        protected internal override bool TryGetIntersection(TimeStep step, Body first, Body second, out IContact contact)
765        {
766            long id = PairID.GetId(first.ID, second.ID);
767            Arbiter arbiter;
768            if (arbiters.TryGetValue(id, out arbiter))
769            {
770                arbiter.Update(step);
771               /* if (!arbiter.Collided)
772                {
773                    arbiter.OnRemoved();
774                    arbiters.Remove(id);
775                }*/
776            }
777            else
778            {
779                arbiter = new Arbiter(this, first, second);
780                arbiter.Update(step);
781                //if (arbiter.Collided)
782                //{
783                    arbiters.Add(id, arbiter);
784                //}
785            }
786            contact = arbiter;
787            return arbiter.Collided;
788        }
789
790        Arbiter[] RemoveEmpty(TimeStep step)
791        {
792            foreach (KeyValuePair<long, Arbiter> pair in arbiters)
793            {
794                Arbiter value = pair.Value;
795                //if (!value.Collided || value.LastUpdate != step.UpdateCount)
796                if (value.LastUpdate + removeTimout < step.UpdateCount)
797                {
798                    pair.Value.OnRemoved();
799                    empty.Add(pair.Key);
800                }
801                else if (value.IgnoresCollisionResponse ||
802                    value.body1.IsFrozen &&
803                    value.body2.IsFrozen)
804                {
805
806                }
807                else if (value.Collided && value.LastUpdate == step.UpdateCount)
808                {
809                    arbs2.Add(value);
810                }
811            }
812            for (int index = 0; index < empty.Count; ++index)
813            {
814                arbiters.Remove(empty[index]);
815            }
816            Arbiter[] result = arbs2.ToArray();
817            empty.Clear();
818            arbs2.Clear();
819            return result;
820        }
821
822 
823        protected internal override void Solve(TimeStep step)
824        {
825            Detect(step);
826            Arbiter[] arbs = RemoveEmpty(step);
827            this.Engine.RunLogic(step);
828            if (freezing)
829            {
830                for (int index = 0; index < siJoints.Count; ++index)
831                {
832                    siJoints[index].CheckFrozen();
833                }
834            }
835            for (int index = 0; index < tags.Count; ++index)
836            {
837                SequentialImpulsesTag tag = tags[index];
838                tag.biasVelocity = ALVector2D.Zero;
839
840                if (freezing)
841                {
842                    bool accelSame = tag.body.State.Acceleration == tag.lastAccel;
843
844                    ALVector2D vel = tag.body.State.Velocity;
845                    ALVector2D force = tag.body.State.ForceAccumulator;
846
847
848                    bool isVelZero =
849                        Math.Abs(vel.X) < freezeVelocityTolerance.X &&
850                        Math.Abs(vel.Y) < freezeVelocityTolerance.Y &&
851                        Math.Abs(vel.Angular) < freezeVelocityTolerance.Angular;
852                    bool isForceZero = tag.body.State.ForceAccumulator == ALVector2D.Zero;
853
854
855                    if (accelSame && isVelZero && isForceZero)
856                    {
857                        if (tag.body.Joints.Count == 0)
858                        {
859                            tag.body.idleCount++;
860                        }
861                        if (tag.body.idleCount > freezeTimeout)
862                        {
863                            tag.body.idleCount = freezeTimeout;
864                            tag.body.IsFrozen = true;
865                            tag.body.State.Velocity = ALVector2D.Zero;
866                        }
867                    }
868                    else
869                    {
870                        tag.body.IsFrozen = false;
871                        tag.body.idleCount = 0;
872                    }
873                    tag.lastAccel = tag.body.State.Acceleration;
874                    if (tag.body.IsFrozen)
875                    {
876                        tag.body.State.ForceAccumulator = ALVector2D.Zero;
877                        tag.body.State.Acceleration = ALVector2D.Zero;
878                    }
879                }
880
881                tag.body.UpdateVelocity(step);
882                tag.body.ClearForces();
883            }
884            for (int index = 0; index < arbs.Length; ++index)
885            {
886                arbs[index].PreApply(step.DtInv);
887            }
888            for (int index = 0; index < siJoints.Count; ++index)
889            {
890                siJoints[index].PreStep(step);
891            }
892            for (int i = 0; i < iterations; ++i)
893            {
894                for (int index = 0; index < arbs.Length; ++index)
895                {
896                    arbs[index].Apply();
897                }
898                for (int index = 0; index < siJoints.Count; ++index)
899                {
900                    siJoints[index].ApplyImpulse();
901                }
902            }
903            for (int index = 0; index < tags.Count; ++index)
904            {
905                SequentialImpulsesTag tag = tags[index];
906                if (splitImpulse)
907                {
908                    tag.body.UpdatePosition(step, ref tag.biasVelocity);
909                }
910                else
911                {
912                    tag.body.UpdatePosition(step);
913                }
914                tag.body.ApplyPosition();
915            }
916        }
917        protected internal override void AddBodyRange(List<Body> collection)
918        {
919            foreach (Body item in collection)
920            {
921                if (item.SolverTag == null)
922                {
923                    SequentialImpulsesTag tag = new SequentialImpulsesTag(item);
924                    SetTag(item, tag);
925                    tags.Add(tag);
926                }
927                else
928                {
929                    tags.Add((SequentialImpulsesTag)item.SolverTag);
930                }
931            }
932        }
933        protected internal override void AddJointRange(List<Joint> collection)
934        {
935            ISequentialImpulsesJoint[] newJoints = new ISequentialImpulsesJoint[collection.Count];
936            for (int index = 0; index < newJoints.Length; ++index)
937            {
938                newJoints[index] = (ISequentialImpulsesJoint)collection[index];
939            }
940            siJoints.AddRange(newJoints);
941        }
942        protected internal override void Clear()
943        {
944            arbiters.Clear();
945            siJoints.Clear();
946            tags.Clear();
947        }
948        protected internal override void RemoveExpiredJoints()
949        {
950            siJoints.RemoveAll(IsJointRemoved);
951        }
952        protected internal override void RemoveExpiredBodies()
953        {
954            tags.RemoveAll(IsTagRemoved);
955        }
956        protected internal override void CheckJoint(Joint joint)
957        {
958            if (!(joint is ISequentialImpulsesJoint))
959            {
960                throw new ArgumentException("The joint must implement ISequentialImpulsesJoint to be added to this solver.");
961            }
962        } 
963        #endregion
964    }
965}
Note: See TracBrowser for help on using the repository browser.