Skip to content

Commit d2c8011

Browse files
DDoSDDoS
committed
Update to "Iterate over the islands to solve the contacts and joints"
DanielChappuis/reactphysics3d@475ec5be5fca73c4 ce78e3250375631da96fa03b
1 parent 32ba953 commit d2c8011

File tree

9 files changed

+311
-247
lines changed

9 files changed

+311
-247
lines changed

‎src/main/java/org/spout/physics/body/RigidBody.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -336,13 +336,14 @@ public void removeJointFromJointsList(Constraint joint) {
336336
final JointListElement elementToRemove = mJointsList;
337337
mJointsList = elementToRemove.getNext();
338338
} else {
339-
final JointListElement currentElement = mJointsList;
339+
JointListElement currentElement = mJointsList;
340340
while (currentElement.getNext() != null) {
341341
if (currentElement.getNext().getJoint() == joint) {
342342
final JointListElement elementToRemove = currentElement.getNext();
343343
currentElement.setNext(elementToRemove.getNext());
344344
break;
345345
}
346+
currentElement = currentElement.getNext();
346347
}
347348
}
348349
}

‎src/main/java/org/spout/physics/constraint/BallAndSocketJoint.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -108,10 +108,10 @@ public void initBeforeSolve(ConstraintSolverData constraintSolverData) {
108108

109109
@Override
110110
public void warmstart(ConstraintSolverData constraintSolverData) {
111-
final Vector3 v1 = constraintSolverData.getLinearVelocities().get(mIndexBody1);
112-
final Vector3 v2 = constraintSolverData.getLinearVelocities().get(mIndexBody2);
113-
final Vector3 w1 = constraintSolverData.getAngularVelocities().get(mIndexBody1);
114-
final Vector3 w2 = constraintSolverData.getAngularVelocities().get(mIndexBody2);
111+
final Vector3 v1 = constraintSolverData.getLinearVelocities()[mIndexBody1];
112+
final Vector3 v2 = constraintSolverData.getLinearVelocities()[mIndexBody2];
113+
final Vector3 w1 = constraintSolverData.getAngularVelocities()[mIndexBody1];
114+
final Vector3 w2 = constraintSolverData.getAngularVelocities()[mIndexBody2];
115115
final float inverseMassBody1 = mBody1.getMassInverse();
116116
final float inverseMassBody2 = mBody2.getMassInverse();
117117
if (mBody1.getIsMotionEnabled()) {
@@ -130,10 +130,10 @@ public void warmstart(ConstraintSolverData constraintSolverData) {
130130

131131
@Override
132132
public void solveVelocityConstraint(ConstraintSolverData constraintSolverData) {
133-
final Vector3 v1 = constraintSolverData.getLinearVelocities().get(mIndexBody1);
134-
final Vector3 v2 = constraintSolverData.getLinearVelocities().get(mIndexBody2);
135-
final Vector3 w1 = constraintSolverData.getAngularVelocities().get(mIndexBody1);
136-
final Vector3 w2 = constraintSolverData.getAngularVelocities().get(mIndexBody2);
133+
final Vector3 v1 = constraintSolverData.getLinearVelocities()[mIndexBody1];
134+
final Vector3 v2 = constraintSolverData.getLinearVelocities()[mIndexBody2];
135+
final Vector3 w1 = constraintSolverData.getAngularVelocities()[mIndexBody1];
136+
final Vector3 w2 = constraintSolverData.getAngularVelocities()[mIndexBody2];
137137
float inverseMassBody1 = mBody1.getMassInverse();
138138
float inverseMassBody2 = mBody2.getMassInverse();
139139
final Vector3 Jv = Vector3.subtract(Vector3.subtract(Vector3.add(v2, w2.cross(mR2World)), v1), w1.cross(mR1World));

‎src/main/java/org/spout/physics/constraint/ConstraintSolver.java‎

Lines changed: 98 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -26,13 +26,12 @@
2626
*/
2727
package org.spout.physics.constraint;
2828

29-
import java.util.HashSet;
3029
import java.util.List;
31-
import java.util.Set;
3230

3331
import gnu.trove.map.TObjectIntMap;
3432

3533
import org.spout.physics.body.RigidBody;
34+
import org.spout.physics.engine.Island;
3635
import org.spout.physics.math.Quaternion;
3736
import org.spout.physics.math.Vector3;
3837

@@ -82,10 +81,8 @@
8281
* contact manifold, we need two constraints for tangential friction but also another twist friction constraint to prevent spin of the body around the contact manifold center.
8382
*/
8483
public class ConstraintSolver {
85-
private final Set<Constraint> mJoints;
86-
private final Set<RigidBody> mConstraintBodies = new HashSet<>();
87-
private final List<Vector3> mLinearVelocities;
88-
private final List<Vector3> mAngularVelocities;
84+
private Vector3[] mLinearVelocities;
85+
private Vector3[] mAngularVelocities;
8986
private final List<Vector3> mPositions;
9087
private final List<Quaternion> mOrientations;
9188
private final TObjectIntMap<RigidBody> mMapBodyToConstrainedVelocityIndex;
@@ -94,96 +91,134 @@ public class ConstraintSolver {
9491
private final ConstraintSolverData mConstraintSolverData;
9592

9693
/**
97-
* Constructs a new constraint solver from the joints, linear and angular velocities and body to constrained velocity index map.
94+
* Constructs a new constraint solver from positions, orientations and body to constrained velocity index map.
9895
*
99-
* @param joints The joints
100-
* @param linearVelocities The linear velocities
101-
* @param angularVelocities The angular velocities
96+
* @param positions The positions
97+
* @param orientations The orientations
10298
* @param mapBodyToConstrainedVelocityIndex The map from body to constrained velocity
10399
*/
104-
public ConstraintSolver(Set<Constraint> joints, List<Vector3> linearVelocities, List<Vector3> angularVelocities, List<Vector3> positions, List<Quaternion> orientations,
105-
TObjectIntMap<RigidBody> mapBodyToConstrainedVelocityIndex) {
106-
mJoints = joints;
107-
mLinearVelocities = linearVelocities;
108-
mAngularVelocities = angularVelocities;
100+
public ConstraintSolver(List<Vector3> positions, List<Quaternion> orientations, TObjectIntMap<RigidBody> mapBodyToConstrainedVelocityIndex) {
101+
mLinearVelocities = null;
102+
mAngularVelocities = null;
109103
mPositions = positions;
110104
mOrientations = orientations;
111105
mMapBodyToConstrainedVelocityIndex = mapBodyToConstrainedVelocityIndex;
112106
mIsWarmStartingActive = true;
113-
mConstraintSolverData = new ConstraintSolverData(linearVelocities, angularVelocities, positions, orientations, mapBodyToConstrainedVelocityIndex);
107+
mConstraintSolverData = new ConstraintSolverData(positions, orientations, mapBodyToConstrainedVelocityIndex);
114108
}
115109

116110
/**
117-
* Initializes the constraint solver.
111+
* Initializes the constraint solver for a given island.
118112
*
119113
* @param dt The time delta
114+
* @param island The island
120115
*/
121-
public void initialize(float dt) {
116+
public void initializeForIsland(float dt, Island island) {
117+
if (mLinearVelocities == null) {
118+
throw new IllegalStateException("Linear velocities cannot be null");
119+
}
120+
if (mAngularVelocities == null) {
121+
throw new IllegalStateException("Angular velocities cannot be null");
122+
}
123+
if (island == null) {
124+
throw new IllegalArgumentException("Island cannot be null");
125+
}
126+
if (island.getNbBodies() <= 0) {
127+
throw new IllegalArgumentException("The number of bodies in the island must be greater than zero");
128+
}
129+
if (island.getNbJoints() <= 0) {
130+
throw new IllegalArgumentException("The number of joints in the island must be greater than zero");
131+
}
122132
mTimeStep = dt;
123133
mConstraintSolverData.setTimeStep(mTimeStep);
124134
mConstraintSolverData.setWarmStartingActive(mIsWarmStartingActive);
125-
for (Constraint joint : mJoints) {
126-
final RigidBody body1 = joint.getFirstBody();
127-
final RigidBody body2 = joint.getSecondBody();
128-
mConstraintBodies.add(body1);
129-
mConstraintBodies.add(body2);
130-
joint.initBeforeSolve(mConstraintSolverData);
135+
final Constraint[] joints = island.getJoints();
136+
for (int i = 0; i < island.getNbJoints(); i++) {
137+
joints[i].initBeforeSolve(mConstraintSolverData);
131138
if (mIsWarmStartingActive) {
132-
joint.warmstart(mConstraintSolverData);
139+
joints[i].warmstart(mConstraintSolverData);
133140
}
134141
}
135142
}
136143

137144
/**
138145
* Solves the velocity constraints.
146+
*
147+
* @param island The island to solve
139148
*/
140-
public void solveVelocityConstraints() {
141-
for (Constraint joint : mJoints) {
142-
joint.solveVelocityConstraint(mConstraintSolverData);
149+
public void solveVelocityConstraints(Island island) {
150+
if (island == null) {
151+
throw new IllegalArgumentException("Island cannot be null");
152+
}
153+
if (island.getNbJoints() <= 0) {
154+
throw new IllegalArgumentException("The number of joints in the island must be greater than zero");
155+
}
156+
final Constraint[] joints = island.getJoints();
157+
for (int i = 0; i < island.getNbJoints(); i++) {
158+
joints[i].solveVelocityConstraint(mConstraintSolverData);
143159
}
144160
}
145161

146162
/**
147163
* Solves the position constraints.
164+
*
165+
* @param island The island to solve
148166
*/
149-
public void solvePositionConstraints() {
150-
for (Constraint joint : mJoints) {
151-
joint.solvePositionConstraint(mConstraintSolverData);
167+
public void solvePositionConstraints(Island island) {
168+
if (island == null) {
169+
throw new IllegalArgumentException("Island cannot be null");
170+
}
171+
if (island.getNbJoints() <= 0) {
172+
throw new IllegalArgumentException("The number of joints in the island must be greater than zero");
173+
}
174+
final Constraint[] joints = island.getJoints();
175+
for (int i = 0; i < island.getNbJoints(); i++) {
176+
joints[i].solvePositionConstraint(mConstraintSolverData);
152177
}
153178
}
154179

155180
/**
156-
* Returns true if the body is in at least one constraint.
181+
* Sets the constrained velocities arrays.
157182
*
158-
* @return Whether or not the body is in at least one constraint
183+
* @param constrainedLinearVelocities The constrained linear velocities
184+
* @param constrainedAngularVelocities The constrained angular velocities
159185
*/
160-
public boolean isConstrainedBody(RigidBody body) {
161-
return mConstraintBodies.contains(body);
186+
public void setConstrainedVelocitiesArrays(Vector3[] constrainedLinearVelocities, Vector3[] constrainedAngularVelocities) {
187+
if (constrainedLinearVelocities == null) {
188+
throw new IllegalArgumentException("The constrained linear velocities cannot be null");
189+
}
190+
if (constrainedAngularVelocities == null) {
191+
throw new IllegalArgumentException("The constrained angular velocities cannot be null");
192+
}
193+
mLinearVelocities = constrainedLinearVelocities;
194+
mAngularVelocities = constrainedAngularVelocities;
195+
mConstraintSolverData.setLinearVelocities(mLinearVelocities);
196+
mConstraintSolverData.setAngularVelocities(mAngularVelocities);
162197
}
163198

164199
/**
165200
* This structure contains data from the constraint solver that is used to solve each joint constraint.
166201
*/
167202
public static class ConstraintSolverData {
168203
private float timeStep;
169-
private final List<Vector3> linearVelocities;
170-
private final List<Vector3> angularVelocities;
204+
private Vector3[] linearVelocities;
205+
private Vector3[] angularVelocities;
171206
private final List<Vector3> positions;
172207
private final List<Quaternion> orientations;
173208
private final TObjectIntMap<RigidBody> mapBodyToConstrainedVelocityIndex;
174209
private boolean isWarmStartingActive;
175210

176211
/**
177-
* Constructs a new constraint solver data from the linear and angular velocities and the map from body to constrained velocity index.
212+
* Constructs a new constraint solver data from the position, the orientations and the map from body to constrained velocity index.
178213
*
179-
* @param refLinearVelocities The linear velocities
180-
* @param refAngularVelocities The angular velocities
214+
* @param refPositions The positions
215+
* @param refOrientations The orientations
181216
* @param refMapBodyToConstrainedVelocityIndex The map from body to constrained velocity index
182217
*/
183-
public ConstraintSolverData(List<Vector3> refLinearVelocities, List<Vector3> refAngularVelocities, List<Vector3> refPositions, List<Quaternion> refOrientations,
218+
public ConstraintSolverData(List<Vector3> refPositions, List<Quaternion> refOrientations,
184219
TObjectIntMap<RigidBody> refMapBodyToConstrainedVelocityIndex) {
185-
this.linearVelocities = refLinearVelocities;
186-
this.angularVelocities = refAngularVelocities;
220+
this.linearVelocities = null;
221+
this.angularVelocities = null;
187222
this.positions = refPositions;
188223
this.orientations = refOrientations;
189224
this.mapBodyToConstrainedVelocityIndex = refMapBodyToConstrainedVelocityIndex;
@@ -212,19 +247,37 @@ public void setTimeStep(float timeStep) {
212247
*
213248
* @return The linear velocities
214249
*/
215-
public List<Vector3> getLinearVelocities() {
250+
public Vector3[] getLinearVelocities() {
216251
return linearVelocities;
217252
}
218253

254+
/**
255+
* Sets the linear velocities.
256+
*
257+
* @param linearVelocities The linear velocities
258+
*/
259+
public void setLinearVelocities(Vector3[] linearVelocities) {
260+
this.linearVelocities = linearVelocities;
261+
}
262+
219263
/**
220264
* Returns the list of angular velocities.
221265
*
222266
* @return The angular velocities
223267
*/
224-
public List<Vector3> getAngularVelocities() {
268+
public Vector3[] getAngularVelocities() {
225269
return angularVelocities;
226270
}
227271

272+
/**
273+
* Sets the angular velocities.
274+
*
275+
* @param angularVelocities The angular velocities
276+
*/
277+
public void setAngularVelocities(Vector3[] angularVelocities) {
278+
this.angularVelocities = angularVelocities;
279+
}
280+
228281
/**
229282
* Returns the list of positions.
230283
*

‎src/main/java/org/spout/physics/constraint/FixedJoint.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -136,10 +136,10 @@ public void initBeforeSolve(ConstraintSolverData constraintSolverData) {
136136

137137
@Override
138138
public void warmstart(ConstraintSolverData constraintSolverData) {
139-
final Vector3 v1 = constraintSolverData.getLinearVelocities().get(mIndexBody1);
140-
final Vector3 v2 = constraintSolverData.getLinearVelocities().get(mIndexBody2);
141-
final Vector3 w1 = constraintSolverData.getAngularVelocities().get(mIndexBody1);
142-
final Vector3 w2 = constraintSolverData.getAngularVelocities().get(mIndexBody2);
139+
final Vector3 v1 = constraintSolverData.getLinearVelocities()[mIndexBody1];
140+
final Vector3 v2 = constraintSolverData.getLinearVelocities()[mIndexBody2];
141+
final Vector3 w1 = constraintSolverData.getAngularVelocities()[mIndexBody1];
142+
final Vector3 w2 = constraintSolverData.getAngularVelocities()[mIndexBody2];
143143
final float inverseMassBody1 = mBody1.getMassInverse();
144144
final float inverseMassBody2 = mBody2.getMassInverse();
145145
if (mBody1.getIsMotionEnabled()) {
@@ -160,10 +160,10 @@ public void warmstart(ConstraintSolverData constraintSolverData) {
160160

161161
@Override
162162
public void solveVelocityConstraint(ConstraintSolverData constraintSolverData) {
163-
final Vector3 v1 = constraintSolverData.getLinearVelocities().get(mIndexBody1);
164-
final Vector3 v2 = constraintSolverData.getLinearVelocities().get(mIndexBody2);
165-
final Vector3 w1 = constraintSolverData.getAngularVelocities().get(mIndexBody1);
166-
final Vector3 w2 = constraintSolverData.getAngularVelocities().get(mIndexBody2);
163+
final Vector3 v1 = constraintSolverData.getLinearVelocities()[mIndexBody1];
164+
final Vector3 v2 = constraintSolverData.getLinearVelocities()[mIndexBody2];
165+
final Vector3 w1 = constraintSolverData.getAngularVelocities()[mIndexBody1];
166+
final Vector3 w2 = constraintSolverData.getAngularVelocities()[mIndexBody2];
167167
final float inverseMassBody1 = mBody1.getMassInverse();
168168
final float inverseMassBody2 = mBody2.getMassInverse();
169169
final Vector3 JvTranslation = Vector3.subtract(Vector3.subtract(Vector3.add(v2, w2.cross(mR2World)), v1), w1.cross(mR1World));

‎src/main/java/org/spout/physics/constraint/HingeJoint.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -226,10 +226,10 @@ public void initBeforeSolve(ConstraintSolverData constraintSolverData) {
226226

227227
@Override
228228
public void warmstart(ConstraintSolverData constraintSolverData) {
229-
final Vector3 v1 = constraintSolverData.getLinearVelocities().get(mIndexBody1);
230-
final Vector3 v2 = constraintSolverData.getLinearVelocities().get(mIndexBody2);
231-
final Vector3 w1 = constraintSolverData.getAngularVelocities().get(mIndexBody1);
232-
final Vector3 w2 = constraintSolverData.getAngularVelocities().get(mIndexBody2);
229+
final Vector3 v1 = constraintSolverData.getLinearVelocities()[mIndexBody1];
230+
final Vector3 v2 = constraintSolverData.getLinearVelocities()[mIndexBody2];
231+
final Vector3 w1 = constraintSolverData.getAngularVelocities()[mIndexBody1];
232+
final Vector3 w2 = constraintSolverData.getAngularVelocities()[mIndexBody2];
233233
final float inverseMassBody1 = mBody1.getMassInverse();
234234
final float inverseMassBody2 = mBody2.getMassInverse();
235235
final Vector3 rotationImpulse = Vector3.subtract(Vector3.multiply(Vector3.negate(mB2CrossA1), mImpulseRotation.getX()), Vector3.multiply(mC2CrossA1, mImpulseRotation.getY()));
@@ -257,10 +257,10 @@ public void warmstart(ConstraintSolverData constraintSolverData) {
257257

258258
@Override
259259
public void solveVelocityConstraint(ConstraintSolverData constraintSolverData) {
260-
final Vector3 v1 = constraintSolverData.getLinearVelocities().get(mIndexBody1);
261-
final Vector3 v2 = constraintSolverData.getLinearVelocities().get(mIndexBody2);
262-
final Vector3 w1 = constraintSolverData.getAngularVelocities().get(mIndexBody1);
263-
final Vector3 w2 = constraintSolverData.getAngularVelocities().get(mIndexBody2);
260+
final Vector3 v1 = constraintSolverData.getLinearVelocities()[mIndexBody1];
261+
final Vector3 v2 = constraintSolverData.getLinearVelocities()[mIndexBody2];
262+
final Vector3 w1 = constraintSolverData.getAngularVelocities()[mIndexBody1];
263+
final Vector3 w2 = constraintSolverData.getAngularVelocities()[mIndexBody2];
264264
final float inverseMassBody1 = mBody1.getMassInverse();
265265
final float inverseMassBody2 = mBody2.getMassInverse();
266266
final Vector3 JvTranslation = Vector3.subtract(Vector3.subtract(Vector3.add(v2, w2.cross(mR2World)), v1), w1.cross(mR1World));

0 commit comments

Comments
 (0)