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.
- checksums.yaml +4 -4
- data/.mvn/extensions.xml +8 -0
- data/.mvn/wrapper/maven-wrapper.properties +1 -0
- data/.travis.yml +23 -0
- data/CHANGELOG.md +8 -0
- data/README.md +7 -7
- data/Rakefile +1 -2
- data/lib/box2d.jar +0 -0
- data/lib/pbox2d/version.rb +1 -1
- data/lib/pbox2d.rb +1 -0
- data/pbox2d.gemspec +6 -11
- data/pom.rb +59 -0
- data/pom.xml +82 -73
- data/src/org/jbox2d/JBox2D.gwt.xml +12 -0
- data/src/org/jbox2d/callbacks/ContactAdaptor.java +27 -0
- data/src/org/jbox2d/callbacks/ContactFilter.java +59 -0
- data/src/org/jbox2d/callbacks/ContactImpulse.java +42 -0
- data/src/org/jbox2d/callbacks/ContactListener.java +87 -0
- data/src/org/jbox2d/callbacks/DebugDraw.java +297 -0
- data/src/org/jbox2d/callbacks/DestructionListener.java +53 -0
- data/src/org/jbox2d/callbacks/PairCallback.java +29 -0
- data/src/org/jbox2d/callbacks/ParticleDestructionListener.java +20 -0
- data/src/org/jbox2d/callbacks/ParticleQueryCallback.java +19 -0
- data/src/org/jbox2d/callbacks/ParticleRaycastCallback.java +19 -0
- data/src/org/jbox2d/callbacks/QueryCallback.java +45 -0
- data/src/org/jbox2d/callbacks/RayCastCallback.java +55 -0
- data/src/org/jbox2d/callbacks/TreeCallback.java +42 -0
- data/src/org/jbox2d/callbacks/TreeRayCastCallback.java +44 -0
- data/src/org/jbox2d/collision/AABB.java +338 -0
- data/src/org/jbox2d/collision/Collision.java +1444 -0
- data/src/org/jbox2d/collision/ContactID.java +106 -0
- data/src/org/jbox2d/collision/Distance.java +773 -0
- data/src/org/jbox2d/collision/DistanceInput.java +41 -0
- data/src/org/jbox2d/collision/DistanceOutput.java +43 -0
- data/src/org/jbox2d/collision/Manifold.java +116 -0
- data/src/org/jbox2d/collision/ManifoldPoint.java +104 -0
- data/src/org/jbox2d/collision/RayCastInput.java +47 -0
- data/src/org/jbox2d/collision/RayCastOutput.java +46 -0
- data/src/org/jbox2d/collision/TimeOfImpact.java +526 -0
- data/src/org/jbox2d/collision/WorldManifold.java +200 -0
- data/src/org/jbox2d/collision/broadphase/BroadPhase.java +92 -0
- data/src/org/jbox2d/collision/broadphase/BroadPhaseStrategy.java +88 -0
- data/src/org/jbox2d/collision/broadphase/DefaultBroadPhaseBuffer.java +268 -0
- data/src/org/jbox2d/collision/broadphase/DynamicTree.java +883 -0
- data/src/org/jbox2d/collision/broadphase/DynamicTreeFlatNodes.java +873 -0
- data/src/org/jbox2d/collision/broadphase/DynamicTreeNode.java +54 -0
- data/src/org/jbox2d/collision/broadphase/Pair.java +46 -0
- data/src/org/jbox2d/collision/shapes/ChainShape.java +264 -0
- data/src/org/jbox2d/collision/shapes/CircleShape.java +207 -0
- data/src/org/jbox2d/collision/shapes/EdgeShape.java +254 -0
- data/src/org/jbox2d/collision/shapes/MassData.java +105 -0
- data/src/org/jbox2d/collision/shapes/PolygonShape.java +718 -0
- data/src/org/jbox2d/collision/shapes/Shape.java +136 -0
- data/src/org/jbox2d/collision/shapes/ShapeType.java +32 -0
- data/src/org/jbox2d/common/BufferUtils.java +209 -0
- data/src/org/jbox2d/common/Color3f.java +88 -0
- data/src/org/jbox2d/common/IViewportTransform.java +133 -0
- data/src/org/jbox2d/common/Mat22.java +609 -0
- data/src/org/jbox2d/common/Mat33.java +290 -0
- data/src/org/jbox2d/common/MathUtils.java +335 -0
- data/src/org/jbox2d/common/OBBViewportTransform.java +174 -0
- data/src/org/jbox2d/common/PlatformMathUtils.java +46 -0
- data/src/org/jbox2d/common/RaycastResult.java +37 -0
- data/src/org/jbox2d/common/Rot.java +150 -0
- data/src/org/jbox2d/common/Settings.java +246 -0
- data/src/org/jbox2d/common/Sweep.java +116 -0
- data/src/org/jbox2d/common/Timer.java +46 -0
- data/src/org/jbox2d/common/Transform.java +203 -0
- data/src/org/jbox2d/common/Vec2.java +388 -0
- data/src/org/jbox2d/common/Vec3.java +170 -0
- data/src/org/jbox2d/dynamics/Body.java +1246 -0
- data/src/org/jbox2d/dynamics/BodyDef.java +382 -0
- data/src/org/jbox2d/dynamics/BodyType.java +41 -0
- data/src/org/jbox2d/dynamics/ContactManager.java +293 -0
- data/src/org/jbox2d/dynamics/Filter.java +62 -0
- data/src/org/jbox2d/dynamics/Fixture.java +454 -0
- data/src/org/jbox2d/dynamics/FixtureDef.java +214 -0
- data/src/org/jbox2d/dynamics/FixtureProxy.java +38 -0
- data/src/org/jbox2d/dynamics/Island.java +602 -0
- data/src/org/jbox2d/dynamics/Profile.java +97 -0
- data/src/org/jbox2d/dynamics/SolverData.java +33 -0
- data/src/org/jbox2d/dynamics/TimeStep.java +46 -0
- data/src/org/jbox2d/dynamics/World.java +2075 -0
- data/src/org/jbox2d/dynamics/contacts/ChainAndCircleContact.java +57 -0
- data/src/org/jbox2d/dynamics/contacts/ChainAndPolygonContact.java +57 -0
- data/src/org/jbox2d/dynamics/contacts/CircleContact.java +50 -0
- data/src/org/jbox2d/dynamics/contacts/Contact.java +365 -0
- data/src/org/jbox2d/dynamics/contacts/ContactCreator.java +35 -0
- data/src/org/jbox2d/dynamics/contacts/ContactEdge.java +56 -0
- data/src/org/jbox2d/dynamics/contacts/ContactPositionConstraint.java +49 -0
- data/src/org/jbox2d/dynamics/contacts/ContactRegister.java +31 -0
- data/src/org/jbox2d/dynamics/contacts/ContactSolver.java +1104 -0
- data/src/org/jbox2d/dynamics/contacts/ContactVelocityConstraint.java +60 -0
- data/src/org/jbox2d/dynamics/contacts/EdgeAndCircleContact.java +52 -0
- data/src/org/jbox2d/dynamics/contacts/EdgeAndPolygonContact.java +52 -0
- data/src/org/jbox2d/dynamics/contacts/PolygonAndCircleContact.java +51 -0
- data/src/org/jbox2d/dynamics/contacts/PolygonContact.java +50 -0
- data/src/org/jbox2d/dynamics/contacts/Position.java +31 -0
- data/src/org/jbox2d/dynamics/contacts/Velocity.java +31 -0
- data/src/org/jbox2d/dynamics/joints/ConstantVolumeJoint.java +258 -0
- data/src/org/jbox2d/dynamics/joints/ConstantVolumeJointDef.java +75 -0
- data/src/org/jbox2d/dynamics/joints/DistanceJoint.java +356 -0
- data/src/org/jbox2d/dynamics/joints/DistanceJointDef.java +106 -0
- data/src/org/jbox2d/dynamics/joints/FrictionJoint.java +294 -0
- data/src/org/jbox2d/dynamics/joints/FrictionJointDef.java +78 -0
- data/src/org/jbox2d/dynamics/joints/GearJoint.java +520 -0
- data/src/org/jbox2d/dynamics/joints/GearJointDef.java +58 -0
- data/src/org/jbox2d/dynamics/joints/Jacobian.java +32 -0
- data/src/org/jbox2d/dynamics/joints/Joint.java +235 -0
- data/src/org/jbox2d/dynamics/joints/JointDef.java +65 -0
- data/src/org/jbox2d/dynamics/joints/JointEdge.java +57 -0
- data/src/org/jbox2d/dynamics/joints/JointType.java +28 -0
- data/src/org/jbox2d/dynamics/joints/LimitState.java +28 -0
- data/src/org/jbox2d/dynamics/joints/MotorJoint.java +339 -0
- data/src/org/jbox2d/dynamics/joints/MotorJointDef.java +55 -0
- data/src/org/jbox2d/dynamics/joints/MouseJoint.java +262 -0
- data/src/org/jbox2d/dynamics/joints/MouseJointDef.java +62 -0
- data/src/org/jbox2d/dynamics/joints/PrismaticJoint.java +808 -0
- data/src/org/jbox2d/dynamics/joints/PrismaticJointDef.java +120 -0
- data/src/org/jbox2d/dynamics/joints/PulleyJoint.java +393 -0
- data/src/org/jbox2d/dynamics/joints/PulleyJointDef.java +105 -0
- data/src/org/jbox2d/dynamics/joints/RevoluteJoint.java +554 -0
- data/src/org/jbox2d/dynamics/joints/RevoluteJointDef.java +137 -0
- data/src/org/jbox2d/dynamics/joints/RopeJoint.java +276 -0
- data/src/org/jbox2d/dynamics/joints/RopeJointDef.java +34 -0
- data/src/org/jbox2d/dynamics/joints/WeldJoint.java +424 -0
- data/src/org/jbox2d/dynamics/joints/WeldJointDef.java +85 -0
- data/src/org/jbox2d/dynamics/joints/WheelJoint.java +498 -0
- data/src/org/jbox2d/dynamics/joints/WheelJointDef.java +98 -0
- data/src/org/jbox2d/particle/ParticleBodyContact.java +17 -0
- data/src/org/jbox2d/particle/ParticleColor.java +52 -0
- data/src/org/jbox2d/particle/ParticleContact.java +14 -0
- data/src/org/jbox2d/particle/ParticleDef.java +24 -0
- data/src/org/jbox2d/particle/ParticleGroup.java +154 -0
- data/src/org/jbox2d/particle/ParticleGroupDef.java +62 -0
- data/src/org/jbox2d/particle/ParticleGroupType.java +8 -0
- data/src/org/jbox2d/particle/ParticleSystem.java +2172 -0
- data/src/org/jbox2d/particle/ParticleType.java +28 -0
- data/src/org/jbox2d/particle/StackQueue.java +44 -0
- data/src/org/jbox2d/particle/VoronoiDiagram.java +209 -0
- data/src/org/jbox2d/pooling/IDynamicStack.java +47 -0
- data/src/org/jbox2d/pooling/IOrderedStack.java +57 -0
- data/src/org/jbox2d/pooling/IWorldPool.java +101 -0
- data/src/org/jbox2d/pooling/arrays/FloatArray.java +50 -0
- data/src/org/jbox2d/pooling/arrays/GeneratorArray.java +33 -0
- data/src/org/jbox2d/pooling/arrays/IntArray.java +53 -0
- data/src/org/jbox2d/pooling/arrays/Vec2Array.java +57 -0
- data/src/org/jbox2d/pooling/normal/CircleStack.java +77 -0
- data/src/org/jbox2d/pooling/normal/DefaultWorldPool.java +331 -0
- data/src/org/jbox2d/pooling/normal/MutableStack.java +72 -0
- data/src/org/jbox2d/pooling/normal/OrderedStack.java +73 -0
- data/src/org/jbox2d/pooling/stacks/DynamicIntStack.java +60 -0
- metadata +161 -14
- 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
|
+
}
|