fix #23 implement shape query

collision.go

@@ -188,9 +188,9 @@ func SegmentToPoly(info *CollisionInfo) {
 
 	// If the closest points are nearer than the sum of the radii...
 	if points.d-segment.r-polyshape.r <= 0 && (
-	// Reject endcap collisions if tangents are provided.
-	(!points.a.Equal(segment.ta) || n.Dot(segment.a_tangent.Rotate(rot)) <= 0) &&
-		(!points.a.Equal(segment.tb) || n.Dot(segment.b_tangent.Rotate(rot)) <= 0)) {
+		// Reject endcap collisions if tangents are provided.
+		(!points.a.Equal(segment.ta) || n.Dot(segment.a_tangent.Rotate(rot)) <= 0) &&
+			(!points.a.Equal(segment.tb) || n.Dot(segment.b_tangent.Rotate(rot)) <= 0)) {
 		ContactPoints(SupportEdgeForSegment(segment, n), SupportEdgeForPoly(polyshape, n.Neg()), points, info)
 	}
 }
@@ -497,6 +497,8 @@ var BuiltinCollisionFuncs = [9]CollisionFunc{
 	PolyToPoly,
 }
 
+// Collide performs a collision between two shapes
+// Deprecated - use Collide below
 func (info *CollisionInfo) Collide(a, b *Shape) {
 	// Make sure the shape types are in order.
 	if a.Order() > b.Order() {
@@ -509,3 +511,15 @@ func (info *CollisionInfo) Collide(a, b *Shape) {
 
 	BuiltinCollisionFuncs[info.a.Order()+info.b.Order()*SHAPE_TYPE_NUM](info)
 }
+
+// Collide performs a collision between two shapes
+func Collide(a, b *Shape, collisionID uint32, contacts []Contact) CollisionInfo {
+	info := CollisionInfo{
+		a:           a,
+		b:           b,
+		collisionId: collisionID,
+		arr:         contacts,
+	}
+	info.Collide(a, b)
+	return info
+}

shape.go

@@ -237,3 +237,35 @@ func NewShape(class ShapeClass, body *Body, massInfo *ShapeMassInfo) *Shape {
 		},
 	}
 }
+
+func ShapesCollide(a, b *Shape) ContactPointSet {
+	contacts := make([]Contact, MAX_CONTACTS_PER_ARBITER)
+	info := Collide(a, b, 0, contacts)
+
+	var set ContactPointSet
+	set.Count = info.count
+
+	// Collide may have swapped the contact order, flip the normal.
+	swapped := a != info.a
+	if swapped {
+		set.Normal = info.n.Neg()
+	} else {
+		set.Normal = info.n
+	}
+
+	for i := 0; i < info.count; i++ {
+		p1 := contacts[i].r1
+		p2 := contacts[i].r2
+
+		if swapped {
+			set.Points[i].PointA = p2
+			set.Points[i].PointB = p1
+		} else {
+			set.Points[i].PointA = p1
+			set.Points[i].PointB = p2
+		}
+		set.Points[i].Distance = p2.Sub(p1).Dot(set.Normal)
+	}
+
+	return set
+}

space.go

@@ -427,7 +427,7 @@ func SpaceCollideShapesFunc(obj interface{}, b *Shape, collisionId uint32, vspac
 	// Narrow-phase collision detection.
 	info := CollisionInfo{
 		collisionId: collisionId,
-		arr: space.ContactBufferGetArray(),
+		arr:         space.ContactBufferGetArray(),
 	}
 	info.Collide(a, b)
 
@@ -452,14 +452,14 @@ func SpaceCollideShapesFunc(obj interface{}, b *Shape, collisionId uint32, vspac
 
 	// Ignore the arbiter if it has been flagged
 	if arb.state != CP_ARBITER_STATE_IGNORE &&
-	// Call PreSolve
+		// Call PreSolve
 		arb.handler.PreSolveFunc(arb, space, arb.handler.UserData) &&
-	// Check (again) in case the pre-solve() callback called cpArbiterIgnored().
+		// Check (again) in case the pre-solve() callback called cpArbiterIgnored().
 		arb.state != CP_ARBITER_STATE_IGNORE &&
-	// Process, but don't add collisions for sensors.
+		// Process, but don't add collisions for sensors.
 		!(a.sensor || b.sensor) &&
-	// Don't process collisions between two infinite mass bodies.
-	// This includes collisions between two kinematic bodies, or a kinematic body and a static body.
+		// Don't process collisions between two infinite mass bodies.
+		// This includes collisions between two kinematic bodies, or a kinematic body and a static body.
 		!(a.body.m == INFINITY && b.body.m == INFINITY) {
 		space.arbiters = append(space.arbiters, arb)
 	} else {
@@ -502,7 +502,7 @@ func (space *Space) ContactBufferGetArray() []Contact {
 	}
 
 	head := space.contactBuffersHead
-	return head.contacts[head.numContacts:head.numContacts+MAX_CONTACTS_PER_ARBITER]
+	return head.contacts[head.numContacts : head.numContacts+MAX_CONTACTS_PER_ARBITER]
 }
 
 func QueryReject(a, b *Shape) bool {
@@ -974,7 +974,7 @@ func NearestPointQueryNearest(obj interface{}, shape *Shape, collisionId uint32,
 type SpaceBBQueryFunc func(shape *Shape, data interface{})
 
 type BBQueryContext struct {
-	bb BB
+	bb     BB
 	filter ShapeFilter
 	f      SpaceBBQueryFunc
 }
@@ -1089,3 +1089,43 @@ func (space *Space) AddPostStepCallback(f PostStepCallbackFunc, key, data interf
 	}
 	return false
 }
+
+// ShapeQuery queries a space for any shapes overlapping the given shape and call the callback for each shape found.
+func (space *Space) ShapeQuery(shape *Shape, callback func(shape *Shape, points *ContactPointSet)) bool {
+	body := shape.body
+	var bb BB
+	if body != nil {
+		bb = shape.Update(body.transform)
+	} else {
+		bb = shape.bb
+	}
+
+	var anyCollision bool
+
+	var shapeQuery SpatialIndexQuery
+	shapeQuery = func(obj interface{}, b *Shape, collisionId uint32, _ interface{}) uint32 {
+		a := obj.(*Shape)
+		if a.Filter.Reject(b.Filter) || a == b {
+			return collisionId
+		}
+
+		contactPointSet := ShapesCollide(a, b)
+		if contactPointSet.Count > 0 {
+			if callback != nil {
+				callback(b, &contactPointSet)
+			}
+			anyCollision = !(a.sensor || b.sensor)
+		}
+
+		return collisionId
+	}
+
+	space.Lock()
+	{
+		space.dynamicShapes.class.Query(shape, bb, shapeQuery, nil)
+		space.staticShapes.class.Query(shape, bb, shapeQuery, nil)
+	}
+	space.Unlock(true)
+
+	return anyCollision
+}

space_test.go

@@ -0,0 +1,25 @@
+package cp
+
+import "testing"
+
+func TestSpace_ShapeQuery(t *testing.T) {
+	space := NewSpace()
+	circle := space.AddShape(NewCircle(space.StaticBody, 1, Vector{}))
+	space.ShapeQuery(circle, func(shape *Shape, points *ContactPointSet) {
+		t.Fatal("Shouldn't collide with itself")
+	})
+	box := NewBox(NewBody(1, 1), 1, 1, 1)
+
+	var called int
+	space.ShapeQuery(box, func(shape *Shape, points *ContactPointSet) {
+		called++
+	})
+	if called != 1 {
+		t.Error("Expected box to collide with circle")
+	}
+
+	box.body.SetPosition(Vector{3, 0})
+	space.ShapeQuery(box, func(shape *Shape, points *ContactPointSet) {
+		t.Error("Box should be just out of range")
+	})
+}