small rewrite of Body to State; TODO Intergrator rewrite