#
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 |

29 | using Scalar = System.Double; |

30 | #else |

31 | using Scalar = System.Single; |

32 | #endif |

33 | using System; |

34 | using System.Collections.ObjectModel; |

35 | |

36 | using AdvanceMath; |

37 | |

38 | |

39 | |

40 | |

41 | |

42 | |

43 | namespace 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.