// TODO: Use ScrewTools
private void populateMapsAndLists(
ArrayList<InverseDynamicsJoint> jointsInOrderListToPack,
ArrayList<RigidBody> rigidBodiesInOrderListToPack) {
ArrayList<RigidBody> morgue = new ArrayList<RigidBody>();
morgue.add(rootBody);
while (!morgue.isEmpty()) {
RigidBody currentBody = morgue.get(0);
if (!currentBody.isRootBody()) {
rigidBodiesInOrderListToPack.add(currentBody);
}
if (currentBody.hasChildrenJoints()) {
List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints();
for (InverseDynamicsJoint joint : childrenJoints) {
if (!jointsToIgnore.contains(joint)) {
RigidBody successor = joint.getSuccessor();
if (successor != null) {
if (rigidBodiesInOrderListToPack.contains(successor)) {
throw new RuntimeException("This algorithm doesn't do loops.");
}
jointsInOrderListToPack.add(joint);
morgue.add(successor);
}
}
}
}
morgue.remove(currentBody);
}
}
private static int determineSize(RigidBody[] rigidBodiesInOrder) {
int ret = 0;
for (RigidBody rigidBody : rigidBodiesInOrder) {
ret += rigidBody.getParentJoint().getDegreesOfFreedom();
}
return ret;
}
public void addRigidBody(RigidBody body, short group, short mask) {
if (!body.isStaticOrKinematicObject()) {
body.setGravity(gravity);
}
if (body.getCollisionShape() != null) {
addCollisionObject(body, group, mask);
}
}
@Override
public void setGravity(Vector3f gravity) {
this.gravity.set(gravity);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.setGravity(gravity);
}
}
}
private static int[] createMassMatrixIndices(RigidBody[] rigidBodiesInOrder) {
int[] ret = new int[rigidBodiesInOrder.length];
int currentIndex = 0;
for (int i = 0; i < rigidBodiesInOrder.length; i++) {
RigidBody rigidBody = rigidBodiesInOrder[i];
ret[i] = currentIndex;
currentIndex += rigidBody.getParentJoint().getDegreesOfFreedom();
}
return ret;
}
/** Apply gravity, call this once per timestep. */
public void applyGravity() {
// todo: iterate over awake simulation islands!
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null && body.isActive()) {
body.applyGravity();
}
}
}
private void stepBack() {
RigidBody obj;
for (int i = 0; i < physObjs.size(); i++) {
obj = physObjs.get(i);
obj.cm.set(obj.cmold);
obj.v.set(obj.vold);
obj.theta = obj.thetaold;
for (int j = 0; j < obj.verticies.size() && j < obj.originalVerticies.size(); j++) {
obj.verticies.get(j).set(obj.originalVerticies.get(j));
}
}
}
@Override
public void clearForces() {
// todo: iterate over awake simulation islands!
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.clearForces();
}
}
}
private void setIntegrate() {
RigidBody obj;
for (int i = 0; i < physObjs.size(); i++) {
obj = physObjs.get(i);
obj.cmold.set(obj.cm);
obj.vold.set(obj.v);
obj.thetaold = obj.theta;
for (int j = 0; j < obj.verticies.size(); j++) {
obj.originalVerticies.get(j).set(obj.verticies.get(j));
}
}
}
private void resolveCollisions(
RigidBody collBody1, RigidBody collBody2, Vector2 collisionNormal, Vector2 collisionPoint) {
float restitution = collBody1.restitution * collBody2.restitution;
Vector2 cm2CornerPrep1 = collisionPoint.sub(collBody1.cm).getPerpendicular();
Vector2 cm2CornerPrep2 = collisionPoint.sub(collBody2.cm).getPerpendicular();
Vector2 v1 = collBody1.v; // .add( cm2CornerPrep1.mul( collBody1.angVel ));
Vector2 v2 = collBody2.v; // .add( cm2CornerPrep2.mul( collBody2.angVel ));
if (collBody1 != null && collBody2 != null) {
if (collBody1.isStatic) {
float jNum = -(1 + restitution) * v2.dotProduct(collisionNormal);
float prep2 = cm2CornerPrep2.dotProduct(collisionNormal) / collBody2.inertia;
float jDen =
collisionNormal.dotProduct(collisionNormal) * (1 / collBody2.mass) + prep2 * prep2;
float impulse = jNum / jDen;
collBody2.v = collBody2.v.add(collisionNormal.mul(impulse / collBody2.mass));
if (collBody2.isRotated) {
collBody2.angVel +=
collisionNormal.mul(impulse).dotProduct(cm2CornerPrep2) / collBody2.inertia;
}
} else {
if (collBody2.isStatic) {
float jNum = -(1 + restitution) * v1.dotProduct(collisionNormal);
float prep1 = cm2CornerPrep1.dotProduct(collisionNormal) / collBody1.inertia;
float jDen =
collisionNormal.dotProduct(collisionNormal) * (1 / collBody1.mass) + prep1 * prep1;
float impulse = jNum / jDen;
collBody1.v = collBody1.v.add(collisionNormal.mul(impulse / collBody1.mass));
if (collBody1.isRotated) {
collBody1.angVel +=
collisionNormal.mul(impulse).dotProduct(cm2CornerPrep1) / collBody1.inertia;
}
} else {
Vector2 relVel = v1.sub(v2);
float jNum = -(1 + restitution) * relVel.dotProduct(collisionNormal);
float prep1 = cm2CornerPrep1.dotProduct(collisionNormal) / collBody1.inertia;
float prep2 = cm2CornerPrep2.dotProduct(collisionNormal) / collBody2.inertia;
float jDen =
collisionNormal.dotProduct(collisionNormal)
* (1 / collBody2.mass + 1 / collBody1.mass)
+ prep2 * prep2
+ prep1 * prep1;
float impulse = jNum / jDen;
collBody1.v = collBody1.v.add(collisionNormal.mul(impulse / collBody1.mass));
collBody2.v = collBody2.v.add(collisionNormal.mul(-impulse / collBody2.mass));
if (collBody1.isRotated) {
collBody1.angVel +=
collisionNormal.mul(impulse).dotProduct(cm2CornerPrep1) / collBody1.inertia;
}
if (collBody2.isRotated) {
collBody2.angVel +=
collisionNormal.mul(impulse).dotProduct(cm2CornerPrep2) / collBody2.inertia;
}
}
}
}
}
protected void saveKinematicState(float timeStep) {
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
// Transform predictedTrans = new Transform();
if (body.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
if (body.isKinematicObject()) {
// to calculate velocities next frame
body.saveKinematicState(timeStep);
}
}
}
}
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyRotation() {
Random random = new Random(1766L);
RigidBody elevator = new RigidBody("elevator", world);
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
RigidBodyTransform transformToParent = new RigidBodyTransform();
transformToParent.setIdentity();
RevoluteJoint joint =
ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
RigidBody body =
ScrewTools.addRigidBody(
"body",
joint,
RandomTools.generateRandomDiagonalMatrix3d(random),
random.nextDouble(),
new Vector3d());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector linearMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
FrameVector angularMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());
FrameVector linearMomentumCheck = new FrameVector(world);
Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
angularMomentumCheckVector.scale(joint.getQd());
inertia.transform(angularMomentumCheckVector);
FrameVector angularMomentumCheck =
new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
angularMomentumCheck.changeFrame(world);
double epsilon = 1e-9;
JUnitTools.assertTuple3dEquals(
linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
JUnitTools.assertTuple3dEquals(
angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
assertTrue(angularMomentum.length() > epsilon);
}
public void onStart() {
if (rigidBody != null) {
rigidBody.onStart();
}
int x = 0;
for (x = 0; x < components.size(); x++) {
components.get(x).onStart();
}
}
public void compute() {
MatrixTools.setToZero(massMatrix);
for (int i = 0; i < allRigidBodiesInOrder.length; i++) {
crbInertiasInOrder[i].set(allRigidBodiesInOrder[i].getInertia());
}
for (int i = allRigidBodiesInOrder.length - 1; i >= 0; i--) {
RigidBody currentBody = allRigidBodiesInOrder[i];
InverseDynamicsJoint parentJoint = currentBody.getParentJoint();
CompositeRigidBodyInertia currentBodyInertia = crbInertiasInOrder[i];
GeometricJacobian motionSubspace = parentJoint.getMotionSubspace();
setUnitMomenta(currentBodyInertia, motionSubspace);
setDiagonalTerm(i, motionSubspace);
setOffDiagonalTerms(i);
buildCrbInertia(i);
}
}
@Override
public void addRigidBody(RigidBody body) {
if (!body.isStaticOrKinematicObject()) {
body.setGravity(gravity);
}
if (body.getCollisionShape() != null) {
boolean isDynamic = !(body.isStaticObject() || body.isKinematicObject());
short collisionFilterGroup =
isDynamic
? (short) CollisionFilterGroups.DEFAULT_FILTER
: (short) CollisionFilterGroups.STATIC_FILTER;
short collisionFilterMask =
isDynamic
? (short) CollisionFilterGroups.ALL_FILTER
: (short) (CollisionFilterGroups.ALL_FILTER ^ CollisionFilterGroups.STATIC_FILTER);
addCollisionObject(body, collisionFilterGroup, collisionFilterMask);
}
}
private Momentum computeMomentum(RigidBody elevator, ReferenceFrame frame) {
elevator.updateFramesRecursively();
TwistCalculator twistCalculator = new TwistCalculator(world, elevator);
twistCalculator.compute();
MomentumCalculator momentumCalculator = new MomentumCalculator(twistCalculator);
Momentum momentum = new Momentum(frame);
momentumCalculator.computeAndPack(momentum);
return momentum;
}
public void getCoMAcceleration(FrameVector comAccelerationToPack) {
boolean firstIteration = true;
double totalMass = 0.0;
for (RigidBody rigidBody : rigidBodies) {
double mass = rigidBody.getInertia().getMass();
rigidBody.getCoMOffset(comLocation);
spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(
linkLinearMomentumDot, base, rigidBody, comLocation);
linkLinearMomentumDot.scale(mass);
if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot);
else comAccelerationToPack.add(linkLinearMomentumDot);
totalMass += mass;
firstIteration = false;
}
comAccelerationToPack.scale(1.0 / totalMass);
}
private void integrate(float time) {
for (int i = 0; i < modifers.size(); i++) {
modifers.get(i).update();
}
RigidBody obj;
for (int i = 0; i < physObjs.size(); i++) {
obj = physObjs.get(i);
if (!obj.isGhost) {
obj.f = obj.f.add(g.mul(obj.mass));
}
obj.dcm.set(obj.v.mul(time));
obj.cm.set(obj.cm.add(obj.dcm));
obj.v.set(obj.v.add(obj.f.mul(time).div(obj.mass)));
if (obj.isRotated) {
obj.angVel += obj.torque * time / obj.inertia;
obj.theta += obj.angVel * time;
obj.angVel += obj.torque * time / obj.inertia;
obj.torque = obj.angVel * time * -obj.angDamp * obj.inertia;
}
obj.f.set(obj.v.mul(time * -obj.damp * obj.mass));
}
Particle part;
for (int p = 0; p < particles.size(); p++) {
part = particles.get(p);
if (!part.isGhost) {
part.f = part.f.add(g.mul(part.mass));
}
part.cm.set(part.cm.add(part.v.mul(time)));
}
}
public void draw() {
if (isDestroyed) {
return;
}
if (rigidBody != null) {
rigidBody.draw();
}
for (int x = 0; x < components.size(); x++) {
if (components.get(x).isEnabled()) {
components.get(x).draw();
}
}
}
protected void synchronizeMotionStates() {
Transform interpolatedTransform = Stack.alloc(Transform.class);
Transform tmpTrans = Stack.alloc(Transform.class);
Vector3f tmpLinVel = Stack.alloc(Vector3f.class);
Vector3f tmpAngVel = Stack.alloc(Vector3f.class);
// todo: iterate over awake simulation islands!
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null && body.getMotionState() != null && !body.isStaticOrKinematicObject()) {
// we need to call the update at least once, even for sleeping objects
// otherwise the 'graphics' transform never updates properly
// so todo: add 'dirty' flag
// if (body->getActivationState() != ISLAND_SLEEPING)
{
TransformUtil.integrateTransform(
body.getInterpolationWorldTransform(tmpTrans),
body.getInterpolationLinearVelocity(tmpLinVel),
body.getInterpolationAngularVelocity(tmpAngVel),
localTime * body.getHitFraction(),
interpolatedTransform);
body.getMotionState().setWorldTransform(interpolatedTransform);
}
}
}
if (getDebugDrawer() != null
&& (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
for (int i = 0; i < vehicles.size(); i++) {
for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
// synchronize the wheels with the (interpolated) chassis worldtransform
vehicles.getQuick(i).updateWheelTransform(v, true);
}
}
}
}
protected void predictUnconstraintMotion(float timeStep) {
Transform tmpTrans = Stack.alloc(Transform.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
if (!body.isStaticOrKinematicObject()) {
if (body.isActive()) {
body.integrateVelocities(timeStep);
// damping
body.applyDamping(timeStep);
body.predictIntegratedTransform(
timeStep, body.getInterpolationWorldTransform(tmpTrans));
}
}
}
}
}
public void update() {
if (isDestroyed) {
return;
}
// technically should happen somewhereElse
if (rigidBody != null) {
// System.out.println("Updating RigidBody");
rigidBody.update();
}
for (int x = 0; x < components.size(); x++) {
if (components.get(x).isEnabled()) {
// System.out.println("Updating:" + c.getClass().getSimpleName());
components.get(x).update();
}
}
if (isDestroying) {
if (destroyTimeStamp < Time.getTime()) {
destroy();
}
}
}
protected void calculateSimulationIslands() {
getSimulationIslandManager()
.updateActivationState(getCollisionWorld(), getCollisionWorld().getDispatcher());
int i;
int numConstraints = constraints.size();
for (i = 0; i < numConstraints; i++) {
TypedConstraint constraint = constraints.getQuick(i);
RigidBody colObj0 = constraint.getRigidBodyA();
RigidBody colObj1 = constraint.getRigidBodyB();
if (((colObj0 != null) && (!colObj0.isStaticOrKinematicObject()))
&& ((colObj1 != null) && (!colObj1.isStaticOrKinematicObject()))) {
if (colObj0.isActive() || colObj1.isActive()) {
getSimulationIslandManager()
.getUnionFind()
.unite((colObj0).getIslandTag(), (colObj1).getIslandTag());
}
}
}
// Store the island id in each body
getSimulationIslandManager().storeIslandActivationState(getCollisionWorld());
}
protected void integrateTransforms(float timeStep) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Transform tmpTrans = Stack.alloc(Transform.class);
Transform predictedTrans = Stack.alloc(Transform.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.setHitFraction(1f);
if (body.isActive() && (!body.isStaticOrKinematicObject())) {
body.predictIntegratedTransform(timeStep, predictedTrans);
tmp.sub(predictedTrans.origin, body.getWorldTransform(tmpTrans).origin);
float squareMotion = tmp.lengthSquared();
if (body.getCcdSquareMotionThreshold() != 0f
&& body.getCcdSquareMotionThreshold() < squareMotion) {
if (body.getCollisionShape().isConvex()) {
ClosestNotMeConvexResultCallback sweepResults =
new ClosestNotMeConvexResultCallback(
body,
body.getWorldTransform(tmpTrans).origin,
predictedTrans.origin,
getBroadphase().getOverlappingPairCache(),
getDispatcher());
// ConvexShape convexShape = (ConvexShape)body.getCollisionShape();
SphereShape tmpSphere =
new SphereShape(
body
.getCcdSweptSphereRadius()); // btConvexShape* convexShape =
// static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.collisionFilterGroup = body.getBroadphaseProxy().collisionFilterGroup;
sweepResults.collisionFilterMask = body.getBroadphaseProxy().collisionFilterMask;
convexSweepTest(
tmpSphere, body.getWorldTransform(tmpTrans), predictedTrans, sweepResults);
// JAVA NOTE: added closestHitFraction test to prevent objects being stuck
if (sweepResults.hasHit() && (sweepResults.closestHitFraction > 0.0001f)) {
body.setHitFraction(sweepResults.closestHitFraction);
body.predictIntegratedTransform(timeStep * body.getHitFraction(), predictedTrans);
body.setHitFraction(0f);
// System.out.printf("clamped integration to hit fraction = %f\n",
// sweepResults.closestHitFraction);
}
}
}
body.proceedToTransform(predictedTrans);
}
}
}
}
protected void updateActivationState(float timeStep) {
Vector3f tmp = Stack.alloc(Vector3f.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.updateDeactivation(timeStep);
if (body.wantsSleeping()) {
if (body.isStaticOrKinematicObject()) {
body.setActivationState(CollisionObject.ISLAND_SLEEPING);
} else {
if (body.getActivationState() == CollisionObject.ACTIVE_TAG) {
body.setActivationState(CollisionObject.WANTS_DEACTIVATION);
}
if (body.getActivationState() == CollisionObject.ISLAND_SLEEPING) {
tmp.set(0f, 0f, 0f);
body.setAngularVelocity(tmp);
body.setLinearVelocity(tmp);
}
}
} else {
if (body.getActivationState() != CollisionObject.DISABLE_DEACTIVATION) {
body.setActivationState(CollisionObject.ACTIVE_TAG);
}
}
}
}
}
public void setRigidBody(RigidBody rigidBody) {
// System.out.println("Created RigidBody:"+rigidBody);
rigidBody.setGameObject(this);
this.rigidBody = rigidBody;
rigidBody.setGameObject(this);
}
