Gear joints involving static bodies sometimes don't work

[Dinaroozie]

This seems to be an issue with the way islands are created sometimes with gear joints. My use case was as follows:

G1 and G2 are dynamic rigid bodies (gears), W is a static body (world), R1 and R2 are both revolute joints. R1 has G1 and W as bodyA and bodyB respectively, and R2 has G2 and W as bodyA and bodyB. Finally, a gear joint is created that connects R1 and R2.

In my case, when I did this nothing worked (it was as though the gear joint didn't exist). Upon further investigation, the gear joint was being created but not being updated, because it wasn't being added to any islands. I was able to workaround the issue by swapping bodyA and bodyB on both the revolute joints (assigned G1/G2 to bodyB instead of bodyA).

I believe what's going on is that gear joints have four bodies involved, but only one joint edge to connect them. The joint edge connects one of the bodies from the first 'sub joint', to one of the bodies in the other 'sub joint'. Which of these bodies actually gets connected is somewhat arbitrary. When the islands are created by 'floodfilling' bodies via joint edge connections, the flood fill ignores joints that connect static bodies - in my case, the gear joint edge was unluckily connecting static bodies, being ignored by the flood fill, and thus the gear joint was never getting added to an island, and never updating.

I suspect this could be solved by ensuring a gear joint's m_bodyA and m_bodyB choose the dynamic body (of which there is presumably at least one) from each joint. But I can't say I fully understand the maths behind the constraint solving system, so maybe it's more complicated than that.

It's maybe worth noting that in my scenario both the static bodies involved were actually the same body. I don't think this is relevant, but the way my project is set up makes it difficult to test the alternative, so I can't say for sure.

[erincatto]

Fixed in #652 (well I added some asserts and comments).