begin and end sensor events

Author: erincatto

samples/sample_stacking.cpp

@@ -16,6 +16,43 @@
 class SingleBox : public Sample
 {
 public:
+#if 1
+	explicit SingleBox( Settings& settings )
+		: Sample( settings )
+	{
+		// todo_erin test me
+		// https://discord.com/channels/460295137705197568/1064661081903075429/1330362522888437814
+		assert( false );
+
+		if ( settings.restart == false )
+		{
+			g_camera.m_center = { 0.0f, 2.5f };
+			g_camera.m_zoom = 3.5f;
+		}
+
+		float extent = 0.5f;
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			b2Polygon box = b2MakeBox( extent, extent );
+			bodyDef.position = { 0.0f, 20.0f };
+			;
+			bodyDef.type = b2_dynamicBody;
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			b2Polygon box = b2MakeBox( extent, extent );
+			bodyDef.position = { 0.0f, 0.0f };
+			bodyDef.type = b2_staticBody;
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+	}
+#else
 	explicit SingleBox( Settings& settings )
 		: Sample( settings )
 	{
@@ -43,6 +80,7 @@ class SingleBox : public Sample
 		b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
 		b2CreatePolygonShape( bodyId, &shapeDef, &box );
 	}
+#endif
 
 	void Step( Settings& settings ) override
 	{

src/sensor.c

@@ -6,6 +6,7 @@
 #include "array.h"
 #include "body.h"
 #include "contact.h"
+#include "ctz.h"
 #include "shape.h"
 #include "world.h"
 
@@ -14,7 +15,7 @@
 #include <stddef.h>
 #include <stdlib.h>
 
-B2_ARRAY_SOURCE(b2Sensor, b2Sensor);
+B2_ARRAY_SOURCE( b2Sensor, b2Sensor );
 
 struct b2SensorTaskContext
 {
@@ -48,6 +49,8 @@ struct b2SensorQueryContext
 
 static bool b2SensorQueryCallback( int proxyId, int shapeId, void* context )
 {
+	B2_MAYBE_UNUSED( proxyId );
+
 	struct b2SensorQueryContext* queryContext = context;
 	b2Shape* sensorShape = queryContext->sensorShape;
 	int sensorShapeId = sensorShape->id;
@@ -109,19 +112,19 @@ static int b2CompareShapeRefs( const void* a, const void* b )
 	const b2ShapeRef* sa = a;
 	const b2ShapeRef* sb = b;
 
-	if (sa->shapeId < sb->shapeId)
+	if ( sa->shapeId < sb->shapeId )
 	{
 		return -1;
 	}
 
-	if (sa->shapeId == sb->shapeId)
+	if ( sa->shapeId == sb->shapeId )
 	{
-		if (sa->generation < sb->generation)
+		if ( sa->generation < sb->generation )
 		{
 			return -1;
 		}
-		
-		if (sa->generation == sb->generation)
+
+		if ( sa->generation == sb->generation )
 		{
 			return 0;
 		}
@@ -130,11 +133,6 @@ static int b2CompareShapeRefs( const void* a, const void* b )
 	return 1;
 }
 
-static bool b2AreShapeRefsEqual(const b2ShapeRef* a, const b2ShapeRef* b)
-{
-	return a->shapeId == b->shapeId && a->generation == b->generation;
-}
-
 void b2OverlapSensors( b2World* world )
 {
 	int sensorCount = world->sensors.count;
@@ -182,34 +180,122 @@ void b2OverlapSensors( b2World* world )
 
 		int count1 = sensor->overlaps1.count;
 		int count2 = sensor->overlaps2.count;
-		int index1 = 0, index2 = 0;
-		while (index1 < count1 && index2 < count2)
+		if ( count1 != count2 )
 		{
-			b2ShapeRef* s1 = sensor->overlaps1.data + index1;
-			b2ShapeRef* s2 = sensor->overlaps2.data + index2;
-
-			if ( s1->shapeId == s2->shapeId && s1->generation == s2->generation)
+			// something changed
+			b2SetBit( &taskContext.sensorEventBits, sensorIndex );
+		}
+		else
+		{
+			for ( int i = 0; i < count1; ++i )
 			{
-				index1 += 1;
-				index2 += 1;
+				b2ShapeRef* s1 = sensor->overlaps1.data + i;
+				b2ShapeRef* s2 = sensor->overlaps2.data + i;
+
+				if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation )
+				{
+					// something changed
+					b2SetBit( &taskContext.sensorEventBits, sensorIndex );
+					break;
+				}
 			}
-			else
+		}
+	}
+
+	// Iterate sensors bits and publish events
+	// Process contact state changes. Iterate over set bits
+	uint64_t* bits = taskContext.sensorEventBits.bits;
+	uint32_t blockCount = taskContext.sensorEventBits.blockCount;
+
+	for ( uint32_t k = 0; k < blockCount; ++k )
+	{
+		uint64_t word = bits[k];
+		while ( word != 0 )
+		{
+			uint32_t ctz = b2CTZ64( word );
+			int sensorIndex = (int)( 64 * k + ctz );
+
+			b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorIndex );
+			b2Shape* sensorShape = b2ShapeArray_Get( &world->shapes, sensor->shapeId );
+			b2ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation };
+
+			int count1 = sensor->overlaps1.count;
+			int count2 = sensor->overlaps2.count;
+			const b2ShapeRef* refs1 = sensor->overlaps1.data;
+			const b2ShapeRef* refs2 = sensor->overlaps2.data;
+
+			// overlaps1 can have overlaps that end
+			// overlaps2 can have overlaps that begin
+			int index1 = 0, index2 = 0;
+			while ( index1 < count1 && index2 < count2 )
 			{
-				b2SetBit( &taskContext.sensorEventBits, sensorIndex );
+				const b2ShapeRef* r1 = refs1 + index1;
+				const b2ShapeRef* r2 = refs2 + index2;
+				if ( r1->shapeId == r2->shapeId )
+				{
+					if ( r1->generation < r2->generation )
+					{
+						// end
+						b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+						b2SensorEndTouchEvent event = { sensorId, visitorId };
+						b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event );
+						index1 += 1;
+					}
+					else if ( r1->generation > r2->generation )
+					{
+						// begin
+						b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+						b2SensorBeginTouchEvent event = { sensorId, visitorId };
+						b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event );
+						index2 += 1;
+					}
+					else
+					{
+						// persisted
+						index1 += 1;
+						index2 += 1;
+					}
+				}
+				else if ( r1->shapeId < r2->shapeId )
+				{
+					// end
+					b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+					b2SensorEndTouchEvent event = { sensorId, visitorId };
+					b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event );
+					index1 += 1;
+				}
+				else
+				{
+					// begin
+					b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+					b2SensorBeginTouchEvent event = { sensorId, visitorId };
+					b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event );
+					index2 += 1;
+				}
+			}
 
-				// Begin event
-				b2BitSet_Set( &taskContext.sensorEventBits, sensorIndex );
+			while ( index1 < count1 )
+			{
+				// end
+				const b2ShapeRef* r1 = refs1 + index1;
+				b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+				b2SensorEndTouchEvent event = { sensorId, visitorId };
+				b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event );
 				index1 += 1;
 			}
-			else
+
+			while ( index2 < count2 )
 			{
+				// begin
+				const b2ShapeRef* r2 = refs2 + index2;
+				b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+				b2SensorBeginTouchEvent event = { sensorId, visitorId };
+				b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event );
 				index2 += 1;
 			}
 		}
-		for (int index1 = 0; index1 < count1; ++index1)
-		{
-		}
-	}
 
-	// Iterate sensors bits and publish events
+		// Clear the smallest set bit
+		word = word & ( word - 1 );
+	}
 }