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,520 @@
|
|
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 11:34:45 AM Jan 23, 2011
|
26
|
+
*/
|
27
|
+
package org.jbox2d.dynamics.joints;
|
28
|
+
|
29
|
+
import org.jbox2d.common.Rot;
|
30
|
+
import org.jbox2d.common.Settings;
|
31
|
+
import org.jbox2d.common.Transform;
|
32
|
+
import org.jbox2d.common.Vec2;
|
33
|
+
import org.jbox2d.dynamics.Body;
|
34
|
+
import org.jbox2d.dynamics.SolverData;
|
35
|
+
import org.jbox2d.pooling.IWorldPool;
|
36
|
+
|
37
|
+
//Gear Joint:
|
38
|
+
//C0 = (coordinate1 + ratio * coordinate2)_initial
|
39
|
+
//C = (coordinate1 + ratio * coordinate2) - C0 = 0
|
40
|
+
//J = [J1 ratio * J2]
|
41
|
+
//K = J * invM * JT
|
42
|
+
//= J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
|
43
|
+
//
|
44
|
+
//Revolute:
|
45
|
+
//coordinate = rotation
|
46
|
+
//Cdot = angularVelocity
|
47
|
+
//J = [0 0 1]
|
48
|
+
//K = J * invM * JT = invI
|
49
|
+
//
|
50
|
+
//Prismatic:
|
51
|
+
//coordinate = dot(p - pg, ug)
|
52
|
+
//Cdot = dot(v + cross(w, r), ug)
|
53
|
+
//J = [ug cross(r, ug)]
|
54
|
+
//K = J * invM * JT = invMass + invI * cross(r, ug)^2
|
55
|
+
|
56
|
+
/**
|
57
|
+
* A gear joint is used to connect two joints together. Either joint can be a revolute or prismatic
|
58
|
+
* joint. You specify a gear ratio to bind the motions together: coordinate1 + ratio * coordinate2 =
|
59
|
+
* constant The ratio can be negative or positive. If one joint is a revolute joint and the other
|
60
|
+
* joint is a prismatic joint, then the ratio will have units of length or units of 1/length.
|
61
|
+
*
|
62
|
+
* @warning The revolute and prismatic joints must be attached to fixed bodies (which must be body1
|
63
|
+
* on those joints).
|
64
|
+
* @warning You have to manually destroy the gear joint if joint1 or joint2 is destroyed.
|
65
|
+
* @author Daniel Murphy
|
66
|
+
*/
|
67
|
+
public class GearJoint extends Joint {
|
68
|
+
|
69
|
+
private final Joint m_joint1;
|
70
|
+
private final Joint m_joint2;
|
71
|
+
|
72
|
+
private final JointType m_typeA;
|
73
|
+
private final JointType m_typeB;
|
74
|
+
|
75
|
+
// Body A is connected to body C
|
76
|
+
// Body B is connected to body D
|
77
|
+
private final Body m_bodyC;
|
78
|
+
private final Body m_bodyD;
|
79
|
+
|
80
|
+
// Solver shared
|
81
|
+
private final Vec2 m_localAnchorA = new Vec2();
|
82
|
+
private final Vec2 m_localAnchorB = new Vec2();
|
83
|
+
private final Vec2 m_localAnchorC = new Vec2();
|
84
|
+
private final Vec2 m_localAnchorD = new Vec2();
|
85
|
+
|
86
|
+
private final Vec2 m_localAxisC = new Vec2();
|
87
|
+
private final Vec2 m_localAxisD = new Vec2();
|
88
|
+
|
89
|
+
private float m_referenceAngleA;
|
90
|
+
private float m_referenceAngleB;
|
91
|
+
|
92
|
+
private float m_constant;
|
93
|
+
private float m_ratio;
|
94
|
+
|
95
|
+
private float m_impulse;
|
96
|
+
|
97
|
+
// Solver temp
|
98
|
+
private int m_indexA, m_indexB, m_indexC, m_indexD;
|
99
|
+
private final Vec2 m_lcA = new Vec2(), m_lcB = new Vec2(), m_lcC = new Vec2(),
|
100
|
+
m_lcD = new Vec2();
|
101
|
+
private float m_mA, m_mB, m_mC, m_mD;
|
102
|
+
private float m_iA, m_iB, m_iC, m_iD;
|
103
|
+
private final Vec2 m_JvAC = new Vec2(), m_JvBD = new Vec2();
|
104
|
+
private float m_JwA, m_JwB, m_JwC, m_JwD;
|
105
|
+
private float m_mass;
|
106
|
+
|
107
|
+
protected GearJoint(IWorldPool argWorldPool, GearJointDef def) {
|
108
|
+
super(argWorldPool, def);
|
109
|
+
|
110
|
+
m_joint1 = def.joint1;
|
111
|
+
m_joint2 = def.joint2;
|
112
|
+
|
113
|
+
m_typeA = m_joint1.getType();
|
114
|
+
m_typeB = m_joint2.getType();
|
115
|
+
|
116
|
+
assert (m_typeA == JointType.REVOLUTE || m_typeA == JointType.PRISMATIC);
|
117
|
+
assert (m_typeB == JointType.REVOLUTE || m_typeB == JointType.PRISMATIC);
|
118
|
+
|
119
|
+
float coordinateA, coordinateB;
|
120
|
+
|
121
|
+
// TODO_ERIN there might be some problem with the joint edges in Joint.
|
122
|
+
|
123
|
+
m_bodyC = m_joint1.getBodyA();
|
124
|
+
m_bodyA = m_joint1.getBodyB();
|
125
|
+
|
126
|
+
// Get geometry of joint1
|
127
|
+
Transform xfA = m_bodyA.m_xf;
|
128
|
+
float aA = m_bodyA.m_sweep.a;
|
129
|
+
Transform xfC = m_bodyC.m_xf;
|
130
|
+
float aC = m_bodyC.m_sweep.a;
|
131
|
+
|
132
|
+
if (m_typeA == JointType.REVOLUTE) {
|
133
|
+
RevoluteJoint revolute = (RevoluteJoint) def.joint1;
|
134
|
+
m_localAnchorC.set(revolute.m_localAnchorA);
|
135
|
+
m_localAnchorA.set(revolute.m_localAnchorB);
|
136
|
+
m_referenceAngleA = revolute.m_referenceAngle;
|
137
|
+
m_localAxisC.setZero();
|
138
|
+
|
139
|
+
coordinateA = aA - aC - m_referenceAngleA;
|
140
|
+
} else {
|
141
|
+
Vec2 pA = pool.popVec2();
|
142
|
+
Vec2 temp = pool.popVec2();
|
143
|
+
PrismaticJoint prismatic = (PrismaticJoint) def.joint1;
|
144
|
+
m_localAnchorC.set(prismatic.m_localAnchorA);
|
145
|
+
m_localAnchorA.set(prismatic.m_localAnchorB);
|
146
|
+
m_referenceAngleA = prismatic.m_referenceAngle;
|
147
|
+
m_localAxisC.set(prismatic.m_localXAxisA);
|
148
|
+
|
149
|
+
Vec2 pC = m_localAnchorC;
|
150
|
+
Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, temp);
|
151
|
+
temp.addLocal(xfA.p).subLocal(xfC.p);
|
152
|
+
Rot.mulTransUnsafe(xfC.q, temp, pA);
|
153
|
+
coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC);
|
154
|
+
pool.pushVec2(2);
|
155
|
+
}
|
156
|
+
|
157
|
+
m_bodyD = m_joint2.getBodyA();
|
158
|
+
m_bodyB = m_joint2.getBodyB();
|
159
|
+
|
160
|
+
// Get geometry of joint2
|
161
|
+
Transform xfB = m_bodyB.m_xf;
|
162
|
+
float aB = m_bodyB.m_sweep.a;
|
163
|
+
Transform xfD = m_bodyD.m_xf;
|
164
|
+
float aD = m_bodyD.m_sweep.a;
|
165
|
+
|
166
|
+
if (m_typeB == JointType.REVOLUTE) {
|
167
|
+
RevoluteJoint revolute = (RevoluteJoint) def.joint2;
|
168
|
+
m_localAnchorD.set(revolute.m_localAnchorA);
|
169
|
+
m_localAnchorB.set(revolute.m_localAnchorB);
|
170
|
+
m_referenceAngleB = revolute.m_referenceAngle;
|
171
|
+
m_localAxisD.setZero();
|
172
|
+
|
173
|
+
coordinateB = aB - aD - m_referenceAngleB;
|
174
|
+
} else {
|
175
|
+
Vec2 pB = pool.popVec2();
|
176
|
+
Vec2 temp = pool.popVec2();
|
177
|
+
PrismaticJoint prismatic = (PrismaticJoint) def.joint2;
|
178
|
+
m_localAnchorD.set(prismatic.m_localAnchorA);
|
179
|
+
m_localAnchorB.set(prismatic.m_localAnchorB);
|
180
|
+
m_referenceAngleB = prismatic.m_referenceAngle;
|
181
|
+
m_localAxisD.set(prismatic.m_localXAxisA);
|
182
|
+
|
183
|
+
Vec2 pD = m_localAnchorD;
|
184
|
+
Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, temp);
|
185
|
+
temp.addLocal(xfB.p).subLocal(xfD.p);
|
186
|
+
Rot.mulTransUnsafe(xfD.q, temp, pB);
|
187
|
+
coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD);
|
188
|
+
pool.pushVec2(2);
|
189
|
+
}
|
190
|
+
|
191
|
+
m_ratio = def.ratio;
|
192
|
+
|
193
|
+
m_constant = coordinateA + m_ratio * coordinateB;
|
194
|
+
|
195
|
+
m_impulse = 0.0f;
|
196
|
+
}
|
197
|
+
|
198
|
+
@Override
|
199
|
+
public void getAnchorA(Vec2 argOut) {
|
200
|
+
m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
|
201
|
+
}
|
202
|
+
|
203
|
+
@Override
|
204
|
+
public void getAnchorB(Vec2 argOut) {
|
205
|
+
m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
|
206
|
+
}
|
207
|
+
|
208
|
+
@Override
|
209
|
+
public void getReactionForce(float inv_dt, Vec2 argOut) {
|
210
|
+
argOut.set(m_JvAC).mulLocal(m_impulse);
|
211
|
+
argOut.mulLocal(inv_dt);
|
212
|
+
}
|
213
|
+
|
214
|
+
@Override
|
215
|
+
public float getReactionTorque(float inv_dt) {
|
216
|
+
float L = m_impulse * m_JwA;
|
217
|
+
return inv_dt * L;
|
218
|
+
}
|
219
|
+
|
220
|
+
public void setRatio(float argRatio) {
|
221
|
+
m_ratio = argRatio;
|
222
|
+
}
|
223
|
+
|
224
|
+
public float getRatio() {
|
225
|
+
return m_ratio;
|
226
|
+
}
|
227
|
+
|
228
|
+
@Override
|
229
|
+
public void initVelocityConstraints(SolverData data) {
|
230
|
+
m_indexA = m_bodyA.m_islandIndex;
|
231
|
+
m_indexB = m_bodyB.m_islandIndex;
|
232
|
+
m_indexC = m_bodyC.m_islandIndex;
|
233
|
+
m_indexD = m_bodyD.m_islandIndex;
|
234
|
+
m_lcA.set(m_bodyA.m_sweep.localCenter);
|
235
|
+
m_lcB.set(m_bodyB.m_sweep.localCenter);
|
236
|
+
m_lcC.set(m_bodyC.m_sweep.localCenter);
|
237
|
+
m_lcD.set(m_bodyD.m_sweep.localCenter);
|
238
|
+
m_mA = m_bodyA.m_invMass;
|
239
|
+
m_mB = m_bodyB.m_invMass;
|
240
|
+
m_mC = m_bodyC.m_invMass;
|
241
|
+
m_mD = m_bodyD.m_invMass;
|
242
|
+
m_iA = m_bodyA.m_invI;
|
243
|
+
m_iB = m_bodyB.m_invI;
|
244
|
+
m_iC = m_bodyC.m_invI;
|
245
|
+
m_iD = m_bodyD.m_invI;
|
246
|
+
|
247
|
+
// Vec2 cA = data.positions[m_indexA].c;
|
248
|
+
float aA = data.positions[m_indexA].a;
|
249
|
+
Vec2 vA = data.velocities[m_indexA].v;
|
250
|
+
float wA = data.velocities[m_indexA].w;
|
251
|
+
|
252
|
+
// Vec2 cB = data.positions[m_indexB].c;
|
253
|
+
float aB = data.positions[m_indexB].a;
|
254
|
+
Vec2 vB = data.velocities[m_indexB].v;
|
255
|
+
float wB = data.velocities[m_indexB].w;
|
256
|
+
|
257
|
+
// Vec2 cC = data.positions[m_indexC].c;
|
258
|
+
float aC = data.positions[m_indexC].a;
|
259
|
+
Vec2 vC = data.velocities[m_indexC].v;
|
260
|
+
float wC = data.velocities[m_indexC].w;
|
261
|
+
|
262
|
+
// Vec2 cD = data.positions[m_indexD].c;
|
263
|
+
float aD = data.positions[m_indexD].a;
|
264
|
+
Vec2 vD = data.velocities[m_indexD].v;
|
265
|
+
float wD = data.velocities[m_indexD].w;
|
266
|
+
|
267
|
+
Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot();
|
268
|
+
qA.set(aA);
|
269
|
+
qB.set(aB);
|
270
|
+
qC.set(aC);
|
271
|
+
qD.set(aD);
|
272
|
+
|
273
|
+
m_mass = 0.0f;
|
274
|
+
|
275
|
+
Vec2 temp = pool.popVec2();
|
276
|
+
|
277
|
+
if (m_typeA == JointType.REVOLUTE) {
|
278
|
+
m_JvAC.setZero();
|
279
|
+
m_JwA = 1.0f;
|
280
|
+
m_JwC = 1.0f;
|
281
|
+
m_mass += m_iA + m_iC;
|
282
|
+
} else {
|
283
|
+
Vec2 rC = pool.popVec2();
|
284
|
+
Vec2 rA = pool.popVec2();
|
285
|
+
Rot.mulToOutUnsafe(qC, m_localAxisC, m_JvAC);
|
286
|
+
Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC);
|
287
|
+
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA);
|
288
|
+
m_JwC = Vec2.cross(rC, m_JvAC);
|
289
|
+
m_JwA = Vec2.cross(rA, m_JvAC);
|
290
|
+
m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
|
291
|
+
pool.pushVec2(2);
|
292
|
+
}
|
293
|
+
|
294
|
+
if (m_typeB == JointType.REVOLUTE) {
|
295
|
+
m_JvBD.setZero();
|
296
|
+
m_JwB = m_ratio;
|
297
|
+
m_JwD = m_ratio;
|
298
|
+
m_mass += m_ratio * m_ratio * (m_iB + m_iD);
|
299
|
+
} else {
|
300
|
+
Vec2 u = pool.popVec2();
|
301
|
+
Vec2 rD = pool.popVec2();
|
302
|
+
Vec2 rB = pool.popVec2();
|
303
|
+
Rot.mulToOutUnsafe(qD, m_localAxisD, u);
|
304
|
+
Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD);
|
305
|
+
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB);
|
306
|
+
m_JvBD.set(u).mulLocal(m_ratio);
|
307
|
+
m_JwD = m_ratio * Vec2.cross(rD, u);
|
308
|
+
m_JwB = m_ratio * Vec2.cross(rB, u);
|
309
|
+
m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
|
310
|
+
pool.pushVec2(3);
|
311
|
+
}
|
312
|
+
|
313
|
+
// Compute effective mass.
|
314
|
+
m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
|
315
|
+
|
316
|
+
if (data.step.warmStarting) {
|
317
|
+
vA.x += (m_mA * m_impulse) * m_JvAC.x;
|
318
|
+
vA.y += (m_mA * m_impulse) * m_JvAC.y;
|
319
|
+
wA += m_iA * m_impulse * m_JwA;
|
320
|
+
|
321
|
+
vB.x += (m_mB * m_impulse) * m_JvBD.x;
|
322
|
+
vB.y += (m_mB * m_impulse) * m_JvBD.y;
|
323
|
+
wB += m_iB * m_impulse * m_JwB;
|
324
|
+
|
325
|
+
vC.x -= (m_mC * m_impulse) * m_JvAC.x;
|
326
|
+
vC.y -= (m_mC * m_impulse) * m_JvAC.y;
|
327
|
+
wC -= m_iC * m_impulse * m_JwC;
|
328
|
+
|
329
|
+
vD.x -= (m_mD * m_impulse) * m_JvBD.x;
|
330
|
+
vD.y -= (m_mD * m_impulse) * m_JvBD.y;
|
331
|
+
wD -= m_iD * m_impulse * m_JwD;
|
332
|
+
} else {
|
333
|
+
m_impulse = 0.0f;
|
334
|
+
}
|
335
|
+
pool.pushVec2(1);
|
336
|
+
pool.pushRot(4);
|
337
|
+
|
338
|
+
// data.velocities[m_indexA].v = vA;
|
339
|
+
data.velocities[m_indexA].w = wA;
|
340
|
+
// data.velocities[m_indexB].v = vB;
|
341
|
+
data.velocities[m_indexB].w = wB;
|
342
|
+
// data.velocities[m_indexC].v = vC;
|
343
|
+
data.velocities[m_indexC].w = wC;
|
344
|
+
// data.velocities[m_indexD].v = vD;
|
345
|
+
data.velocities[m_indexD].w = wD;
|
346
|
+
}
|
347
|
+
|
348
|
+
@Override
|
349
|
+
public void solveVelocityConstraints(SolverData data) {
|
350
|
+
Vec2 vA = data.velocities[m_indexA].v;
|
351
|
+
float wA = data.velocities[m_indexA].w;
|
352
|
+
Vec2 vB = data.velocities[m_indexB].v;
|
353
|
+
float wB = data.velocities[m_indexB].w;
|
354
|
+
Vec2 vC = data.velocities[m_indexC].v;
|
355
|
+
float wC = data.velocities[m_indexC].w;
|
356
|
+
Vec2 vD = data.velocities[m_indexD].v;
|
357
|
+
float wD = data.velocities[m_indexD].w;
|
358
|
+
|
359
|
+
Vec2 temp1 = pool.popVec2();
|
360
|
+
Vec2 temp2 = pool.popVec2();
|
361
|
+
float Cdot =
|
362
|
+
Vec2.dot(m_JvAC, temp1.set(vA).subLocal(vC)) + Vec2.dot(m_JvBD, temp2.set(vB).subLocal(vD));
|
363
|
+
Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
|
364
|
+
pool.pushVec2(2);
|
365
|
+
|
366
|
+
float impulse = -m_mass * Cdot;
|
367
|
+
m_impulse += impulse;
|
368
|
+
|
369
|
+
vA.x += (m_mA * impulse) * m_JvAC.x;
|
370
|
+
vA.y += (m_mA * impulse) * m_JvAC.y;
|
371
|
+
wA += m_iA * impulse * m_JwA;
|
372
|
+
|
373
|
+
vB.x += (m_mB * impulse) * m_JvBD.x;
|
374
|
+
vB.y += (m_mB * impulse) * m_JvBD.y;
|
375
|
+
wB += m_iB * impulse * m_JwB;
|
376
|
+
|
377
|
+
vC.x -= (m_mC * impulse) * m_JvAC.x;
|
378
|
+
vC.y -= (m_mC * impulse) * m_JvAC.y;
|
379
|
+
wC -= m_iC * impulse * m_JwC;
|
380
|
+
|
381
|
+
vD.x -= (m_mD * impulse) * m_JvBD.x;
|
382
|
+
vD.y -= (m_mD * impulse) * m_JvBD.y;
|
383
|
+
wD -= m_iD * impulse * m_JwD;
|
384
|
+
|
385
|
+
|
386
|
+
// data.velocities[m_indexA].v = vA;
|
387
|
+
data.velocities[m_indexA].w = wA;
|
388
|
+
// data.velocities[m_indexB].v = vB;
|
389
|
+
data.velocities[m_indexB].w = wB;
|
390
|
+
// data.velocities[m_indexC].v = vC;
|
391
|
+
data.velocities[m_indexC].w = wC;
|
392
|
+
// data.velocities[m_indexD].v = vD;
|
393
|
+
data.velocities[m_indexD].w = wD;
|
394
|
+
}
|
395
|
+
|
396
|
+
public Joint getJoint1() {
|
397
|
+
return m_joint1;
|
398
|
+
}
|
399
|
+
|
400
|
+
public Joint getJoint2() {
|
401
|
+
return m_joint2;
|
402
|
+
}
|
403
|
+
|
404
|
+
@Override
|
405
|
+
public boolean solvePositionConstraints(SolverData data) {
|
406
|
+
Vec2 cA = data.positions[m_indexA].c;
|
407
|
+
float aA = data.positions[m_indexA].a;
|
408
|
+
Vec2 cB = data.positions[m_indexB].c;
|
409
|
+
float aB = data.positions[m_indexB].a;
|
410
|
+
Vec2 cC = data.positions[m_indexC].c;
|
411
|
+
float aC = data.positions[m_indexC].a;
|
412
|
+
Vec2 cD = data.positions[m_indexD].c;
|
413
|
+
float aD = data.positions[m_indexD].a;
|
414
|
+
|
415
|
+
Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot();
|
416
|
+
qA.set(aA);
|
417
|
+
qB.set(aB);
|
418
|
+
qC.set(aC);
|
419
|
+
qD.set(aD);
|
420
|
+
|
421
|
+
float linearError = 0.0f;
|
422
|
+
|
423
|
+
float coordinateA, coordinateB;
|
424
|
+
|
425
|
+
Vec2 temp = pool.popVec2();
|
426
|
+
Vec2 JvAC = pool.popVec2();
|
427
|
+
Vec2 JvBD = pool.popVec2();
|
428
|
+
float JwA, JwB, JwC, JwD;
|
429
|
+
float mass = 0.0f;
|
430
|
+
|
431
|
+
if (m_typeA == JointType.REVOLUTE) {
|
432
|
+
JvAC.setZero();
|
433
|
+
JwA = 1.0f;
|
434
|
+
JwC = 1.0f;
|
435
|
+
mass += m_iA + m_iC;
|
436
|
+
|
437
|
+
coordinateA = aA - aC - m_referenceAngleA;
|
438
|
+
} else {
|
439
|
+
Vec2 rC = pool.popVec2();
|
440
|
+
Vec2 rA = pool.popVec2();
|
441
|
+
Vec2 pC = pool.popVec2();
|
442
|
+
Vec2 pA = pool.popVec2();
|
443
|
+
Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC);
|
444
|
+
Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC);
|
445
|
+
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA);
|
446
|
+
JwC = Vec2.cross(rC, JvAC);
|
447
|
+
JwA = Vec2.cross(rA, JvAC);
|
448
|
+
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
|
449
|
+
|
450
|
+
pC.set(m_localAnchorC).subLocal(m_lcC);
|
451
|
+
Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA);
|
452
|
+
coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC);
|
453
|
+
pool.pushVec2(4);
|
454
|
+
}
|
455
|
+
|
456
|
+
if (m_typeB == JointType.REVOLUTE) {
|
457
|
+
JvBD.setZero();
|
458
|
+
JwB = m_ratio;
|
459
|
+
JwD = m_ratio;
|
460
|
+
mass += m_ratio * m_ratio * (m_iB + m_iD);
|
461
|
+
|
462
|
+
coordinateB = aB - aD - m_referenceAngleB;
|
463
|
+
} else {
|
464
|
+
Vec2 u = pool.popVec2();
|
465
|
+
Vec2 rD = pool.popVec2();
|
466
|
+
Vec2 rB = pool.popVec2();
|
467
|
+
Vec2 pD = pool.popVec2();
|
468
|
+
Vec2 pB = pool.popVec2();
|
469
|
+
Rot.mulToOutUnsafe(qD, m_localAxisD, u);
|
470
|
+
Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD);
|
471
|
+
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB);
|
472
|
+
JvBD.set(u).mulLocal(m_ratio);
|
473
|
+
JwD = Vec2.cross(rD, u);
|
474
|
+
JwB = Vec2.cross(rB, u);
|
475
|
+
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
|
476
|
+
|
477
|
+
pD.set(m_localAnchorD).subLocal(m_lcD);
|
478
|
+
Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB);
|
479
|
+
coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD);
|
480
|
+
pool.pushVec2(5);
|
481
|
+
}
|
482
|
+
|
483
|
+
float C = (coordinateA + m_ratio * coordinateB) - m_constant;
|
484
|
+
|
485
|
+
float impulse = 0.0f;
|
486
|
+
if (mass > 0.0f) {
|
487
|
+
impulse = -C / mass;
|
488
|
+
}
|
489
|
+
pool.pushVec2(3);
|
490
|
+
pool.pushRot(4);
|
491
|
+
|
492
|
+
cA.x += (m_mA * impulse) * JvAC.x;
|
493
|
+
cA.y += (m_mA * impulse) * JvAC.y;
|
494
|
+
aA += m_iA * impulse * JwA;
|
495
|
+
|
496
|
+
cB.x += (m_mB * impulse) * JvBD.x;
|
497
|
+
cB.y += (m_mB * impulse) * JvBD.y;
|
498
|
+
aB += m_iB * impulse * JwB;
|
499
|
+
|
500
|
+
cC.x -= (m_mC * impulse) * JvAC.x;
|
501
|
+
cC.y -= (m_mC * impulse) * JvAC.y;
|
502
|
+
aC -= m_iC * impulse * JwC;
|
503
|
+
|
504
|
+
cD.x -= (m_mD * impulse) * JvBD.x;
|
505
|
+
cD.y -= (m_mD * impulse) * JvBD.y;
|
506
|
+
aD -= m_iD * impulse * JwD;
|
507
|
+
|
508
|
+
// data.positions[m_indexA].c = cA;
|
509
|
+
data.positions[m_indexA].a = aA;
|
510
|
+
// data.positions[m_indexB].c = cB;
|
511
|
+
data.positions[m_indexB].a = aB;
|
512
|
+
// data.positions[m_indexC].c = cC;
|
513
|
+
data.positions[m_indexC].a = aC;
|
514
|
+
// data.positions[m_indexD].c = cD;
|
515
|
+
data.positions[m_indexD].a = aD;
|
516
|
+
|
517
|
+
// TODO_ERIN not implemented
|
518
|
+
return linearError < Settings.linearSlop;
|
519
|
+
}
|
520
|
+
}
|
@@ -0,0 +1,58 @@
|
|
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 5:20:39 AM Jan 22, 2011
|
26
|
+
*/
|
27
|
+
package org.jbox2d.dynamics.joints;
|
28
|
+
|
29
|
+
/**
|
30
|
+
* Gear joint definition. This definition requires two existing revolute or prismatic joints (any
|
31
|
+
* combination will work). The provided joints must attach a dynamic body to a static body.
|
32
|
+
*
|
33
|
+
* @author Daniel Murphy
|
34
|
+
*/
|
35
|
+
public class GearJointDef extends JointDef {
|
36
|
+
/**
|
37
|
+
* The first revolute/prismatic joint attached to the gear joint.
|
38
|
+
*/
|
39
|
+
public Joint joint1;
|
40
|
+
|
41
|
+
/**
|
42
|
+
* The second revolute/prismatic joint attached to the gear joint.
|
43
|
+
*/
|
44
|
+
public Joint joint2;
|
45
|
+
|
46
|
+
/**
|
47
|
+
* Gear ratio.
|
48
|
+
*
|
49
|
+
* @see GearJoint
|
50
|
+
*/
|
51
|
+
public float ratio;
|
52
|
+
|
53
|
+
public GearJointDef() {
|
54
|
+
super(JointType.GEAR);
|
55
|
+
joint1 = null;
|
56
|
+
joint2 = null;
|
57
|
+
}
|
58
|
+
}
|
@@ -0,0 +1,32 @@
|
|
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
|
+
|
28
|
+
public class Jacobian {
|
29
|
+
public final Vec2 linearA = new Vec2();
|
30
|
+
public float angularA;
|
31
|
+
public float angularB;
|
32
|
+
}
|