Sensor fixes (#165)

Sensor sleeping was broken and asserting. Fixing sensor flags and
testing sensors more.

Fixes #162

README.md

@@ -4,7 +4,7 @@
 [![Build Status](https://github.com/erincatto/box2c/actions/workflows/build.yml/badge.svg)](https://github.com/erincatto/box2c/actions)
 
 # Box2D v3.0 Notes
-This repository is alpha and ready for testing. It should build on recent versions of clang and gcc. However, you will need the latest Visual Studio version for C11 atomics to compile (17.8.3+). TODO: mingw
+This repository is beta and ready for testing. It should build on recent versions of clang and gcc. However, you will need the latest Visual Studio version for C11 atomics to compile (17.8.3+).
 
 AVX2 CPU support is assumed. You can turn this off in the CMake options and use SSE2 instead.
 

docs/migration.md

@@ -34,7 +34,7 @@ b2WorldDef worldDef = b2DefaultWorldDef();
 worldDef.gravity = gravity;
 b2WorldId worldId = b2CreateWorld(&worldDef);
 ```
-There is now a required world definition. C does not have constructors, so you need to initialize **ALL** structures that you pass to Box2D. Box2D provides and initialization helper for almost all structures. For example `b2DefaultWorldDef()` is used here to initialize `b2WorldDef`. `b2WorldDef` provides many options, but the defaults are good enough to get going.
+There is now a required world definition. C does not have constructors, so you need to initialize **ALL** structures that you pass to Box2D. Box2D provides an initialization helper for almost all structures. For example `b2DefaultWorldDef()` is used here to initialize `b2WorldDef`. `b2WorldDef` provides many options, but the defaults are good enough to get going.
 
 In Version 3.0, Box2D objects are generally hidden and you only have an identifier. This keeps the API small. So when you create a world you just get a `b2WorldId` which you should treat as an atomic object, like `int` or `float`. It is small and should be passed by value.
 
@@ -75,10 +75,8 @@ bodyId = b2_nullBodyId;
 ```
 Notice there is a little magic here in Version 3.0. `b2BodyId` knows what world it comes from. So you do not need to provide `worldId` when destroying the body. Version 3.0 supports up to 128 worlds. This may increased or be overridden in the future.
 
-There is a significant behavior change with body destruction in Version 3.0.
-> Destroying a body no longer destroys the attached joints, it is up to you to destroy them.
-
-Shapes are still destroyed automatically. However, `b2DestructionListener` is gone. This holds to the theme of fewer callbacks.
+Shapes and joints are still destroyed automatically. However, `b2DestructionListener` is gone. This holds to the theme of fewer callbacks. However, you can now use 
+`b2Shape_IsValid()` and `b2Joint_IsValid()`.
 
 ### Creating a shape
 Shape creation has been streamlined in Version 3.0. `b2Fixture` is gone. I feel like it was a confusing concept so I hope you don't miss it.

include/box2d/id.h

@@ -3,7 +3,7 @@
 
 #pragma once
 
-#include "box2d/base.h"
+#include "base.h"
 
 #include <stdint.h>
 

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.c

@@ -482,7 +482,7 @@ static bool b2TestShapeOverlap(const b2Shape* shapeA, b2Transform xfA, const b2S
 	return output.distance < 10.0f * FLT_EPSILON;
 }
 
-// Update the contact manifold and touching status.
+// Update the contact manifold and touching status. Also updates sensor overlap.
 // Note: do not assume the shape AABBs are overlapping or are valid.
 bool b2UpdateContact(b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA,
 					 b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB)

src/contact.h

@@ -13,20 +13,24 @@ typedef struct b2World b2World;
 
 enum b2ContactFlags
 {
-	// Set when the shapes are touching.
-	b2_contactTouchingFlag = 0x00000002,
+	// Set when the solid 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
+	// todo this is not used, perhaps have b2Body_GetSensorContactData()
+	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 +79,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

@@ -343,12 +343,11 @@ static void b2CollideTask(int startIndex, int endIndex, uint32_t threadIndex, vo
 			b2Vec2 centerOffsetA = b2RotateVector(transformA.q, bodySimA->localCenter);
 			b2Vec2 centerOffsetB = b2RotateVector(transformB.q, bodySimB->localCenter);
 
+			// This updates solid contacts and sensors
 			bool touching =
 				b2UpdateContact(world, contactSim, shapeA, transformA, centerOffsetA, shapeB, transformB, centerOffsetB);
 
-			// Move
-
-			// State changes that affect island connectivity
+			// State changes that affect island connectivity. Also contact and sensor events.
 			if (touching == true && wasTouching == false)
 			{
 				contactSim->simFlags |= b2_simStartedTouching;
@@ -562,6 +561,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 +578,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 +616,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 +636,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 +2898,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 +2921,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 +2934,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);
 	}