p = p1 + t * d v = v1 + s * e p1 + t * d = v1 + s * e s * e - t * d = p1 - v1
(output *B2RayCastOutput, input B2RayCastInput, xf B2Transform, childIndex int)
| 73 | // p1 + t * d = v1 + s * e |
| 74 | // s * e - t * d = p1 - v1 |
| 75 | func (edge B2EdgeShape) RayCast(output *B2RayCastOutput, input B2RayCastInput, xf B2Transform, childIndex int) bool { |
| 76 | |
| 77 | // Put the ray into the edge's frame of reference. |
| 78 | p1 := B2RotVec2MulT(xf.Q, B2Vec2Sub(input.P1, xf.P)) |
| 79 | p2 := B2RotVec2MulT(xf.Q, B2Vec2Sub(input.P2, xf.P)) |
| 80 | d := B2Vec2Sub(p2, p1) |
| 81 | |
| 82 | v1 := edge.M_vertex1 |
| 83 | v2 := edge.M_vertex2 |
| 84 | e := B2Vec2Sub(v2, v1) |
| 85 | normal := MakeB2Vec2(e.Y, -e.X) |
| 86 | normal.Normalize() |
| 87 | |
| 88 | // q = p1 + t * d |
| 89 | // dot(normal, q - v1) = 0 |
| 90 | // dot(normal, p1 - v1) + t * dot(normal, d) = 0 |
| 91 | numerator := B2Vec2Dot(normal, B2Vec2Sub(v1, p1)) |
| 92 | denominator := B2Vec2Dot(normal, d) |
| 93 | |
| 94 | if denominator == 0.0 { |
| 95 | return false |
| 96 | } |
| 97 | |
| 98 | t := numerator / denominator |
| 99 | if t < 0.0 || input.MaxFraction < t { |
| 100 | return false |
| 101 | } |
| 102 | |
| 103 | q := B2Vec2Add(p1, B2Vec2MulScalar(t, d)) |
| 104 | |
| 105 | // q = v1 + s * r |
| 106 | // s = dot(q - v1, r) / dot(r, r) |
| 107 | r := B2Vec2Sub(v2, v1) |
| 108 | rr := B2Vec2Dot(r, r) |
| 109 | if rr == 0.0 { |
| 110 | return false |
| 111 | } |
| 112 | |
| 113 | s := B2Vec2Dot(B2Vec2Sub(q, v1), r) / rr |
| 114 | if s < 0.0 || 1.0 < s { |
| 115 | return false |
| 116 | } |
| 117 | |
| 118 | output.Fraction = t |
| 119 | if numerator > 0.0 { |
| 120 | output.Normal = B2RotVec2Mul(xf.Q, normal).OperatorNegate() |
| 121 | } else { |
| 122 | output.Normal = B2RotVec2Mul(xf.Q, normal) |
| 123 | } |
| 124 | |
| 125 | return true |
| 126 | } |
| 127 | |
| 128 | func (edge B2EdgeShape) ComputeAABB(aabb *B2AABB, xf B2Transform, childIndex int) { |
| 129 |
nothing calls this directly
no test coverage detected