PhysicsSystem.java

    private void physicsTimeStep(Level level) {

        // Integrate forces
        level.forEachComponent(PhysicsComponent.class, physComp -> {
            physComp.momentum.add(physComp.force);
            physComp.angularMomentum.add(physComp.torque);
        });

        // Solve velocity constraints
        level.forEachComponent(PhysicsComponent.class, component ->
                component.joints.forEach(joint ->
                        joint.initVelocityConstraints(level)));

        for (int i = 0; i < Constants.VELOCITY_CONSTRAINT_ITERS; i++) {
            level.forEachComponent(PhysicsComponent.class, component ->
                    component.joints.forEach(joint ->
                            joint.solveVelocityConstraints(level)));
        }

        // Integrate velocities
        level.forEachEntityWithComponent(PhysicsComponent.class, entity -> {
            PositionComponent posComp = level.getComponent(entity, PositionComponent.class);
            PhysicsComponent physComp = level.getComponent(entity, PhysicsComponent.class);

            Vector3f velocity = physComp.getVelocity();
            Vector3fc pos = posComp.getPos();
            posComp.setPos(pos.x() + velocity.x,
                    pos.y() + velocity.y,
                    pos.z() + velocity.z);


            Vector3f angularVelocity = physComp.getAngularVelocity(posComp.getRot());
            if (angularVelocity.lengthSquared() > 1E-6f)
                posComp.setRot(new Quaternionf(new AxisAngle4f(angularVelocity.length(), angularVelocity.normalize(new Vector3f())))
                        .mul(posComp.getRot()).normalize());
        });

        // Solve position constraints
        boolean positionSolved = false;
        for (int i = 0; i < Constants.POSITION_CONSTRAINT_ITERS; i++) {

            boolean jointsSolved = true;
            for (BaseComponent baseComp : level.getComponentLists().get(PhysicsComponent.class).values()) {
                PhysicsComponent physComp = (PhysicsComponent) baseComp;
                for (Joint joint : physComp.joints) {
                    jointsSolved &= joint.solvePositionConstraints(level);
                }
            }

            if (jointsSolved) {
                positionSolved = true;
                break;
            }
        }
    }