Adds DEV_ASSERTS that will halt at runtime if the BVH is misused with invalid IDs, and adds ERR_FAIL macros to prevent calling with invalid IDs. Any such misuse is a bug in the physics, but this should flag any errors quickly.