Continue work on switching to new maths

Contact solver appears to be broken
Signed-off-by: Aleksi Sapon <QcTechs@gmail.com>