pbox2d 0.6.0-java → 0.8.0-java

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (154) hide show
  1. checksums.yaml +4 -4
  2. data/.mvn/extensions.xml +8 -0
  3. data/.mvn/wrapper/maven-wrapper.properties +1 -0
  4. data/.travis.yml +23 -0
  5. data/CHANGELOG.md +8 -0
  6. data/README.md +7 -7
  7. data/Rakefile +1 -2
  8. data/lib/box2d.jar +0 -0
  9. data/lib/pbox2d/version.rb +1 -1
  10. data/lib/pbox2d.rb +1 -0
  11. data/pbox2d.gemspec +6 -11
  12. data/pom.rb +59 -0
  13. data/pom.xml +82 -73
  14. data/src/org/jbox2d/JBox2D.gwt.xml +12 -0
  15. data/src/org/jbox2d/callbacks/ContactAdaptor.java +27 -0
  16. data/src/org/jbox2d/callbacks/ContactFilter.java +59 -0
  17. data/src/org/jbox2d/callbacks/ContactImpulse.java +42 -0
  18. data/src/org/jbox2d/callbacks/ContactListener.java +87 -0
  19. data/src/org/jbox2d/callbacks/DebugDraw.java +297 -0
  20. data/src/org/jbox2d/callbacks/DestructionListener.java +53 -0
  21. data/src/org/jbox2d/callbacks/PairCallback.java +29 -0
  22. data/src/org/jbox2d/callbacks/ParticleDestructionListener.java +20 -0
  23. data/src/org/jbox2d/callbacks/ParticleQueryCallback.java +19 -0
  24. data/src/org/jbox2d/callbacks/ParticleRaycastCallback.java +19 -0
  25. data/src/org/jbox2d/callbacks/QueryCallback.java +45 -0
  26. data/src/org/jbox2d/callbacks/RayCastCallback.java +55 -0
  27. data/src/org/jbox2d/callbacks/TreeCallback.java +42 -0
  28. data/src/org/jbox2d/callbacks/TreeRayCastCallback.java +44 -0
  29. data/src/org/jbox2d/collision/AABB.java +338 -0
  30. data/src/org/jbox2d/collision/Collision.java +1444 -0
  31. data/src/org/jbox2d/collision/ContactID.java +106 -0
  32. data/src/org/jbox2d/collision/Distance.java +773 -0
  33. data/src/org/jbox2d/collision/DistanceInput.java +41 -0
  34. data/src/org/jbox2d/collision/DistanceOutput.java +43 -0
  35. data/src/org/jbox2d/collision/Manifold.java +116 -0
  36. data/src/org/jbox2d/collision/ManifoldPoint.java +104 -0
  37. data/src/org/jbox2d/collision/RayCastInput.java +47 -0
  38. data/src/org/jbox2d/collision/RayCastOutput.java +46 -0
  39. data/src/org/jbox2d/collision/TimeOfImpact.java +526 -0
  40. data/src/org/jbox2d/collision/WorldManifold.java +200 -0
  41. data/src/org/jbox2d/collision/broadphase/BroadPhase.java +92 -0
  42. data/src/org/jbox2d/collision/broadphase/BroadPhaseStrategy.java +88 -0
  43. data/src/org/jbox2d/collision/broadphase/DefaultBroadPhaseBuffer.java +268 -0
  44. data/src/org/jbox2d/collision/broadphase/DynamicTree.java +883 -0
  45. data/src/org/jbox2d/collision/broadphase/DynamicTreeFlatNodes.java +873 -0
  46. data/src/org/jbox2d/collision/broadphase/DynamicTreeNode.java +54 -0
  47. data/src/org/jbox2d/collision/broadphase/Pair.java +46 -0
  48. data/src/org/jbox2d/collision/shapes/ChainShape.java +264 -0
  49. data/src/org/jbox2d/collision/shapes/CircleShape.java +207 -0
  50. data/src/org/jbox2d/collision/shapes/EdgeShape.java +254 -0
  51. data/src/org/jbox2d/collision/shapes/MassData.java +105 -0
  52. data/src/org/jbox2d/collision/shapes/PolygonShape.java +718 -0
  53. data/src/org/jbox2d/collision/shapes/Shape.java +136 -0
  54. data/src/org/jbox2d/collision/shapes/ShapeType.java +32 -0
  55. data/src/org/jbox2d/common/BufferUtils.java +209 -0
  56. data/src/org/jbox2d/common/Color3f.java +88 -0
  57. data/src/org/jbox2d/common/IViewportTransform.java +133 -0
  58. data/src/org/jbox2d/common/Mat22.java +609 -0
  59. data/src/org/jbox2d/common/Mat33.java +290 -0
  60. data/src/org/jbox2d/common/MathUtils.java +335 -0
  61. data/src/org/jbox2d/common/OBBViewportTransform.java +174 -0
  62. data/src/org/jbox2d/common/PlatformMathUtils.java +46 -0
  63. data/src/org/jbox2d/common/RaycastResult.java +37 -0
  64. data/src/org/jbox2d/common/Rot.java +150 -0
  65. data/src/org/jbox2d/common/Settings.java +246 -0
  66. data/src/org/jbox2d/common/Sweep.java +116 -0
  67. data/src/org/jbox2d/common/Timer.java +46 -0
  68. data/src/org/jbox2d/common/Transform.java +203 -0
  69. data/src/org/jbox2d/common/Vec2.java +388 -0
  70. data/src/org/jbox2d/common/Vec3.java +170 -0
  71. data/src/org/jbox2d/dynamics/Body.java +1246 -0
  72. data/src/org/jbox2d/dynamics/BodyDef.java +382 -0
  73. data/src/org/jbox2d/dynamics/BodyType.java +41 -0
  74. data/src/org/jbox2d/dynamics/ContactManager.java +293 -0
  75. data/src/org/jbox2d/dynamics/Filter.java +62 -0
  76. data/src/org/jbox2d/dynamics/Fixture.java +454 -0
  77. data/src/org/jbox2d/dynamics/FixtureDef.java +214 -0
  78. data/src/org/jbox2d/dynamics/FixtureProxy.java +38 -0
  79. data/src/org/jbox2d/dynamics/Island.java +602 -0
  80. data/src/org/jbox2d/dynamics/Profile.java +97 -0
  81. data/src/org/jbox2d/dynamics/SolverData.java +33 -0
  82. data/src/org/jbox2d/dynamics/TimeStep.java +46 -0
  83. data/src/org/jbox2d/dynamics/World.java +2075 -0
  84. data/src/org/jbox2d/dynamics/contacts/ChainAndCircleContact.java +57 -0
  85. data/src/org/jbox2d/dynamics/contacts/ChainAndPolygonContact.java +57 -0
  86. data/src/org/jbox2d/dynamics/contacts/CircleContact.java +50 -0
  87. data/src/org/jbox2d/dynamics/contacts/Contact.java +365 -0
  88. data/src/org/jbox2d/dynamics/contacts/ContactCreator.java +35 -0
  89. data/src/org/jbox2d/dynamics/contacts/ContactEdge.java +56 -0
  90. data/src/org/jbox2d/dynamics/contacts/ContactPositionConstraint.java +49 -0
  91. data/src/org/jbox2d/dynamics/contacts/ContactRegister.java +31 -0
  92. data/src/org/jbox2d/dynamics/contacts/ContactSolver.java +1104 -0
  93. data/src/org/jbox2d/dynamics/contacts/ContactVelocityConstraint.java +60 -0
  94. data/src/org/jbox2d/dynamics/contacts/EdgeAndCircleContact.java +52 -0
  95. data/src/org/jbox2d/dynamics/contacts/EdgeAndPolygonContact.java +52 -0
  96. data/src/org/jbox2d/dynamics/contacts/PolygonAndCircleContact.java +51 -0
  97. data/src/org/jbox2d/dynamics/contacts/PolygonContact.java +50 -0
  98. data/src/org/jbox2d/dynamics/contacts/Position.java +31 -0
  99. data/src/org/jbox2d/dynamics/contacts/Velocity.java +31 -0
  100. data/src/org/jbox2d/dynamics/joints/ConstantVolumeJoint.java +258 -0
  101. data/src/org/jbox2d/dynamics/joints/ConstantVolumeJointDef.java +75 -0
  102. data/src/org/jbox2d/dynamics/joints/DistanceJoint.java +356 -0
  103. data/src/org/jbox2d/dynamics/joints/DistanceJointDef.java +106 -0
  104. data/src/org/jbox2d/dynamics/joints/FrictionJoint.java +294 -0
  105. data/src/org/jbox2d/dynamics/joints/FrictionJointDef.java +78 -0
  106. data/src/org/jbox2d/dynamics/joints/GearJoint.java +520 -0
  107. data/src/org/jbox2d/dynamics/joints/GearJointDef.java +58 -0
  108. data/src/org/jbox2d/dynamics/joints/Jacobian.java +32 -0
  109. data/src/org/jbox2d/dynamics/joints/Joint.java +235 -0
  110. data/src/org/jbox2d/dynamics/joints/JointDef.java +65 -0
  111. data/src/org/jbox2d/dynamics/joints/JointEdge.java +57 -0
  112. data/src/org/jbox2d/dynamics/joints/JointType.java +28 -0
  113. data/src/org/jbox2d/dynamics/joints/LimitState.java +28 -0
  114. data/src/org/jbox2d/dynamics/joints/MotorJoint.java +339 -0
  115. data/src/org/jbox2d/dynamics/joints/MotorJointDef.java +55 -0
  116. data/src/org/jbox2d/dynamics/joints/MouseJoint.java +262 -0
  117. data/src/org/jbox2d/dynamics/joints/MouseJointDef.java +62 -0
  118. data/src/org/jbox2d/dynamics/joints/PrismaticJoint.java +808 -0
  119. data/src/org/jbox2d/dynamics/joints/PrismaticJointDef.java +120 -0
  120. data/src/org/jbox2d/dynamics/joints/PulleyJoint.java +393 -0
  121. data/src/org/jbox2d/dynamics/joints/PulleyJointDef.java +105 -0
  122. data/src/org/jbox2d/dynamics/joints/RevoluteJoint.java +554 -0
  123. data/src/org/jbox2d/dynamics/joints/RevoluteJointDef.java +137 -0
  124. data/src/org/jbox2d/dynamics/joints/RopeJoint.java +276 -0
  125. data/src/org/jbox2d/dynamics/joints/RopeJointDef.java +34 -0
  126. data/src/org/jbox2d/dynamics/joints/WeldJoint.java +424 -0
  127. data/src/org/jbox2d/dynamics/joints/WeldJointDef.java +85 -0
  128. data/src/org/jbox2d/dynamics/joints/WheelJoint.java +498 -0
  129. data/src/org/jbox2d/dynamics/joints/WheelJointDef.java +98 -0
  130. data/src/org/jbox2d/particle/ParticleBodyContact.java +17 -0
  131. data/src/org/jbox2d/particle/ParticleColor.java +52 -0
  132. data/src/org/jbox2d/particle/ParticleContact.java +14 -0
  133. data/src/org/jbox2d/particle/ParticleDef.java +24 -0
  134. data/src/org/jbox2d/particle/ParticleGroup.java +154 -0
  135. data/src/org/jbox2d/particle/ParticleGroupDef.java +62 -0
  136. data/src/org/jbox2d/particle/ParticleGroupType.java +8 -0
  137. data/src/org/jbox2d/particle/ParticleSystem.java +2172 -0
  138. data/src/org/jbox2d/particle/ParticleType.java +28 -0
  139. data/src/org/jbox2d/particle/StackQueue.java +44 -0
  140. data/src/org/jbox2d/particle/VoronoiDiagram.java +209 -0
  141. data/src/org/jbox2d/pooling/IDynamicStack.java +47 -0
  142. data/src/org/jbox2d/pooling/IOrderedStack.java +57 -0
  143. data/src/org/jbox2d/pooling/IWorldPool.java +101 -0
  144. data/src/org/jbox2d/pooling/arrays/FloatArray.java +50 -0
  145. data/src/org/jbox2d/pooling/arrays/GeneratorArray.java +33 -0
  146. data/src/org/jbox2d/pooling/arrays/IntArray.java +53 -0
  147. data/src/org/jbox2d/pooling/arrays/Vec2Array.java +57 -0
  148. data/src/org/jbox2d/pooling/normal/CircleStack.java +77 -0
  149. data/src/org/jbox2d/pooling/normal/DefaultWorldPool.java +331 -0
  150. data/src/org/jbox2d/pooling/normal/MutableStack.java +72 -0
  151. data/src/org/jbox2d/pooling/normal/OrderedStack.java +73 -0
  152. data/src/org/jbox2d/pooling/stacks/DynamicIntStack.java +60 -0
  153. metadata +161 -14
  154. data/lib/jbox2d-library-2.3.1-SNAPSHOT.jar +0 -0
@@ -0,0 +1,424 @@
1
+ /*******************************************************************************
2
+ * Copyright (c) 2013, Daniel Murphy
3
+ * All rights reserved.
4
+ *
5
+ * Redistribution and use in source and binary forms, with or without modification,
6
+ * are permitted provided that the following conditions are met:
7
+ * * Redistributions of source code must retain the above copyright notice,
8
+ * this list of conditions and the following disclaimer.
9
+ * * Redistributions in binary form must reproduce the above copyright notice,
10
+ * this list of conditions and the following disclaimer in the documentation
11
+ * and/or other materials provided with the distribution.
12
+ *
13
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
14
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
16
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
17
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
18
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
19
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
20
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
21
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
22
+ * POSSIBILITY OF SUCH DAMAGE.
23
+ ******************************************************************************/
24
+ /**
25
+ * Created at 3:38:38 AM Jan 15, 2011
26
+ */
27
+ package org.jbox2d.dynamics.joints;
28
+
29
+ import org.jbox2d.common.Mat33;
30
+ import org.jbox2d.common.MathUtils;
31
+ import org.jbox2d.common.Rot;
32
+ import org.jbox2d.common.Settings;
33
+ import org.jbox2d.common.Vec2;
34
+ import org.jbox2d.common.Vec3;
35
+ import org.jbox2d.dynamics.SolverData;
36
+ import org.jbox2d.pooling.IWorldPool;
37
+
38
+ //Point-to-point constraint
39
+ //C = p2 - p1
40
+ //Cdot = v2 - v1
41
+ // = v2 + cross(w2, r2) - v1 - cross(w1, r1)
42
+ //J = [-I -r1_skew I r2_skew ]
43
+ //Identity used:
44
+ //w k % (rx i + ry j) = w * (-ry i + rx j)
45
+
46
+ //Angle constraint
47
+ //C = angle2 - angle1 - referenceAngle
48
+ //Cdot = w2 - w1
49
+ //J = [0 0 -1 0 0 1]
50
+ //K = invI1 + invI2
51
+
52
+ /**
53
+ * A weld joint essentially glues two bodies together. A weld joint may distort somewhat because the
54
+ * island constraint solver is approximate.
55
+ *
56
+ * @author Daniel Murphy
57
+ */
58
+ public class WeldJoint extends Joint {
59
+
60
+ private float m_frequencyHz;
61
+ private float m_dampingRatio;
62
+ private float m_bias;
63
+
64
+ // Solver shared
65
+ private final Vec2 m_localAnchorA;
66
+ private final Vec2 m_localAnchorB;
67
+ private float m_referenceAngle;
68
+ private float m_gamma;
69
+ private final Vec3 m_impulse;
70
+
71
+
72
+ // Solver temp
73
+ private int m_indexA;
74
+ private int m_indexB;
75
+ private final Vec2 m_rA = new Vec2();
76
+ private final Vec2 m_rB = new Vec2();
77
+ private final Vec2 m_localCenterA = new Vec2();
78
+ private final Vec2 m_localCenterB = new Vec2();
79
+ private float m_invMassA;
80
+ private float m_invMassB;
81
+ private float m_invIA;
82
+ private float m_invIB;
83
+ private final Mat33 m_mass = new Mat33();
84
+
85
+ protected WeldJoint(IWorldPool argWorld, WeldJointDef def) {
86
+ super(argWorld, def);
87
+ m_localAnchorA = new Vec2(def.localAnchorA);
88
+ m_localAnchorB = new Vec2(def.localAnchorB);
89
+ m_referenceAngle = def.referenceAngle;
90
+ m_frequencyHz = def.frequencyHz;
91
+ m_dampingRatio = def.dampingRatio;
92
+
93
+ m_impulse = new Vec3();
94
+ m_impulse.setZero();
95
+ }
96
+
97
+ public float getReferenceAngle() {
98
+ return m_referenceAngle;
99
+ }
100
+
101
+ public Vec2 getLocalAnchorA() {
102
+ return m_localAnchorA;
103
+ }
104
+
105
+ public Vec2 getLocalAnchorB() {
106
+ return m_localAnchorB;
107
+ }
108
+
109
+ public float getFrequency() {
110
+ return m_frequencyHz;
111
+ }
112
+
113
+ public void setFrequency(float frequencyHz) {
114
+ this.m_frequencyHz = frequencyHz;
115
+ }
116
+
117
+ public float getDampingRatio() {
118
+ return m_dampingRatio;
119
+ }
120
+
121
+ public void setDampingRatio(float dampingRatio) {
122
+ this.m_dampingRatio = dampingRatio;
123
+ }
124
+
125
+ @Override
126
+ public void getAnchorA(Vec2 argOut) {
127
+ m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
128
+ }
129
+
130
+ @Override
131
+ public void getAnchorB(Vec2 argOut) {
132
+ m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
133
+ }
134
+
135
+ @Override
136
+ public void getReactionForce(float inv_dt, Vec2 argOut) {
137
+ argOut.set(m_impulse.x, m_impulse.y);
138
+ argOut.mulLocal(inv_dt);
139
+ }
140
+
141
+ @Override
142
+ public float getReactionTorque(float inv_dt) {
143
+ return inv_dt * m_impulse.z;
144
+ }
145
+
146
+ @Override
147
+ public void initVelocityConstraints(final SolverData data) {
148
+ m_indexA = m_bodyA.m_islandIndex;
149
+ m_indexB = m_bodyB.m_islandIndex;
150
+ m_localCenterA.set(m_bodyA.m_sweep.localCenter);
151
+ m_localCenterB.set(m_bodyB.m_sweep.localCenter);
152
+ m_invMassA = m_bodyA.m_invMass;
153
+ m_invMassB = m_bodyB.m_invMass;
154
+ m_invIA = m_bodyA.m_invI;
155
+ m_invIB = m_bodyB.m_invI;
156
+
157
+ // Vec2 cA = data.positions[m_indexA].c;
158
+ float aA = data.positions[m_indexA].a;
159
+ Vec2 vA = data.velocities[m_indexA].v;
160
+ float wA = data.velocities[m_indexA].w;
161
+
162
+ // Vec2 cB = data.positions[m_indexB].c;
163
+ float aB = data.positions[m_indexB].a;
164
+ Vec2 vB = data.velocities[m_indexB].v;
165
+ float wB = data.velocities[m_indexB].w;
166
+
167
+ final Rot qA = pool.popRot();
168
+ final Rot qB = pool.popRot();
169
+ final Vec2 temp = pool.popVec2();
170
+
171
+ qA.set(aA);
172
+ qB.set(aB);
173
+
174
+ // Compute the effective masses.
175
+ Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
176
+ Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
177
+
178
+ // J = [-I -r1_skew I r2_skew]
179
+ // [ 0 -1 0 1]
180
+ // r_skew = [-ry; rx]
181
+
182
+ // Matlab
183
+ // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
184
+ // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
185
+ // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
186
+
187
+ float mA = m_invMassA, mB = m_invMassB;
188
+ float iA = m_invIA, iB = m_invIB;
189
+
190
+ final Mat33 K = pool.popMat33();
191
+
192
+ K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
193
+ K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
194
+ K.ez.x = -m_rA.y * iA - m_rB.y * iB;
195
+ K.ex.y = K.ey.x;
196
+ K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
197
+ K.ez.y = m_rA.x * iA + m_rB.x * iB;
198
+ K.ex.z = K.ez.x;
199
+ K.ey.z = K.ez.y;
200
+ K.ez.z = iA + iB;
201
+
202
+ if (m_frequencyHz > 0.0f) {
203
+ K.getInverse22(m_mass);
204
+
205
+ float invM = iA + iB;
206
+ float m = invM > 0.0f ? 1.0f / invM : 0.0f;
207
+
208
+ float C = aB - aA - m_referenceAngle;
209
+
210
+ // Frequency
211
+ float omega = 2.0f * MathUtils.PI * m_frequencyHz;
212
+
213
+ // Damping coefficient
214
+ float d = 2.0f * m * m_dampingRatio * omega;
215
+
216
+ // Spring stiffness
217
+ float k = m * omega * omega;
218
+
219
+ // magic formulas
220
+ float h = data.step.dt;
221
+ m_gamma = h * (d + h * k);
222
+ m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
223
+ m_bias = C * h * k * m_gamma;
224
+
225
+ invM += m_gamma;
226
+ m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
227
+ } else {
228
+ K.getSymInverse33(m_mass);
229
+ m_gamma = 0.0f;
230
+ m_bias = 0.0f;
231
+ }
232
+
233
+ if (data.step.warmStarting) {
234
+ final Vec2 P = pool.popVec2();
235
+ // Scale impulses to support a variable time step.
236
+ m_impulse.mulLocal(data.step.dtRatio);
237
+
238
+ P.set(m_impulse.x, m_impulse.y);
239
+
240
+ vA.x -= mA * P.x;
241
+ vA.y -= mA * P.y;
242
+ wA -= iA * (Vec2.cross(m_rA, P) + m_impulse.z);
243
+
244
+ vB.x += mB * P.x;
245
+ vB.y += mB * P.y;
246
+ wB += iB * (Vec2.cross(m_rB, P) + m_impulse.z);
247
+ pool.pushVec2(1);
248
+ } else {
249
+ m_impulse.setZero();
250
+ }
251
+
252
+ // data.velocities[m_indexA].v.set(vA);
253
+ data.velocities[m_indexA].w = wA;
254
+ // data.velocities[m_indexB].v.set(vB);
255
+ data.velocities[m_indexB].w = wB;
256
+
257
+ pool.pushVec2(1);
258
+ pool.pushRot(2);
259
+ pool.pushMat33(1);
260
+ }
261
+
262
+ @Override
263
+ public void solveVelocityConstraints(final SolverData data) {
264
+ Vec2 vA = data.velocities[m_indexA].v;
265
+ float wA = data.velocities[m_indexA].w;
266
+ Vec2 vB = data.velocities[m_indexB].v;
267
+ float wB = data.velocities[m_indexB].w;
268
+
269
+ float mA = m_invMassA, mB = m_invMassB;
270
+ float iA = m_invIA, iB = m_invIB;
271
+
272
+ final Vec2 Cdot1 = pool.popVec2();
273
+ final Vec2 P = pool.popVec2();
274
+ final Vec2 temp = pool.popVec2();
275
+ if (m_frequencyHz > 0.0f) {
276
+ float Cdot2 = wB - wA;
277
+
278
+ float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
279
+ m_impulse.z += impulse2;
280
+
281
+ wA -= iA * impulse2;
282
+ wB += iB * impulse2;
283
+
284
+ Vec2.crossToOutUnsafe(wB, m_rB, Cdot1);
285
+ Vec2.crossToOutUnsafe(wA, m_rA, temp);
286
+ Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
287
+
288
+ final Vec2 impulse1 = P;
289
+ Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1);
290
+ impulse1.negateLocal();
291
+
292
+ m_impulse.x += impulse1.x;
293
+ m_impulse.y += impulse1.y;
294
+
295
+ vA.x -= mA * P.x;
296
+ vA.y -= mA * P.y;
297
+ wA -= iA * Vec2.cross(m_rA, P);
298
+
299
+ vB.x += mB * P.x;
300
+ vB.y += mB * P.y;
301
+ wB += iB * Vec2.cross(m_rB, P);
302
+ } else {
303
+ Vec2.crossToOutUnsafe(wA, m_rA, temp);
304
+ Vec2.crossToOutUnsafe(wB, m_rB, Cdot1);
305
+ Cdot1.addLocal(vB).subLocal(vA).subLocal(temp);
306
+ float Cdot2 = wB - wA;
307
+
308
+ final Vec3 Cdot = pool.popVec3();
309
+ Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
310
+
311
+ final Vec3 impulse = pool.popVec3();
312
+ Mat33.mulToOutUnsafe(m_mass, Cdot, impulse);
313
+ impulse.negateLocal();
314
+ m_impulse.addLocal(impulse);
315
+
316
+ P.set(impulse.x, impulse.y);
317
+
318
+ vA.x -= mA * P.x;
319
+ vA.y -= mA * P.y;
320
+ wA -= iA * (Vec2.cross(m_rA, P) + impulse.z);
321
+
322
+ vB.x += mB * P.x;
323
+ vB.y += mB * P.y;
324
+ wB += iB * (Vec2.cross(m_rB, P) + impulse.z);
325
+
326
+ pool.pushVec3(2);
327
+ }
328
+
329
+ // data.velocities[m_indexA].v.set(vA);
330
+ data.velocities[m_indexA].w = wA;
331
+ // data.velocities[m_indexB].v.set(vB);
332
+ data.velocities[m_indexB].w = wB;
333
+
334
+ pool.pushVec2(3);
335
+ }
336
+
337
+ @Override
338
+ public boolean solvePositionConstraints(final SolverData data) {
339
+ Vec2 cA = data.positions[m_indexA].c;
340
+ float aA = data.positions[m_indexA].a;
341
+ Vec2 cB = data.positions[m_indexB].c;
342
+ float aB = data.positions[m_indexB].a;
343
+ final Rot qA = pool.popRot();
344
+ final Rot qB = pool.popRot();
345
+ final Vec2 temp = pool.popVec2();
346
+ final Vec2 rA = pool.popVec2();
347
+ final Vec2 rB = pool.popVec2();
348
+
349
+ qA.set(aA);
350
+ qB.set(aB);
351
+
352
+ float mA = m_invMassA, mB = m_invMassB;
353
+ float iA = m_invIA, iB = m_invIB;
354
+
355
+ Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
356
+ Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
357
+ float positionError, angularError;
358
+
359
+ final Mat33 K = pool.popMat33();
360
+ final Vec2 C1 = pool.popVec2();
361
+ final Vec2 P = pool.popVec2();
362
+
363
+ K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
364
+ K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
365
+ K.ez.x = -rA.y * iA - rB.y * iB;
366
+ K.ex.y = K.ey.x;
367
+ K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
368
+ K.ez.y = rA.x * iA + rB.x * iB;
369
+ K.ex.z = K.ez.x;
370
+ K.ey.z = K.ez.y;
371
+ K.ez.z = iA + iB;
372
+ if (m_frequencyHz > 0.0f) {
373
+ C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
374
+
375
+ positionError = C1.length();
376
+ angularError = 0.0f;
377
+
378
+ K.solve22ToOut(C1, P);
379
+ P.negateLocal();
380
+
381
+ cA.x -= mA * P.x;
382
+ cA.y -= mA * P.y;
383
+ aA -= iA * Vec2.cross(rA, P);
384
+
385
+ cB.x += mB * P.x;
386
+ cB.y += mB * P.y;
387
+ aB += iB * Vec2.cross(rB, P);
388
+ } else {
389
+ C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
390
+ float C2 = aB - aA - m_referenceAngle;
391
+
392
+ positionError = C1.length();
393
+ angularError = MathUtils.abs(C2);
394
+
395
+ final Vec3 C = pool.popVec3();
396
+ final Vec3 impulse = pool.popVec3();
397
+ C.set(C1.x, C1.y, C2);
398
+
399
+ K.solve33ToOut(C, impulse);
400
+ impulse.negateLocal();
401
+ P.set(impulse.x, impulse.y);
402
+
403
+ cA.x -= mA * P.x;
404
+ cA.y -= mA * P.y;
405
+ aA -= iA * (Vec2.cross(rA, P) + impulse.z);
406
+
407
+ cB.x += mB * P.x;
408
+ cB.y += mB * P.y;
409
+ aB += iB * (Vec2.cross(rB, P) + impulse.z);
410
+ pool.pushVec3(2);
411
+ }
412
+
413
+ // data.positions[m_indexA].c.set(cA);
414
+ data.positions[m_indexA].a = aA;
415
+ // data.positions[m_indexB].c.set(cB);
416
+ data.positions[m_indexB].a = aB;
417
+
418
+ pool.pushVec2(5);
419
+ pool.pushRot(2);
420
+ pool.pushMat33(1);
421
+
422
+ return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
423
+ }
424
+ }
@@ -0,0 +1,85 @@
1
+ /*******************************************************************************
2
+ * Copyright (c) 2013, Daniel Murphy
3
+ * All rights reserved.
4
+ *
5
+ * Redistribution and use in source and binary forms, with or without modification,
6
+ * are permitted provided that the following conditions are met:
7
+ * * Redistributions of source code must retain the above copyright notice,
8
+ * this list of conditions and the following disclaimer.
9
+ * * Redistributions in binary form must reproduce the above copyright notice,
10
+ * this list of conditions and the following disclaimer in the documentation
11
+ * and/or other materials provided with the distribution.
12
+ *
13
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
14
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
16
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
17
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
18
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
19
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
20
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
21
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
22
+ * POSSIBILITY OF SUCH DAMAGE.
23
+ ******************************************************************************/
24
+ package org.jbox2d.dynamics.joints;
25
+
26
+ import org.jbox2d.common.Vec2;
27
+ import org.jbox2d.dynamics.Body;
28
+ import org.jbox2d.dynamics.joints.JointDef;
29
+ import org.jbox2d.dynamics.joints.JointType;
30
+
31
+ /**
32
+ * Created at 3:38:52 AM Jan 15, 2011
33
+ */
34
+
35
+ /**
36
+ * @author Daniel Murphy
37
+ */
38
+ public class WeldJointDef extends JointDef {
39
+ /**
40
+ * The local anchor point relative to body1's origin.
41
+ */
42
+ public final Vec2 localAnchorA;
43
+
44
+ /**
45
+ * The local anchor point relative to body2's origin.
46
+ */
47
+ public final Vec2 localAnchorB;
48
+
49
+ /**
50
+ * The body2 angle minus body1 angle in the reference state (radians).
51
+ */
52
+ public float referenceAngle;
53
+
54
+ /**
55
+ * The mass-spring-damper frequency in Hertz. Rotation only. Disable softness with a value of 0.
56
+ */
57
+ public float frequencyHz;
58
+
59
+ /**
60
+ * The damping ratio. 0 = no damping, 1 = critical damping.
61
+ */
62
+ public float dampingRatio;
63
+
64
+ public WeldJointDef() {
65
+ super(JointType.WELD);
66
+ localAnchorA = new Vec2();
67
+ localAnchorB = new Vec2();
68
+ referenceAngle = 0.0f;
69
+ }
70
+
71
+ /**
72
+ * Initialize the bodies, anchors, and reference angle using a world anchor point.
73
+ *
74
+ * @param bA
75
+ * @param bB
76
+ * @param anchor
77
+ */
78
+ public void initialize(Body bA, Body bB, Vec2 anchor) {
79
+ bodyA = bA;
80
+ bodyB = bB;
81
+ bodyA.getLocalPointToOut(anchor, localAnchorA);
82
+ bodyB.getLocalPointToOut(anchor, localAnchorB);
83
+ referenceAngle = bodyB.getAngle() - bodyA.getAngle();
84
+ }
85
+ }