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>