Mask the high half when decoding SAP pairs to avoid a negative index

[Abhayindia]

Fixes #3535.

mj_SAP packs a pair of item indices as (id1<<16) + id2 into a signed int. Once id1 >= 0x8000 that sets bit 31, so the packed value is negative. Two sites decode the high half with a bare arithmetic shift:

int bf1 = bfid[sappair[i] >> 16];   // engine_collision_driver.c, body broadphase
int e1  = elid[sappair[i] >> 16];   // mj_collideFlexSAP

so the shift sign-extends and the array is indexed at a negative offset - an out-of-bounds read (SEGV in the flex path once the garbage id feeds unchecked pointer arithmetic). A third site decoding the same packing already masks it:

int bf1 = (broadphasepair[i]>>16) & 0xFFFF;   // line ~639

This brings the other two in line with & 0xFFFF. Reachable with a 2D flex over ~33k elements and selfcollide="auto", or a plain model with more than 32768 mutually-collidable bodies (mj_SAP only rejects n >= 0x10000).

No unit test: triggering it needs >32768 collidable bodies, and a model that size takes minutes to compile. The issue has a full AddressSanitizer trace of the pre-fix crash.

[sylvesterkaczmarek]

Masking the high half fixes the negative decode, but the packed value is still created with signed (id1 << 16), which is undefined once id1 >= 0x8000. Please make the packed representation unsigned end-to-end rather than only masking the decode.

[Abhayindia]

good call. reworked it: the pair buffers, mj_SAP/mj_broadphase/add_pair and the bfsort comparator are all unsigned now, so the (id << 16) pack is defined and the decodes go back to a plain >> 16 / & 0xFFFF. no signed shift left in the path. force-pushed.

[sylvesterkaczmarek]

Rechecked the current head. Pair storage and sort buffers are unsigned end to end, and packing casts before the shift, so IDs at or above 0x8000 no longer invoke signed-shift undefined behaviour. My previous blocker is resolved.