Convert class to struct and fix struct copy back

Fixed some velocity and position constraint copy back operations and convert some classes back to structs.