pbox2d 0.6.0-java → 0.8.0-java
Sign up to get free protection for your applications and to get access to all the features.
- 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
|
+
}
|