sensor fixes

samples/sample_bodies.cpp

@@ -584,21 +584,27 @@ class Sleep : public Sample
 
 			b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}};
 			b2ShapeDef shapeDef = b2DefaultShapeDef();
-			b2CreateSegmentShape(groundId, &shapeDef, &segment);
+			m_groundShapeId = b2CreateSegmentShape(groundId, &shapeDef, &segment);
 		}
 
-		// Sleeping body
+		// Sleeping body with sensors
+		for (int i = 0; i < 2; ++i)
 		{
 			b2BodyDef bodyDef = b2DefaultBodyDef();
 			bodyDef.type = b2_dynamicBody;
-			bodyDef.position = {-4.0f, 3.0f};
+			bodyDef.position = {-4.0f, 3.0f + 2.0f * i};
 			bodyDef.isAwake = false;
 			bodyDef.enableSleep = true;
 			b2BodyId bodyId = b2CreateBody(m_worldId, &bodyDef);
 
-			b2Capsule capsule = {{0.0f, 1.0f}, {1.0f, 1.0f}, 1.0f};
+			b2Capsule capsule = {{0.0f, 1.0f}, {1.0f, 1.0f}, 0.75f};
 			b2ShapeDef shapeDef = b2DefaultShapeDef();
 			b2CreateCapsuleShape(bodyId, &shapeDef, &capsule);
+
+			shapeDef.isSensor = true;
+			capsule.radius = 1.0f;
+			m_sensorIds[i] = b2CreateCapsuleShape(bodyId, &shapeDef, &capsule);
+			m_sensorTouching[i] = false;
 		}
 
 		// Sleeping body but sleep is disabled
@@ -695,12 +701,61 @@ class Sleep : public Sample
 		ImGui::End();
 	}
 
+	void Step(Settings& settings) override
+	{
+		Sample::Step(settings);
+
+		// Detect sensors touching the ground
+		b2SensorEvents sensorEvents = b2World_GetSensorEvents(m_worldId);
+
+		for (int i = 0; i < sensorEvents.beginCount; ++i)
+		{
+			b2SensorBeginTouchEvent* event = sensorEvents.beginEvents + i;
+			if (B2_ID_EQUALS(event->visitorShapeId, m_groundShapeId))
+			{
+				if (B2_ID_EQUALS(event->sensorShapeId, m_sensorIds[0]))
+				{
+					m_sensorTouching[0] = true;
+				}
+				else if (B2_ID_EQUALS(event->sensorShapeId, m_sensorIds[1]))
+				{
+					m_sensorTouching[1] = true;
+				}
+			}
+		}
+
+		for (int i = 0; i < sensorEvents.endCount; ++i)
+		{
+			b2SensorEndTouchEvent* event = sensorEvents.endEvents + i;
+			if (B2_ID_EQUALS(event->visitorShapeId, m_groundShapeId))
+			{
+				if (B2_ID_EQUALS(event->sensorShapeId, m_sensorIds[0]))
+				{
+					m_sensorTouching[0] = false;
+				}
+				else if (B2_ID_EQUALS(event->sensorShapeId, m_sensorIds[1]))
+				{
+					m_sensorTouching[1] = false;
+				}
+			}
+		}
+
+		for (int i = 0; i < 2; ++i)
+		{
+			g_draw.DrawString(5, m_textLine, "sensor touch %d = %s", i, m_sensorTouching[i] ? "true" : "false");
+			m_textLine += m_textIncrement;
+		}
+	}
+
 	static Sample* Create(Settings& settings)
 	{
 		return new Sleep(settings);
 	}
 
 	b2BodyId m_pendulumId;
+	b2ShapeId m_groundShapeId;
+	b2ShapeId m_sensorIds[2];
+	bool m_sensorTouching[2];
 };
 
 static int sampleSleep = RegisterSample("Bodies", "Sleep", Sleep::Create);

src/broad_phase.c

@@ -224,6 +224,12 @@ static bool b2PairQueryCallback(int proxyId, int shapeId, void* context)
 		return true;
 	}
 
+	// Sensors don't collide with other sensors
+	if (shapeA->isSensor == true && shapeB->isSensor == true)
+	{
+		return true;
+	}
+
 	// Does a joint override collision?
 	b2Body* bodyA = b2GetBody(world, bodyIdA);
 	b2Body* bodyB = b2GetBody(world, bodyIdB);

src/contact.h

@@ -13,20 +13,23 @@ typedef struct b2World b2World;
 
 enum b2ContactFlags
 {
-	// Set when the shapes are touching.
-	b2_contactTouchingFlag = 0x00000002,
+	// Set when the non-sensor shapes are touching.
+	b2_contactTouchingFlag = 0x00000001,
 
 	// Contact has a hit event
-	b2_contactHitEventFlag = 0x00000004,
+	b2_contactHitEventFlag = 0x00000002,
 
 	// One of the shapes is a sensor
-	b2_contactSensorFlag = 0x00000010,
+	b2_contactSensorFlag = 0x0000004,
+
+	// Set when a sensor is touching
+	b2_contactSensorTouchingFlag = 0x00000008,
 
 	// This contact wants sensor events
-	b2_contactEnableSensorEvents = 0x00000100,
+	b2_contactEnableSensorEvents = 0x00000010,
 
 	// This contact wants contact events
-	b2_contactEnableContactEvents = 0x00000200,
+	b2_contactEnableContactEvents = 0x00000020,
 };
 
 // A contact edge is used to connect bodies and contacts together
@@ -75,25 +78,26 @@ typedef struct b2Contact
 	bool isMarked;
 } b2Contact;
 
+// Shifted to be distinct from b2ContactFlags
 enum b2ContactSimFlags
 {
-	// Set when the shapes are touching.
-	b2_simTouchingFlag = 0x00000001,
+	// Set when the shapes are touching, including sensors
+	b2_simTouchingFlag = 0x00010000,
 
 	// This contact no longer has overlapping AABBs
-	b2_simDisjoint = 0x00000002,
+	b2_simDisjoint = 0x00020000,
 
 	// This contact started touching
-	b2_simStartedTouching = 0x00000004,
+	b2_simStartedTouching = 0x00040000,
 
 	// This contact stopped touching
-	b2_simStoppedTouching = 0x00000008,
+	b2_simStoppedTouching = 0x00080000,
 
 	// This contact has a hit event
-	b2_simEnableHitEvent = 0x00000010,
+	b2_simEnableHitEvent = 0x00100000,
 
 	// This contact wants pre-solve events
-	b2_simEnablePreSolveEvents = 0x00000020,
+	b2_simEnablePreSolveEvents = 0x00200000,
 };
 
 /// The class manages contact between two shapes. A contact exists for each overlapping

src/solver.c

@@ -827,6 +827,12 @@ static bool b2ContinuousQueryCallback(int proxyId, int shapeId, void* context)
 		return true;
 	}
 
+	// Skip sensors
+	if (shape->isSensor == true)
+	{
+		return true;
+	}
+
 	b2CheckIndex(world->bodyArray, shape->bodyId);
 	b2Body* body = world->bodyArray + shape->bodyId;
 	b2BodySim* bodySim = b2GetBodySim(world, body);
@@ -948,6 +954,8 @@ static void b2SolveContinuous(b2World* world, int bodySimIndex)
 		b2Shape* fastShape = shapes + shapeId;
 		B2_ASSERT(fastShape->isFast == true);
 
+		shapeId = fastShape->nextShapeId;
+
 		// Clear flag (keep set on body)
 		fastShape->isFast = false;
 
@@ -962,15 +970,19 @@ static void b2SolveContinuous(b2World* world, int bodySimIndex)
 		// Store this for later
 		fastShape->aabb = box2;
 
+		// No continuous collision for sensors
+		if (fastShape->isSensor)
+		{
+			continue;
+		}
+
 		b2DynamicTree_Query(staticTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context);
 
 		if (isBullet)
 		{
 			b2DynamicTree_Query(kinematicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context);
 			b2DynamicTree_Query(dynamicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context);
 		}
-
-		shapeId = fastShape->nextShapeId;
 	}
 
 	const float speculativeDistance = b2_speculativeDistance;

src/solver_set.c

@@ -294,7 +294,8 @@ void b2TrySleepIsland(b2World* world, int islandId)
 				B2_ASSERT(0 <= localIndex && localIndex < awakeSet->contacts.count);
 				b2ContactSim* contactSim = awakeSet->contacts.data + localIndex;
 
-				B2_ASSERT((contact->flags & b2_contactTouchingFlag) == 0 && contactSim->manifold.pointCount == 0);
+				B2_ASSERT(contactSim->manifold.pointCount == 0);
+				B2_ASSERT((contact->flags & b2_contactTouchingFlag) == 0 || (contact->flags & b2_contactSensorFlag) != 0);
 
 				// move the non-touching contact to the disabled set
 				contact->setIndex = b2_disabledSet;

src/world.c

@@ -562,6 +562,7 @@ static void b2Collide(b2StepContext* context)
 				B2_ASSERT(contact->islandId == B2_NULL_INDEX);
 				if ((flags & b2_contactSensorFlag) != 0)
 				{
+					// Contact is a sensor
 					if ((flags & b2_contactEnableSensorEvents) != 0)
 					{
 						if (shapeA->isSensor)
@@ -578,10 +579,11 @@ static void b2Collide(b2StepContext* context)
 					}
 
 					contactSim->simFlags &= ~b2_simStartedTouching;
-					contact->flags |= b2_contactTouchingFlag;
+					contact->flags |= b2_contactSensorTouchingFlag;
 				}
 				else
 				{
+					// Contact is solid
 					if (flags & b2_contactEnableContactEvents)
 					{
 						b2ContactBeginTouchEvent event = {shapeIdA, shapeIdB};
@@ -615,10 +617,12 @@ static void b2Collide(b2StepContext* context)
 			else if (simFlags & b2_simStoppedTouching)
 			{
 				contactSim->simFlags &= ~b2_simStoppedTouching;
-				contact->flags &= ~b2_contactTouchingFlag;
 
 				if ((flags & b2_contactSensorFlag) != 0)
 				{
+					// Contact is a sensor
+					contact->flags &= ~b2_contactSensorTouchingFlag;
+
 					if ((flags & b2_contactEnableSensorEvents) != 0)
 					{
 						if (shapeA->isSensor)
@@ -633,9 +637,13 @@ static void b2Collide(b2StepContext* context)
 							b2Array_Push(world->sensorEndEventArray, event);
 						}
 					}
+
 				}
 				else
 				{
+					// Contact is solid
+					contact->flags &= ~b2_contactTouchingFlag;
+
 					if (contact->flags & b2_contactEnableContactEvents)
 					{
 						b2ContactEndTouchEvent event = {shapeIdA, shapeIdB};
@@ -2891,13 +2899,18 @@ void b2ValidateContacts(b2World* world)
 		allocatedContactCount += 1;
 
 		bool touching = (contact->flags & b2_contactTouchingFlag) != 0;
+		bool sensorTouching = (contact->flags & b2_contactSensorTouchingFlag) != 0;
+		bool isSensor = (contact->flags & b2_contactSensorFlag) != 0;
+
+		B2_ASSERT(touching == false || sensorTouching == false);
+		B2_ASSERT(touching == false || isSensor == false);
 
 		int setId = contact->setIndex;
 
 		if (setId == b2_awakeSet)
 		{
 			// If touching and not a sensor
-			if (touching && (contact->flags & b2_contactSensorFlag) == 0)
+			if (touching && isSensor == false)
 			{
 				B2_ASSERT(0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount);
 			}
@@ -2909,11 +2922,11 @@ void b2ValidateContacts(b2World* world)
 		else if (setId >= b2_firstSleepingSet)
 		{
 			// Only touching contacts allowed in a sleeping set
-			B2_ASSERT(touching == true);
+			B2_ASSERT(touching == true && isSensor == false);
 		}
 		else
 		{
-			// Sleeping and non-touching contacts belong in the disabled set
+			// Sleeping and non-touching contacts or sensor contacts belong in the disabled set
 			B2_ASSERT(touching == false && setId == b2_disabledSet);
 		}
 
@@ -2922,10 +2935,10 @@ void b2ValidateContacts(b2World* world)
 		B2_ASSERT(contactSim->bodyIdA == contact->edges[0].bodyId);
 		B2_ASSERT(contactSim->bodyIdB == contact->edges[1].bodyId);
 
+		// Sim touching is true for solid and sensor contacts
 		bool simTouching = (contactSim->simFlags & b2_simTouchingFlag) != 0;
-		B2_ASSERT(touching == simTouching);
-		// A touching contact should have contact points unless it is a sensor
-		B2_ASSERT(simTouching == (contactSim->manifold.pointCount > 0) || (contact->flags & b2_contactSensorFlag) != 0);
+		B2_ASSERT(touching == simTouching || sensorTouching == simTouching);
+
 		B2_ASSERT(0 <= contactSim->manifold.pointCount && contactSim->manifold.pointCount <= 2);
 	}