Added logging of SAP calculations. Fixed a few important corner-cases in SVP/SAP. Started to integrated the SAP/SVP 'is_in_bounds' function in the free-workspace topologies.