Sensor unit test (erincatto#870)

added sensor unit test
added null checks
added WASM link
updated sensor documentation
added b2Body_GetLocalPointVelocity and b2Body_GetWorldPointVelocity
added b2Body_GetMass
erincatto#848 erincatto#864 erincatto#870

Author: erincatto

README.md

@@ -84,5 +84,6 @@ Support development of Box2D through [Github Sponsors](https://github.com/sponso
 Please consider starring this repository and subscribing to my [YouTube channel](https://www.youtube.com/@erin_catto).
 
 ## Ports, wrappers, and bindings
-- https://github.com/EnokViking/Box2DBeef
-- https://github.com/HolyBlackCat/box2cpp
+- Beef bindings - https://github.com/EnokViking/Box2DBeef
+- C++ bindings - https://github.com/HolyBlackCat/box2cpp
+- WASM - https://github.com/Birch-san/box2d3-wasm

docs/simulation.md

@@ -877,18 +877,50 @@ is a shape that detects overlap but does not produce a response.
 
 You can flag any shape as being a sensor. Sensors may be static,
 kinematic, or dynamic. Remember that you may have multiple shapes per
-body and you can have any mix of sensors and solid shapes. Also,
-sensors only form contacts when at least one body is dynamic, so you
-will not get sensors overlap detection for kinematic versus kinematic,
-kinematic versus static, or static versus static. Finally sensors do not
+body and you can have any mix of sensors and solid shapes. Sensors do not
 detect other sensors.
 
+Sensors are processed at the end of the world step and generate begin and end
+events without delay. User operations may cause overlaps to begin or end. These
+are processed the next time step. Such operations include:
+- destroying a body or shape
+- changing a shape filter
+- disabling or enabling a body
+- setting a body transform
+
 Sensors do not detect objects that pass through the sensor shape within 
 one time step. If you have fast moving object and/or small sensors then you
 should use a ray or shape cast to detect these events.
 
-Sensor overlap detection is achieved using events, which are described
-below.
+You can access the current sensor overlaps. Be careful because some shape ids may
+be invalid due to a shape being destroyed. Use `b2Shape_IsValid` to ensure an
+overlapping shape is still valid.
+
+```cpp
+// First determine the required array capacity to hold all the overlapping shape ids.
+int capacity = b2Shape_GetSensorCapacity( sensorShapeId );
+std::vector<b2ShapeId> overlaps;
+overlaps.resize( capacity );
+
+// Now get all overlaps and record the actual count
+int count = b2Shape_GetSensorOverlaps( sensorShapeId, overlaps.data(), capacity );
+overlaps.resize( count );
+
+for ( int i = 0; i < count; ++i )
+{
+    b2ShapeId visitorId = overlaps[i];
+
+    // Ensure the visitorId is valid
+    if ( b2Shape_IsValid( visitorId ) == false )
+    {
+        continue;
+    }
+
+    // process overlap using game logic
+}
+```
+
+Sensor overlap can also be achieved using events, which are described below.
 
 ## Contacts
 Contacts are internal objects created by Box2D to manage collision between pairs of
@@ -1028,18 +1060,6 @@ for (int i = 0; i < sensorEvents.endCount; ++i)
 Sensor events should be processed after the world step and before other game logic. This should
 help you avoid processing stale data.
 
-There are several user operations that can cause sensors to stop touching. Such operations
-include:
-- destroying a body or shape
-- changing the filter on a shape
-- disabling a body
-- setting the body transform
-These may generate end-touch events and these events are included with simulation events available
-after the next call to `b2World_Step`. 
-
-Sensor events are only enabled for a non-sensor shape if `b2ShapeDef::enableSensorEvents`
-is true.
-
 ### Contact Events
 Contact events are available after each world step. Like sensor events these should be
 retrieved and processed before performing other game logic. Otherwise

include/box2d/box2d.h

@@ -288,6 +288,12 @@ B2_API void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity );
 /// Set the angular velocity of a body in radians per second
 B2_API void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity );
 
+/// Get the linear velocity of a local point attached to a body. Usually in meters per second.
+B2_API b2Vec2 b2Body_GetLocalPointVelocity( b2BodyId bodyId, b2Vec2 localPoint );
+
+/// Get the linear velocity of a world point attached to a body. Usually in meters per second.
+B2_API b2Vec2 b2Body_GetWorldPointVelocity( b2BodyId bodyId, b2Vec2 worldPoint );
+
 /// Apply a force at a world point. If the force is not applied at the center of mass,
 /// it will generate a torque and affect the angular velocity. This optionally wakes up the body.
 /// The force is ignored if the body is not awake.
@@ -649,6 +655,9 @@ B2_API int b2Shape_GetSensorOverlaps( b2ShapeId shapeId, b2ShapeId* overlaps, in
 /// Get the current world AABB
 B2_API b2AABB b2Shape_GetAABB( b2ShapeId shapeId );
 
+/// Get the mass data for a shape
+B2_API b2MassData b2Shape_GetMassData( b2ShapeId shapeId );
+
 /// Get the closest point on a shape to a target point. Target and result are in world space.
 /// todo need sample
 B2_API b2Vec2 b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Vec2 target );

samples/sample_bodies.cpp

@@ -553,6 +553,17 @@ class Weeble : public Sample
 		Sample::Step( settings );
 
 		g_draw.DrawCircle( m_explosionPosition, m_explosionRadius, b2_colorAzure );
+
+		// This shows how to get the velocity of a point on a body
+		b2Vec2 localPoint = { 0.0f, 2.0f };
+		b2Vec2 worldPoint = b2Body_GetWorldPoint( m_weebleId, localPoint );
+
+		b2Vec2 v1 = b2Body_GetLocalPointVelocity( m_weebleId, localPoint );
+		b2Vec2 v2 = b2Body_GetWorldPointVelocity( m_weebleId, worldPoint );
+
+		b2Vec2 offset = { 0.05f, 0.0f };
+		g_draw.DrawSegment( worldPoint, worldPoint + v1, b2_colorRed );
+		g_draw.DrawSegment( worldPoint + offset, worldPoint + v2 + offset, b2_colorGreen );
 	}
 
 	static Sample* Create( Settings& settings )

samples/sample_collision.cpp

@@ -2368,7 +2368,7 @@ class Manifold : public Sample
 			}
 			else
 			{
-				g_draw.DrawPoint( p1, 5.0f, b2_colorBlue );
+				g_draw.DrawPoint( p1, 10.0f, b2_colorBlue );
 			}
 
 			if ( m_showIds )
@@ -2551,6 +2551,24 @@ class Manifold : public Sample
 
 		offset = { -10.0f, 0.0f };
 
+		// square-square
+		{
+			b2Polygon box1 = b2MakeSquare( 0.5f );
+			b2Polygon box = b2MakeSquare( 0.5f );
+
+			b2Transform transform1 = { offset, b2Rot_identity };
+			b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q };
+
+			b2Manifold m = b2CollidePolygons( &box1, transform1, &box, transform2 );
+
+			g_draw.DrawSolidPolygon( transform1, box1.vertices, box1.count, box1.radius, color1 );
+			g_draw.DrawSolidPolygon( transform2, box.vertices, box.count, box.radius, color2 );
+
+			DrawManifold( &m, transform1.p, transform2.p );
+
+			offset = b2Add( offset, increment );
+		}
+
 		// box-box
 		{
 			b2Polygon box1 = b2MakeBox( 2.0f, 0.1f );

src/body.c

@@ -814,6 +814,42 @@ void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity )
 	state->angularVelocity = angularVelocity;
 }
 
+b2Vec2 b2Body_GetLocalPointVelocity(b2BodyId bodyId, b2Vec2 localPoint)
+{
+	b2World* world = b2GetWorld( bodyId.world0 );
+	b2Body* body = b2GetBodyFullId( world, bodyId );
+	b2BodyState* state = b2GetBodyState( world, body );
+	if ( state == NULL )
+	{
+		return b2Vec2_zero;
+	}
+
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex );
+	b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex );
+
+	b2Vec2 r = b2RotateVector( bodySim->transform.q, b2Sub(localPoint, bodySim->localCenter) );
+	b2Vec2 v = b2Add(state->linearVelocity, b2CrossSV( state->angularVelocity, r ));
+	return v;
+}
+
+b2Vec2 b2Body_GetWorldPointVelocity(b2BodyId bodyId, b2Vec2 worldPoint)
+{
+	b2World* world = b2GetWorld( bodyId.world0 );
+	b2Body* body = b2GetBodyFullId( world, bodyId );
+	b2BodyState* state = b2GetBodyState( world, body );
+	if ( state == NULL )
+	{
+		return b2Vec2_zero;
+	}
+
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex );
+	b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex );
+
+	b2Vec2 r = b2Sub( worldPoint, bodySim->center );
+	b2Vec2 v = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, r ) );
+	return v;
+}
+
 void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake )
 {
 	b2World* world = b2GetWorld( bodyId.world0 );

src/shape.c

@@ -316,6 +316,10 @@ static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body
 void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass )
 {
 	b2World* world = b2GetWorldLocked( shapeId.world0 );
+	if ( world == NULL )
+	{
+		return;
+	}
 
 	b2Shape* shape = b2GetShape( world, shapeId );
 
@@ -449,6 +453,10 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def )
 void b2DestroyChain( b2ChainId chainId )
 {
 	b2World* world = b2GetWorldLocked( chainId.world0 );
+	if ( world == NULL )
+	{
+		return;
+	}
 
 	b2ChainShape* chain = b2GetChainShape( world, chainId );
 	bool wakeBodies = true;
@@ -503,13 +511,23 @@ b2WorldId b2Chain_GetWorld( b2ChainId chainId )
 int b2Chain_GetSegmentCount( b2ChainId chainId )
 {
 	b2World* world = b2GetWorldLocked( chainId.world0 );
+	if ( world == NULL )
+	{
+		return 0;
+	}
+
 	b2ChainShape* chain = b2GetChainShape( world, chainId );
 	return chain->count;
 }
 
 int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity )
 {
 	b2World* world = b2GetWorldLocked( chainId.world0 );
+	if ( world == NULL )
+	{
+		return 0;
+	}
+
 	b2ChainShape* chain = b2GetChainShape( world, chainId );
 
 	int count = b2MinInt( chain->count, capacity );
@@ -1505,6 +1523,18 @@ b2AABB b2Shape_GetAABB( b2ShapeId shapeId )
 	return shape->aabb;
 }
 
+b2MassData b2Shape_GetMassData(b2ShapeId shapeId)
+{
+	b2World* world = b2GetWorld( shapeId.world0 );
+	if ( world == NULL )
+	{
+		return ( b2MassData ){ 0 };
+	}
+
+	b2Shape* shape = b2GetShape( world, shapeId );
+	return b2ComputeShapeMass( shape );
+}
+
 b2Vec2 b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Vec2 target )
 {
 	b2World* world = b2GetWorld( shapeId.world0 );

test/test_world.c

@@ -326,6 +326,72 @@ int TestWorldCoverage( void )
 	return 0;
 }
 
+static int TestSensor( void )
+{
+	b2WorldDef worldDef = b2DefaultWorldDef();
+	b2WorldId worldId = b2CreateWorld( &worldDef );
+
+	// Wall from x = 1 to x = 2
+	b2BodyDef bodyDef = b2DefaultBodyDef();
+	bodyDef.type = b2_staticBody;
+	bodyDef.position.x = 1.5f;
+	bodyDef.position.y = 11.0f;
+	b2BodyId wallId = b2CreateBody( worldId, &bodyDef );
+	b2Polygon box = b2MakeBox( 0.5f, 10.0f );
+	b2ShapeDef shapeDef = b2DefaultShapeDef();
+	b2CreatePolygonShape( wallId, &shapeDef, &box );
+
+	// Bullet fired towards the wall
+	bodyDef = b2DefaultBodyDef();
+	bodyDef.type = b2_dynamicBody;
+	bodyDef.isBullet = true;
+	bodyDef.gravityScale = 0.0f;
+	bodyDef.position = (b2Vec2){ 7.39814, 4.0 };
+	bodyDef.linearVelocity = (b2Vec2){ -20.0f, 0.0f };
+	b2BodyId bulletId = b2CreateBody( worldId, &bodyDef );
+	shapeDef = b2DefaultShapeDef();
+	shapeDef.isSensor = true;
+	b2Circle circle = { { 0.0f, 0.0f }, 0.1f };
+	b2CreateCircleShape( bulletId, &shapeDef, &circle );
+
+	int beginCount = 0;
+	int endCount = 0;
+
+	while ( true )
+	{
+		float timeStep = 1.0f / 60.0f;
+		int subStepCount = 4;
+		b2World_Step( worldId, timeStep, subStepCount );
+
+		b2Vec2 bulletPos = b2Body_GetPosition( bulletId );
+		//printf( "Bullet pos: %g %g\n", bulletPos.x, bulletPos.y );
+
+		b2SensorEvents events = b2World_GetSensorEvents( worldId );
+
+		if ( events.beginCount > 0 )
+		{
+			beginCount += 1;
+		}
+
+		if ( events.endCount > 0 )
+		{
+			endCount += 1;
+		}
+
+		if ( bulletPos.x < -1.0f )
+		{
+			break;
+		}
+	}
+
+	b2DestroyWorld( worldId );
+
+	ENSURE( beginCount == 1 );
+	ENSURE( endCount == 1 );
+
+	return 0;
+}
+
 int WorldTest( void )
 {
 	RUN_SUBTEST( HelloWorld );
@@ -334,6 +400,7 @@ int WorldTest( void )
 	RUN_SUBTEST( TestIsValid );
 	RUN_SUBTEST( TestWorldRecycle );
 	RUN_SUBTEST( TestWorldCoverage );
+	RUN_SUBTEST( TestSensor );
 
 	return 0;
 }