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

Revision 4507, 4.1 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
27#if UseDouble
28using Scalar = System.Double;
29#else
30using Scalar = System.Single;
31#endif
32using System;
33using System.Collections.ObjectModel;
34
35using AdvanceMath;
36
37
38
39namespace Physics2DDotNet.Joints
40{
41    /// <summary>
42    /// A Joint between 2 Bodies that will keep the Angles between the 2 bodies at a certain amount.
43    /// </summary>
44#if !(WINDOWS_PHONE || XBOX)
45    [Serializable]
46#endif
47    public sealed class AngleJoint : Joint, Solvers.ISequentialImpulsesJoint
48    {
49        Body body1;
50        Body body2;
51        Scalar angle;
52
53        Scalar bias;
54        Scalar biasFactor;
55        Scalar softness;
56        Scalar M;
57
58        public AngleJoint(Body body1, Body body2, Lifespan lifetime)
59            : base(lifetime)
60        {
61            if (body1 == null) { throw new ArgumentNullException("body1"); }
62            if (body2 == null) { throw new ArgumentNullException("body2"); }
63            if (body1 == body2) { throw new ArgumentException("You cannot add a joint to a body to itself"); }
64            this.body1 = body1;
65            this.body2 = body2;
66            this.angle = MathHelper.ClampAngle(body1.State.Position.Angular - body2.State.Position.Angular);
67            this.softness = 0.001f;
68            this.biasFactor = 0.2f;
69        }
70
71
72        public Scalar Angle
73        {
74            get { return angle; }
75            set { MathHelper.ClampAngle(ref value, out angle); }
76        }
77        public Scalar BiasFactor
78        {
79            get { return biasFactor; }
80            set { biasFactor = value; }
81        }
82        public Scalar Softness
83        {
84            get { return softness; }
85            set { softness = value; }
86        }
87        public override ReadOnlyCollection<Body> Bodies
88        {
89            get { return new ReadOnlyCollection<Body>(new Body[2] { body1, body2 }); }
90        }
91
92        public override void CheckFrozen()
93        {
94            if (body1.IsFrozen ^ body2.IsFrozen)
95            {
96                body1.IsFrozen = false;
97                body2.IsFrozen = false;
98                body1.idleCount = 0;
99                body2.idleCount = 0;
100            }
101        }
102
103        void Solvers.ISequentialImpulsesJoint.PreStep(TimeStep step)
104        {
105            Scalar difference = MathHelper.ClampAngle(body1.State.Position.Angular - body2.State.Position.Angular) - angle;
106            bias = -biasFactor * step.DtInv * difference;
107            M = (1 - softness) / (body1.Mass.MomentOfInertiaInv + body2.Mass.MomentOfInertiaInv);
108        }
109        void Solvers.ISequentialImpulsesJoint.ApplyImpulse()
110        {
111            Scalar angularImpulse = M * (bias + (body2.State.Velocity.Angular - body1.State.Velocity.Angular));
112            body1.State.Velocity.Angular += body1.Mass.MomentOfInertiaInv * angularImpulse;
113            body2.State.Velocity.Angular -= body2.Mass.MomentOfInertiaInv * angularImpulse;
114        }
115    }
116}
Note: See TracBrowser for help on using the repository browser.