Made a few changes, seems to have an effect (don't really know why). Collision detection seems to no work completely with the SVP/SAP topologies.