Fix integrate method for SO2

[Toefinder]

When the same vector reference is given to the integrate method as both
for input and for storing the output, the result is incorrectly
calculated due to inplace changes. This problem is fixed in this commit.
Close #1651

[jcarpent]

@Toefinder Thanks for the fix. It is a bit weird to use the same input/ouput arguments at first glance.
For readability, could you remove the line commits which are related to whitespace?

Thanks in advance,

[Toefinder]

It is a bit weird to use the same input/ouput arguments at first glance.

I thought so too, when first examining the code. But integrating in-place is used a lot as operator+=, such as this code. What surprised me even more after realising this bug was why this issue has not shown up earlier.

op.integrate(e_.template segment<LiegroupType::NQ>(iq_, op.nq()),
             v_.segment<LiegroupType::NV>(iv_, op.nv()),
             e_.template segment<LiegroupType::NQ>(iq_, op.nq()));

[jcarpent]

It is a bit weird to use the same input/ouput arguments at first glance.

I thought so too, when first examining the code. But integrating in-place is used a lot as operator+=, such as this code. What surprised me even more after realising this bug was why this issue has not shown up earlier.

I would say that because this part of the code is not more used with operator+= and some compilers may optimise some part of this code.

[jcarpent]

@Toefinder I've just force-pushed to align the histories.

[jcarpent]

Thanks @Toefinder, for your contributions. Greatly appreciated.