diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 420ee5925..19a81f16f 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -35,7 +35,7 @@ static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 2; +static int num_modes = 1; class BasicTest : public CommonDeformableBodyBase { @@ -45,9 +45,15 @@ class BasicTest : public CommonDeformableBodyBase // get deformed shape void getDeformedShape(btReducedSoftBody* rsb, const int mode_n, const btScalar scale = 1) { - for (int i = 0; i < rsb->m_nodes.size(); ++i) - for (int k = 0; k < 3; ++k) - rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale; + // for (int i = 0; i < rsb->m_nodes.size(); ++i) + // for (int k = 0; k < 3; ++k) + // rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale; + + rsb->m_reducedDofs[mode_n] = scale; + rsb->mapToFullDofs(); + std::cout << "-----------\n"; + std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n'; + std::cout << "-----------\n"; } public: @@ -92,13 +98,13 @@ public: void stepSimulation(float deltaTime) { // TODO: remove this. very hacky way of adding initial deformation - btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); - if (first_step /* && !rsb->m_bUpdateRtCst*/) - { - getDeformedShape(rsb, 0, 0.5); - first_step = false; - rsb->mapToReducedDofs(); - } + // btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); + // if (first_step /* && !rsb->m_bUpdateRtCst*/) + // { + // getDeformedShape(rsb, 0, 1); + // first_step = false; + // // rsb->mapToReducedDofs(); + // } float internalTimeStep = 1. / 60.f; m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); @@ -175,7 +181,7 @@ void BasicTest::initPhysics() // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); // rsb->setRigidVelocity(btVector3(0, 1, 0)); - // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); + rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); // getDeformableDynamicsWorld()->addForce(rsb, gravity_force); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index adaf0647c..47f4cfd75 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -36,7 +36,7 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_ m_reducedVelocity.resize(m_nReduced, 0); m_reducedForce.resize(m_nReduced, 0); m_nodalMass.resize(full_size, 0); - endOfTimeStepZeroing(); + m_localMomentArm.resize(m_nFull); } void btReducedSoftBody::setMassProps(const tDenseArray& mass_array) @@ -97,6 +97,75 @@ void btReducedSoftBody::setFixedNodes() m_fixedNodes.push_back(0); } +void btReducedSoftBody::internalInitialization() +{ + // zeroing + endOfTimeStepZeroing(); + // initialize rest position + updateRestNodalPositions(); + // initialize projection matrix + updateExternalForceProjectMatrix(false); + // initialize local nodal moment arm form the CoM + updateLocalMomentArm(); +} + +void btReducedSoftBody::updateLocalMomentArm() +{ + TVStack delta_x; + delta_x.resize(m_nFull); + + for (int i = 0; i < m_nFull; ++i) + { + for (int k = 0; k < 3; ++k) + { + // compute displacement + delta_x[i][k] = 0; + for (int j = 0; j < m_nReduced; ++j) + { + delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j]; + } + } + // get new moment arm Sq + x0 + m_localMomentArm[i] = m_x0[i] - m_initialOrigin + delta_x[i]; + } +} + +void btReducedSoftBody::updateExternalForceProjectMatrix(bool initialized) +{ + // if not initialized, need to compute both P_A and Cq + // otherwise, only need to udpate Cq + if (!initialized) + { + // resize + m_projPA.resize(m_nReduced); + m_projCq.resize(m_nReduced); + + // P_A + for (int r = 0; r < m_nReduced; ++r) + { + m_projPA[r].setValue(0, 0, 0); + for (int i = 0; i < m_nFull; ++i) + { + btScalar ratio = m_nodalMass[i] / m_mass; + m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio, + - m_modes[r][3 * i + 1] * ratio, + - m_modes[r][3 * i + 2] * ratio); + } + } + } + + // C(q) is updated once per position update + for (int r = 0; r < m_nReduced; ++r) + { + m_projCq[r].setValue(0, 0, 0); + for (int i = 0; i < m_nFull; ++i) + { + btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]); + } + } +} + void btReducedSoftBody::endOfTimeStepZeroing() { for (int i = 0; i < m_nReduced; ++i) @@ -110,23 +179,6 @@ void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransfor btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } -void btReducedSoftBody::mapToReducedDofs() -{ - btAssert(m_reducedDofs.size() == m_nReduced); - for (int j = 0; j < m_nReduced; ++j) - { - m_reducedDofs[j] = 0; - for (int i = 0; i < m_nFull; ++i) - { - for (int k = 0; k < 3; ++k) - { - // std::cout << m_nodes[i].m_x[k] - m_x0[i][k] << "\n"; - m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[i][k]); - } - } - } -} - void btReducedSoftBody::updateReducedDofs(btScalar solverdt) { for (int r = 0; r < m_nReduced; ++r) @@ -135,35 +187,14 @@ void btReducedSoftBody::updateReducedDofs(btScalar solverdt) } } -void btReducedSoftBody::updateFullDofs(btScalar solverdt) -{ - for (int i = 0; i < m_nFull; ++i) - { - m_nodes[i].m_x += solverdt * m_nodes[i].m_v; - } -} - void btReducedSoftBody::mapToFullDofs() { - btAssert(m_nodes.size() == m_nFull); - TVStack delta_x; - delta_x.resize(m_nFull); btVector3 origin = m_rigidTransformWorld.getOrigin(); btMatrix3x3 rotation = m_rigidTransformWorld.getBasis(); for (int i = 0; i < m_nFull; ++i) { - for (int k = 0; k < 3; ++k) - { - // compute displacement - delta_x[i][k] = 0; - for (int j = 0; j < m_nReduced; ++j) - { - delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j]; - } - } - // get new coordinates - m_nodes[i].m_x = rotation * (m_x0[i] - m_initialOrigin + delta_x[i]) + origin; //TODO: assume the initial origin is at (0,0,0) + m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin; } } @@ -174,18 +205,18 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt) { btScalar mass_inv = (m_Mr[r] == 0) ? 0 : 1.0 / m_Mr[r]; // TODO: this might be redundant, because Mr is identity btScalar delta_v = solverdt * mass_inv * m_reducedForce[r]; - m_reducedVelocity[r] -= delta_v; //! check this + m_reducedVelocity[r] += delta_v; } } -void btReducedSoftBody::updateFullVelocity(btScalar solverdt) +void btReducedSoftBody::mapToFullVelocity(btScalar solverdt) { TVStack v_from_reduced; v_from_reduced.resize(m_nFull); // btVector3 current_com = m_rigidTransformWorld.getOrigin() - m_initialOrigin; for (int i = 0; i < m_nFull; ++i) { - btVector3 r = m_nodes[i].m_x - m_rigidTransformWorld.getOrigin(); + btVector3 r_com = m_nodes[i].m_x - m_rigidTransformWorld.getOrigin(); // compute velocity contributed by the reduced velocity for (int k = 0; k < 3; ++k) @@ -198,18 +229,11 @@ void btReducedSoftBody::updateFullVelocity(btScalar solverdt) } // get new velocity - m_nodes[i].m_v = m_angularVelocity.cross(r) + + m_nodes[i].m_v = m_angularVelocity.cross(r_com) + m_rigidTransformWorld.getBasis() * v_from_reduced[i] + m_linearVelocity; } -} - -void btReducedSoftBody::updateMeshNodePositions(btScalar solverdt) -{ - for (int i = 0; i < m_nFull; ++i) - { - m_nodes[i].m_x = solverdt * m_nodes[i].m_v; - } + std::cout << m_nodes[0].m_v[0] << '\t' << m_nodes[0].m_v[1] << '\t' << m_nodes[0].m_v[2] << '\n'; } void btReducedSoftBody::proceedToTransform(const btTransform& newTrans) @@ -274,7 +298,7 @@ void btReducedSoftBody::applyTorqueImpulse(const btVector3& torque) #endif } -void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3& rel_pos) +void btReducedSoftBody::applyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos) { if (m_inverseMass != btScalar(0.)) { @@ -288,46 +312,65 @@ void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3& void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt) { - // zero the reduced force vector - endOfTimeStepZeroing(); + // TODO: get correct impulse using the impulse factor + btVector3 ri = m_localMomentArm[n_node]; + btMatrix3x3 ri_skew(0, -ri[2], ri[1], + ri[2], 0, -ri[0], + -ri[1], ri[0], 0); - // impulse leads to the deformation in the reduced space - btVector3 impulse = m_nodalMass[n_node] / dt * (target_vel - m_nodes[n_node].m_v); - for (int i = 0; i < m_nReduced; ++i) + // calcualte impulse factor + btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0); + btMatrix3x3 impulse_factor(inv_mass, 0, 0, + 0, inv_mass, 0, + 0, 0, inv_mass); + impulse_factor -= ri_skew.scaled(m_invInertiaLocal) * ri_skew; + + // get impulse + btVector3 impulse = impulse_factor * (target_vel - m_nodes[n_node].m_v); + + // apply impulse force + applyFullSpaceNodalForce(impulse / dt, n_node); + + // impulse causes rigid motion + applyRigidImpulse(impulse, m_nodes[n_node].m_x); +} + +void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node) +{ + // f_local = R^-1 * f_ext + btVector3 f_local = m_worldTransform.getBasis().transpose() * f_ext; + + // f_scaled = localInvInertia * (r_k x f_local) + btVector3 rk_cross_f_local = m_localMomentArm[n_node].cross(f_local); + btVector3 f_scaled(0, 0, 0); + for (int k = 0; k < 3; ++k) + { + f_scaled[k] = m_invInertiaLocal[k] * rk_cross_f_local[k]; + } + + // f_ext_r = [S^T * P]_{n_node} * f_local + for (int r = 0; r < m_nReduced; ++r) { for (int k = 0; k < 3; ++k) { - m_reducedForce[i] = m_modes[i][3 * n_node + k] * impulse[k]; + m_reducedForce[r] += m_modes[r][3 * n_node + k] * f_local[k] + + m_projPA[r][k] * f_local[k] + + m_projCq[r][k] * f_scaled[k]; } } - - // impulse causes rigid motion - applyImpulse(impulse, m_nodes[n_node].m_x); - - // apply impulse to the reduced velocity - updateReducedVelocity(dt); - - // apply impulse force, update velocity - updateFullVelocity(dt); } void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) { // update rigid frame velocity m_linearVelocity += dt * gravity; - - // // update nodal velocity - // for (int i = 0; i < m_nFull; ++i) - // { - // m_nodes[i].m_v += dt * gravity; - // } } void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta) { for (int r = 0; r < m_nReduced; ++r) { - m_reducedForce[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); + m_reducedForce[r] = - m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); } } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 08bc045eb..888681005 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -10,11 +10,25 @@ // Reduced deformable body is a simplified deformable object embedded in a rigid frame. class btReducedSoftBody : public btSoftBody { + public: + // + // Typedefs + // + typedef btAlignedObjectArray TVStack; + typedef btAlignedObjectArray tDenseArray; + typedef btAlignedObjectArray > tDenseMatrix; + private: // scaling factors btScalar m_rhoScale; // mass density scale btScalar m_ksScale; // stiffness scale + // projection matrix + TVStack m_projPA; // Eqn. 4.11 from Rahul Sheth's thesis + TVStack m_projCq; + + TVStack m_localMomentArm; // Sq + x0 + protected: // rigid frame btScalar m_mass; // total mass of the rigid frame @@ -28,12 +42,6 @@ class btReducedSoftBody : public btSoftBody btVector3 m_initialOrigin; // initial center of mass (original of the m_rigidTransformWorld) public: - // - // Typedefs - // - typedef btAlignedObjectArray TVStack; - typedef btAlignedObjectArray tDenseArray; - typedef btAlignedObjectArray > tDenseMatrix; btVector3 m_linearVelocity; // @@ -66,6 +74,11 @@ class btReducedSoftBody : public btSoftBody ~btReducedSoftBody() {} + // + // initializing helpers + // + void internalInitialization(); + void setReducedModes(int start_mode, int num_modes, int full_size); void setMassProps(const tDenseArray& mass_array); @@ -82,40 +95,39 @@ class btReducedSoftBody : public btSoftBody void setFixedNodes(); + // + // various internal updates + // virtual void translate(const btVector3& trs); void updateRestNodalPositions(); void updateInertiaTensor(); + void updateLocalMomentArm(); + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + // update the external force projection matrix + void updateExternalForceProjectMatrix(bool initialized); + void endOfTimeStepZeroing(); // - // reduced dof related + // position and velocity update related // // compute reduced degree of freedoms void updateReducedDofs(btScalar solverdt); - // map to reduced degree of freedoms - void mapToReducedDofs(); - - // compute full degree of freedoms - void updateFullDofs(btScalar solverdt); + // compute reduced velocity update + void updateReducedVelocity(btScalar solverdt); // map to full degree of freedoms void mapToFullDofs(); - // compute reduced velocity update - void updateReducedVelocity(btScalar solverdt); - // compute full space velocity from the reduced velocity - void updateFullVelocity(btScalar solverdt); - - // update the full space mesh positions - void updateMeshNodePositions(btScalar solverdt); + void mapToFullVelocity(btScalar solverdt); // // rigid motion related @@ -124,12 +136,23 @@ class btReducedSoftBody : public btSoftBody void applyTorqueImpulse(const btVector3& torque); + void proceedToTransform(const btTransform& newTrans); + + void setCenterOfMassTransform(const btTransform& xform); + + // + // force related + // + // apply impulse to the rigid frame - void applyImpulse(const btVector3& impulse, const btVector3& rel_pos); + void applyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos); // apply impulse to nodes in the full space void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt); + // apply nodal external force in the full space + void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node); + // apply fixed contraints to the nodes void applyFixedContraints(btScalar dt); @@ -139,10 +162,9 @@ class btReducedSoftBody : public btSoftBody // apply reduced force void applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta); - void proceedToTransform(const btTransform& newTrans); - - void setCenterOfMassTransform(const btTransform& xform); - + // + // accessors + // btScalar getTotalMass() const { return m_mass; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp index e8989876d..c0c222aca 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp @@ -126,9 +126,6 @@ void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftB std::string modes_file = std::string(file_path) + "modes.bin"; btReducedSoftBodyHelpers::readBinaryModes(rsb->m_modes, rsb->m_startMode, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str()); // default to 3D - - // get rest position - rsb->updateRestNodalPositions(); // read in full nodal mass std::string M_file = std::string(file_path) + "M_diag_mat.bin"; @@ -140,6 +137,9 @@ void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftB btVector3 inertia(0, 0, 0); calculateLocalInertia(inertia, rsb->getTotalMass(), btVector3(4, 1, 0.5), btVector3(0, 0, 0)); rsb->setInertiaProps(inertia); + + // other internal initialization + rsb->internalInitialization(); } // read in binary files diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index feab9a938..90e8dd231 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -5,6 +5,7 @@ btReducedSoftBodySolver::btReducedSoftBodySolver() { m_dampingAlpha = 0; m_dampingBeta = 0; + m_simTime = 0; m_gravity = btVector3(0, 0, 0); } @@ -33,18 +34,25 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + // rigid motion + btTransform predictedTrans; + rsb->predictIntegratedTransform(solverdt, predictedTrans); + rsb->proceedToTransform(predictedTrans); + + // apply fixed constraints + rsb->applyFixedContraints(solverdt); + + // update reduced velocity and dofs rsb->updateReducedVelocity(solverdt); - // map reduced velocity to full space velocity - rsb->updateFullVelocity(solverdt); - - // update mesh nodal position + // TODO: update mesh nodal position. need it for collision // rsb->updateMeshNodePositions(solverdt); } } void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) { + static bool applied = false; for (int i = 0; i < m_softBodies.size(); ++i) { btReducedSoftBody* rsb = static_cast(m_softBodies[i]); @@ -54,6 +62,13 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt) // add internal force (elastic force & damping force) rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta); + + // apply external force or impulses + if (!applied && m_simTime > 2) + { + rsb->applyFullSpaceImpulse(btVector3(0, -5, 0), 0, solverdt); + applied = true; + } } } @@ -72,19 +87,21 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) rsb->proceedToTransform(predictedTrans); // update mesh nodal positions for the next time step - rsb->updateFullDofs(timeStep); + rsb->mapToFullDofs(); - // end of time step clean up + // end of time step clean up and update + rsb->updateExternalForceProjectMatrix(true); rsb->endOfTimeStepZeroing(); } + m_simTime += timeStep; } void btReducedSoftBodySolver::solveConstraints(btScalar timeStep) { - // for (int i = 0; i < m_softBodies.size(); ++i) - // { - // btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - // rsb->applyFixedContraints(timeStep); - // } + rsb->applyFixedContraints(timeStep); + } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index 33fb28a5d..c456c042a 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -9,6 +9,7 @@ class btReducedSoftBody; class btReducedSoftBodySolver : public btDeformableBodySolver { protected: + btScalar m_simTime; btScalar m_dampingAlpha; btScalar m_dampingBeta; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 3fe69031b..46e886cd8 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -87,23 +87,23 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t ///apply gravity and explicit force to velocity, predict motion predictUnconstraintMotion(timeStep); - ///perform collision detection that involves rigid/multi bodies - btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + // ///perform collision detection that involves rigid/multi bodies + // btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); - btMultiBodyDynamicsWorld::calculateSimulationIslands(); + // btMultiBodyDynamicsWorld::calculateSimulationIslands(); - beforeSolverCallbacks(timeStep); + // beforeSolverCallbacks(timeStep); - ///solve contact constraints and then deformable bodies momemtum equation + // ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); - afterSolverCallbacks(timeStep); + // afterSolverCallbacks(timeStep); - performDeformableCollisionDetection(); + // performDeformableCollisionDetection(); - applyRepulsionForce(timeStep); + // applyRepulsionForce(timeStep); - performGeometricCollisions(timeStep); + // performGeometricCollisions(timeStep); integrateTransforms(timeStep);