RigidBodyGroup.cpp

void RigidBodyGroup::physicsStep(GLfloat timeDelta) {
    for(RigidBody* body: bodies) {
        /* Compute force at current position */
        body->force = _gravity;
        body->physicsStep();

        GLfloat accelerationHalfTimeDelta = (body->force/(body->_mass*2))*timeDelta;

        /* New position */
        body->transformation()[3].setXyz(body->transformation()[3].xyz() + (body->velocity + accelerationHalfTimeDelta)*timeDelta);

        /* Compute force at new position */
        body->force = _gravity;
        body->physicsStep();

        /* New velocity */
        body->velocity += accelerationHalfTimeDelta + (body->force/(body->_mass*2))*timeDelta;
    }
}