New correct width/height algoirthms. Updated optimize origin function so the original origin is always accounted for.