aego/modules/physics/world/queries.go

package world

type queryState struct {
	w            *World
	filter       QueryFilter
	origin       Vec2
	dir          Vec2
	maxDist      float32
	best         RayHit
	found        bool
	hits         []RayHit
	n            int
	shapes       []ShapeHandle
	point        Vec2
	probe        convex
	probeXf      xform
	pick         ShapeHandle
	picked       bool
	castC        convex
	castA        proxy
	castCentroid Vec2
	castDelta    Vec2
	castBest     CastHit
	castFound    bool
	penOut       []CastHit
	penN         int
	rayFn        func(int32, Vec2, Vec2, float32) float32
	allFn        func(int32, Vec2, Vec2, float32) float32
	pointFn      func(int32) bool
	shapeFn      func(int32) bool
	atFn         func(int32) bool
	castFn       func(int32) bool
	penFn        func(int32) bool
}

func (q *queryState) init(w *World) {
	q.w = w
	q.rayFn = q.rayClosest
	q.allFn = q.rayAll
	q.pointFn = q.pointHit
	q.shapeFn = q.shapeHit
	q.atFn = q.atHit
	q.castFn = q.castHit
	q.penFn = q.penHit
}

func (q *queryState) accept(s *shape) bool {
	if s.dead {
		return false
	}
	f := &q.filter
	if s.trigger && !f.IncludeTriggers {
		return false
	}
	if !f.ExcludeBody.IsZero() && s.body == f.ExcludeBody {
		return false
	}
	b := &q.w.bodies.items[s.body.Index]
	if f.SkipDynamic && b.typ == Dynamic {
		return false
	}
	return (f.LayerMask>>b.layer)&1 != 0
}

func (q *queryState) cast(s *shape, child int, maxT float32) (float32, Vec2, bool) {
	b := &q.w.bodies.items[s.body.Index]
	lp := b.xf.invApply(q.origin)
	ld := b.xf.q.invApply(q.dir)
	var t float32
	var n Vec2
	var ok bool
	if s.g.kind == KindChain {
		seg, _, _ := s.g.chain.segment(child)
		t, n, ok = raySegment(seg.v[0], seg.v[1], lp, ld, maxT, s.oneSided)
	} else {
		t, n, ok = rayConvex(&s.g.c, lp, ld, maxT)
	}
	if !ok {
		return 0, Vec2{}, false
	}
	return t, b.xf.q.apply(n), true
}

func (q *queryState) hitAt(t float32, n Vec2, sh ShapeHandle, bh BodyHandle) RayHit {
	return RayHit{
		Point:    q.origin.Add(q.dir.Scale(t)),
		Normal:   n,
		Distance: t,
		Fraction: t / q.maxDist,
		Shape:    sh,
		Body:     bh,
	}
}

func hitLess(a, b RayHit) bool {
	if a.Distance != b.Distance {
		return a.Distance < b.Distance
	}
	return a.Shape.Index < b.Shape.Index
}

func (q *queryState) rayClosest(id int32, _, _ Vec2, maxFraction float32) float32 {
	pi := &q.w.prox[id]
	s := &q.w.shapes.items[pi.shape.Index]
	if !q.accept(s) {
		return -1
	}
	t, n, ok := q.cast(s, int(pi.child), maxFraction*q.maxDist)
	if !ok {
		return -1
	}
	hit := q.hitAt(t, n, pi.shape, s.body)
	if q.found && !hitLess(hit, q.best) {
		return q.best.Fraction
	}
	q.best, q.found = hit, true
	return hit.Fraction
}

func (q *queryState) rayAll(id int32, _, _ Vec2, maxFraction float32) float32 {
	pi := &q.w.prox[id]
	s := &q.w.shapes.items[pi.shape.Index]
	if !q.accept(s) {
		return -1
	}
	t, n, ok := q.cast(s, int(pi.child), maxFraction*q.maxDist)
	if !ok {
		return -1
	}
	size := len(q.hits)
	hit := q.hitAt(t, n, pi.shape, s.body)
	if q.n == size {
		if !hitLess(hit, q.hits[size-1]) {
			return q.hits[size-1].Fraction
		}
		q.n--
	}
	i := q.n
	for i > 0 && hitLess(hit, q.hits[i-1]) {
		q.hits[i] = q.hits[i-1]
		i--
	}
	q.hits[i] = hit
	q.n++
	if q.n == size {
		return q.hits[size-1].Fraction
	}
	return 1
}

func (q *queryState) add(h ShapeHandle) {
	for _, x := range q.shapes[:q.n] {
		if x == h {
			return
		}
	}
	q.shapes[q.n] = h
	q.n++
}

func (q *queryState) pointHit(id int32) bool {
	pi := &q.w.prox[id]
	s := &q.w.shapes.items[pi.shape.Index]
	if s.g.kind == KindChain || !q.accept(s) {
		return true
	}
	b := &q.w.bodies.items[s.body.Index]
	if pointInConvex(&s.g.c, b.xf.invApply(q.point)) {
		q.add(pi.shape)
	}
	return q.n < len(q.shapes)
}

func (q *queryState) shapeHit(id int32) bool {
	pi := &q.w.prox[id]
	s := &q.w.shapes.items[pi.shape.Index]
	if !q.accept(s) {
		return true
	}
	b := &q.w.bodies.items[s.body.Index]
	var m Manifold
	g := s.g.child(int(pi.child))
	collideConvex(&g, b.xf, &q.probe, q.probeXf, 0, &m)
	if m.Count > 0 {
		q.add(pi.shape)
	}
	return q.n < len(q.shapes)
}

func (q *queryState) atHit(id int32) bool {
	pi := &q.w.prox[id]
	s := &q.w.shapes.items[pi.shape.Index]
	if s.g.kind == KindChain || !q.accept(s) {
		return true
	}
	if q.picked && pi.shape.Index >= q.pick.Index {
		return true
	}
	b := &q.w.bodies.items[s.body.Index]
	if pointInConvex(&s.g.c, b.xf.invApply(q.point)) {
		q.pick, q.picked = pi.shape, true
	}
	return true
}

func sortHandles(list []ShapeHandle) {
	for i := 1; i < len(list); i++ {
		for j := i; j > 0 && list[j].Index < list[j-1].Index; j-- {
			list[j], list[j-1] = list[j-1], list[j]
		}
	}
}

func (w *World) Raycast(origin, dir Vec2, maxDist float32, f QueryFilter) (RayHit, bool) {
	d, l := normalizeLen(dir)
	if l == 0 || !finite2(origin) || !finite2(dir) || !finite(maxDist) || maxDist <= 0 {
		return RayHit{}, false
	}
	q := &w.q
	q.filter, q.origin, q.dir, q.maxDist = f, origin, d, maxDist
	q.best, q.found = RayHit{}, false
	w.tree.raycast(origin, origin.Add(d.Scale(maxDist)), 1, q.rayFn)
	return q.best, q.found
}

func (w *World) RaycastAll(origin, dir Vec2, maxDist float32, f QueryFilter, out []RayHit) int {
	d, l := normalizeLen(dir)
	if len(out) == 0 || l == 0 || !finite2(origin) || !finite2(dir) || !finite(maxDist) || maxDist <= 0 {
		return 0
	}
	q := &w.q
	q.filter, q.origin, q.dir, q.maxDist = f, origin, d, maxDist
	q.hits, q.n = out, 0
	w.tree.raycast(origin, origin.Add(d.Scale(maxDist)), 1, q.allFn)
	n := q.n
	q.hits = nil
	return n
}

func (w *World) OverlapPoint(p Vec2, f QueryFilter, out []ShapeHandle) int {
	if len(out) == 0 || !finite2(p) {
		return 0
	}
	q := &w.q
	q.filter, q.point, q.shapes, q.n = f, p, out, 0
	w.tree.query(AABB{Min: p, Max: p}, q.pointFn)
	n := q.n
	q.shapes = nil
	sortHandles(out[:n])
	return n
}

func (w *World) overlapProbe(f QueryFilter, out []ShapeHandle) int {
	q := &w.q
	q.filter, q.shapes, q.n = f, out, 0
	w.tree.query(q.probe.aabb(q.probeXf), q.shapeFn)
	n := q.n
	q.shapes = nil
	sortHandles(out[:n])
	return n
}

func (w *World) OverlapCircle(c Vec2, r float32, f QueryFilter, out []ShapeHandle) int {
	if len(out) == 0 || !finite2(c) || !finite(r) || r <= 0 {
		return 0
	}
	q := &w.q
	q.probe = convex{count: 1, radius: r}
	q.probeXf = xform{p: c, q: Rot{C: 1}}
	return w.overlapProbe(f, out)
}

func (w *World) OverlapBox(c, half Vec2, angle float32, f QueryFilter, out []ShapeHandle) int {
	if len(out) == 0 || !finite2(c) || !finite2(half) || !finite(angle) || half.X <= 0 || half.Y <= 0 {
		return 0
	}
	q := &w.q
	q.probe = convex{count: 4}
	q.probe.v[0] = Vec2{X: -half.X, Y: -half.Y}
	q.probe.v[1] = Vec2{X: half.X, Y: -half.Y}
	q.probe.v[2] = Vec2{X: half.X, Y: half.Y}
	q.probe.v[3] = Vec2{X: -half.X, Y: half.Y}
	q.probe.computeNormals()
	q.probeXf = xform{p: c, q: makeRot(angle)}
	return w.overlapProbe(f, out)
}

func (w *World) ShapeAt(p Vec2, f QueryFilter) (ShapeHandle, bool) {
	if !finite2(p) {
		return ShapeHandle{}, false
	}
	q := &w.q
	q.filter, q.point, q.picked = f, p, false
	w.tree.query(AABB{Min: p, Max: p}, q.atFn)
	return q.pick, q.picked
}

func rayConvex(c *convex, p, d Vec2, maxT float32) (float32, Vec2, bool) {
	switch c.count {
	case 1:
		return rayCircle(c.v[0], c.radius, p, d, maxT)
	case 2:
		if c.radius == 0 {
			return raySegment(c.v[0], c.v[1], p, d, maxT, false)
		}
		return rayCapsule(c.v[0], c.v[1], c.radius, p, d, maxT)
	}
	return rayPolygon(c, p, d, maxT)
}

func rayCircle(center Vec2, r float32, p, d Vec2, maxT float32) (float32, Vec2, bool) {
	s := p.Sub(center)
	b := s.Dot(s) - r*r
	if b < 0 {
		return 0, Vec2{}, false
	}
	c := s.Dot(d)
	sigma := c*c - b
	if sigma < 0 || c > 0 {
		return 0, Vec2{}, false
	}
	t := -c - sqrtf(sigma)
	if t < 0 || t > maxT {
		return 0, Vec2{}, false
	}
	n, _ := normalizeLen(s.Add(d.Scale(t)))
	return t, n, true
}

func raySegment(v1, v2, p, d Vec2, maxT float32, oneSided bool) (float32, Vec2, bool) {
	e := v2.Sub(v1)
	en, l := normalizeLen(e)
	if l == 0 {
		return 0, Vec2{}, false
	}
	n := rightPerp(en)
	num := n.Dot(v1.Sub(p))
	den := n.Dot(d)
	if den == 0 || (oneSided && num > 0) {
		return 0, Vec2{}, false
	}
	t := num / den
	if t < 0 || t > maxT {
		return 0, Vec2{}, false
	}
	q := p.Add(d.Scale(t))
	s := q.Sub(v1).Dot(e) / e.Dot(e)
	if s < 0 || s > 1 {
		return 0, Vec2{}, false
	}
	if num > 0 {
		n = n.Neg()
	}
	return t, n, true
}

func distSqToSegment(p, a, b Vec2) float32 {
	e := b.Sub(a)
	t := float32(0)
	if l := e.LenSq(); l > 0 {
		t = clampf(p.Sub(a).Dot(e)/l, 0, 1)
	}
	return p.Sub(a.Add(e.Scale(t))).LenSq()
}

func rayCapsule(v0, v1 Vec2, r float32, p, d Vec2, maxT float32) (float32, Vec2, bool) {
	if distSqToSegment(p, v0, v1) < r*r {
		return 0, Vec2{}, false
	}
	e, _ := normalizeLen(v1.Sub(v0))
	nn := rightPerp(e).Scale(r)
	best := maxT
	var bn Vec2
	found := false
	try := func(t float32, n Vec2, hit bool) {
		if hit && t <= best {
			best, bn, found = t, n, true
		}
	}
	try(raySegment(v0.Add(nn), v1.Add(nn), p, d, best, true))
	try(raySegment(v1.Sub(nn), v0.Sub(nn), p, d, best, true))
	try(rayCircle(v0, r, p, d, best))
	try(rayCircle(v1, r, p, d, best))
	return best, bn, found
}

func rayPolygon(c *convex, p, d Vec2, maxT float32) (float32, Vec2, bool) {
	lower, upper := float32(0), maxT
	index := -1
	for i := 0; i < c.count; i++ {
		num := c.n[i].Dot(c.v[i].Sub(p))
		den := c.n[i].Dot(d)
		switch {
		case den == 0:
			if num < 0 {
				return 0, Vec2{}, false
			}
		case den < 0 && num < lower*den:
			lower = num / den
			index = i
		case den > 0 && num < upper*den:
			upper = num / den
		}
		if upper < lower {
			return 0, Vec2{}, false
		}
	}
	if index < 0 {
		return 0, Vec2{}, false
	}
	return lower, c.n[index], true
}

func pointInConvex(c *convex, p Vec2) bool {
	switch c.count {
	case 1:
		return p.Sub(c.v[0]).LenSq() <= c.radius*c.radius
	case 2:
		return distSqToSegment(p, c.v[0], c.v[1]) <= c.radius*c.radius
	}
	for i := 0; i < c.count; i++ {
		if c.n[i].Dot(p.Sub(c.v[i])) > c.radius {
			return false
		}
	}
	return true
}