(rayCastCallback B2TreeRayCastCallback, input B2RayCastInput)
| 99 | } |
| 100 | |
| 101 | func (tree B2DynamicTree) RayCast(rayCastCallback B2TreeRayCastCallback, input B2RayCastInput) { |
| 102 | |
| 103 | p1 := input.P1 |
| 104 | p2 := input.P2 |
| 105 | r := B2Vec2Sub(p2, p1) |
| 106 | B2Assert(r.LengthSquared() > 0.0) |
| 107 | r.Normalize() |
| 108 | |
| 109 | // v is perpendicular to the segment. |
| 110 | v := B2Vec2CrossScalarVector(1.0, r) |
| 111 | abs_v := B2Vec2Abs(v) |
| 112 | |
| 113 | // Separating axis for segment (Gino, p80). |
| 114 | // |dot(v, p1 - c)| > dot(|v|, h) |
| 115 | |
| 116 | maxFraction := input.MaxFraction |
| 117 | |
| 118 | // Build a bounding box for the segment. |
| 119 | segmentAABB := MakeB2AABB() |
| 120 | { |
| 121 | t := B2Vec2Add(p1, B2Vec2MulScalar(maxFraction, B2Vec2Sub(p2, p1))) |
| 122 | segmentAABB.LowerBound = B2Vec2Min(p1, t) |
| 123 | segmentAABB.UpperBound = B2Vec2Max(p1, t) |
| 124 | } |
| 125 | |
| 126 | stack := NewB2GrowableStack() |
| 127 | stack.Push(tree.M_root) |
| 128 | |
| 129 | for stack.GetCount() > 0 { |
| 130 | nodeId := stack.Pop().(int) |
| 131 | if nodeId == B2_nullNode { |
| 132 | continue |
| 133 | } |
| 134 | |
| 135 | node := &tree.M_nodes[nodeId] |
| 136 | |
| 137 | if B2TestOverlapBoundingBoxes(node.Aabb, segmentAABB) == false { |
| 138 | continue |
| 139 | } |
| 140 | |
| 141 | // Separating axis for segment (Gino, p80). |
| 142 | // |dot(v, p1 - c)| > dot(|v|, h) |
| 143 | c := node.Aabb.GetCenter() |
| 144 | h := node.Aabb.GetExtents() |
| 145 | |
| 146 | separation := math.Abs(B2Vec2Dot(v, B2Vec2Sub(p1, c))) - B2Vec2Dot(abs_v, h) |
| 147 | if separation > 0.0 { |
| 148 | continue |
| 149 | } |
| 150 | |
| 151 | if node.IsLeaf() { |
| 152 | subInput := MakeB2RayCastInput() |
| 153 | subInput.P1 = input.P1 |
| 154 | subInput.P2 = input.P2 |
| 155 | subInput.MaxFraction = maxFraction |
| 156 | |
| 157 | value := rayCastCallback(subInput, nodeId) |
| 158 |
nothing calls this directly
no test coverage detected