source: 2014/24/EemeliK/Zombieland/Jypeli/Physics2DDotNet/Joints/FixedHingeJoint.cs @ 5974

Revision 5974, 6.9 KB checked in by empaheik, 5 years ago (diff)
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;
35
36using AdvanceMath;
37
38
39
40
41
42
43namespace Physics2DDotNet.Joints
44{
45    /// <summary>
46    /// A joint that makes a single Body Pivot around an Anchor.
47    /// </summary>
48#if !(WINDOWS_PHONE || XBOX)
49    [Serializable]
50#endif
51    public sealed class FixedHingeJoint : Joint, Solvers.ISequentialImpulsesJoint
52    {
53        Solvers.SequentialImpulsesSolver solver;
54        Body body;
55
56
57        Matrix2x2 M;
58        Vector2D localAnchor1;
59        Vector2D anchor;
60        Vector2D r1;
61        Vector2D bias;
62        Vector2D accumulatedImpulse;
63        Scalar biasFactor;
64        Scalar softness;
65        Scalar distanceTolerance;
66
67
68        public FixedHingeJoint(Body body, Vector2D anchor, Lifespan lifetime)
69            : base(lifetime)
70        {
71            if (body == null) { throw new ArgumentNullException("body"); }
72            this.body = body;
73            this.anchor = anchor;
74            body.ApplyPosition();
75            Vector2D.Transform(ref body.Matrices.ToBody, ref anchor, out this.localAnchor1);
76            this.softness = 0.001f;
77            this.biasFactor = 0.2f;
78            this.distanceTolerance = Scalar.PositiveInfinity;
79        }
80
81
82        public Vector2D Anchor
83        {
84            get { return anchor; }
85            set { anchor = value; }
86        }
87
88        public Scalar BiasFactor
89        {
90            get { return biasFactor; }
91            set { biasFactor = value; }
92        }
93        public Scalar Softness
94        {
95            get { return softness; }
96            set { softness = value; }
97        }
98        /// <summary>
99        /// The distance the joint can stretch before breaking.
100        /// </summary>
101        public Scalar DistanceTolerance
102        {
103            get { return distanceTolerance; }
104            set
105            {
106                if (value <= 0) { throw new ArgumentOutOfRangeException("value"); }
107                distanceTolerance = value;
108            }
109        }
110        public override ReadOnlyCollection<Body> Bodies
111        {
112            get { return new ReadOnlyCollection<Body>(new Body[1] { body }); }
113        }
114        public override void CheckFrozen()
115        {
116           
117        }
118
119        protected override void OnAdded(EventArgs e)
120        {
121            this.solver = (Solvers.SequentialImpulsesSolver)Engine.Solver;
122            base.OnAdded(e);
123        }
124        void Solvers.ISequentialImpulsesJoint.PreStep(TimeStep step)
125        {
126
127            Scalar mass1Inv = body.Mass.MassInv;
128            Scalar inertia1Inv = body.Mass.MomentOfInertiaInv;
129
130            // Pre-compute anchors, mass matrix, and bias.
131
132            Vector2D.TransformNormal(ref body.Matrices.ToWorld, ref localAnchor1, out r1);
133           
134            // deltaV = deltaV0 + K * impulse
135            // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
136            //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
137            //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
138
139            Matrix2x2 K;
140            K.m00 = mass1Inv;
141            K.m11 = mass1Inv;
142
143            K.m00 += inertia1Inv * r1.Y * r1.Y;
144            K.m01 = -inertia1Inv * r1.X * r1.Y;
145            K.m10 = -inertia1Inv * r1.X * r1.Y;
146            K.m11 += inertia1Inv * r1.X * r1.X;
147
148            K.m00 += softness;
149            K.m11 += softness;
150
151            Matrix2x2.Invert(ref K, out M);
152
153
154            Vector2D dp;
155            Vector2D.Add(ref body.State.Position.Linear, ref r1, out dp);
156            Vector2D.Subtract(ref anchor, ref dp, out dp);
157           
158            if (!Scalar.IsPositiveInfinity(distanceTolerance) &&
159                dp.MagnitudeSq > distanceTolerance * distanceTolerance)
160            {
161                this.Lifetime.IsExpired = true;
162            }
163
164            if (solver.PositionCorrection)
165            {
166                //bias = -0.1f * dtInv * dp;
167                Scalar flt = -biasFactor * step.DtInv;
168                Vector2D.Multiply(ref dp, ref flt, out bias);
169            }
170            else
171            {
172                bias = Vector2D.Zero;
173            }
174            if (solver.WarmStarting)
175            {
176                PhysicsHelper.SubtractImpulse(
177                    ref body.State.Velocity, ref accumulatedImpulse,
178                    ref r1, ref mass1Inv, ref inertia1Inv);
179            }
180            else
181            {
182                accumulatedImpulse = Vector2D.Zero;
183            }
184            body.ApplyProxy();
185
186        }
187        void Solvers.ISequentialImpulsesJoint.ApplyImpulse()
188        {
189            Scalar mass1Inv = body.Mass.MassInv;
190            Scalar inertia1Inv = body.Mass.MomentOfInertiaInv;
191
192            Vector2D dv;
193            PhysicsHelper.GetRelativeVelocity(ref body.State.Velocity, ref r1, out dv);
194
195
196            Vector2D impulse;
197            impulse.X = bias.X - dv.X - softness * accumulatedImpulse.X;
198            impulse.Y = bias.Y - dv.Y - softness * accumulatedImpulse.Y;
199            Vector2D.Transform(ref  M, ref impulse, out impulse);
200            //impulse = M * (bias - dv - softness * P);
201
202
203            PhysicsHelper.SubtractImpulse(
204                ref body.State.Velocity, ref impulse,
205                ref r1, ref mass1Inv, ref inertia1Inv);
206
207
208            Vector2D.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse);
209            body.ApplyProxy();
210
211        }
212    }
213}
Note: See TracBrowser for help on using the repository browser.