I'm unfamiliar with how to write the optimized code in geometry.c .  Is this something you could help with?