Most moves are on the XY plane - avoid a few multiplications in the
inner loop in this case. When there is a Z move, it is almost always
entirely a Z move - avoid the sqrt() call in the inner loop in this
case.
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>