-
Notifications
You must be signed in to change notification settings - Fork 48
/
Copy pathCollisionB2ShapeEdge.go
145 lines (118 loc) · 3.68 KB
/
CollisionB2ShapeEdge.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
package box2d
/// A line segment (edge) shape. These can be connected in chains or loops
/// to other edge shapes. The connectivity information is used to ensure
/// correct contact normals.
type B2EdgeShape struct {
B2Shape
/// These are the edge vertices
M_vertex1, M_vertex2 B2Vec2
/// Optional adjacent vertices. These are used for smooth collision.
M_vertex0, M_vertex3 B2Vec2
M_hasVertex0, M_hasVertex3 bool
}
func MakeB2EdgeShape() B2EdgeShape {
return B2EdgeShape{
B2Shape: B2Shape{
M_type: B2Shape_Type.E_edge,
M_radius: B2_polygonRadius,
},
M_vertex0: MakeB2Vec2(0, 0),
M_vertex3: MakeB2Vec2(0, 0),
M_hasVertex0: false,
M_hasVertex3: false,
}
}
func NewB2EdgeShape() *B2EdgeShape {
res := MakeB2EdgeShape()
return &res
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
// B2EdgeShape.cpp
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
func (edge *B2EdgeShape) Set(v1 B2Vec2, v2 B2Vec2) {
edge.M_vertex1 = v1
edge.M_vertex2 = v2
edge.M_hasVertex0 = false
edge.M_hasVertex3 = false
}
func (edge B2EdgeShape) Clone() B2ShapeInterface {
clone := NewB2EdgeShape()
clone.M_vertex0 = edge.M_vertex0
clone.M_vertex1 = edge.M_vertex1
clone.M_vertex2 = edge.M_vertex2
clone.M_vertex3 = edge.M_vertex3
clone.M_hasVertex0 = edge.M_hasVertex0
clone.M_hasVertex3 = edge.M_hasVertex3
return clone
}
func (edge *B2EdgeShape) Destroy() {}
func (edge B2EdgeShape) GetChildCount() int {
return 1
}
func (edge B2EdgeShape) TestPoint(xf B2Transform, p B2Vec2) bool {
return false
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
func (edge B2EdgeShape) RayCast(output *B2RayCastOutput, input B2RayCastInput, xf B2Transform, childIndex int) bool {
// Put the ray into the edge's frame of reference.
p1 := B2RotVec2MulT(xf.Q, B2Vec2Sub(input.P1, xf.P))
p2 := B2RotVec2MulT(xf.Q, B2Vec2Sub(input.P2, xf.P))
d := B2Vec2Sub(p2, p1)
v1 := edge.M_vertex1
v2 := edge.M_vertex2
e := B2Vec2Sub(v2, v1)
normal := MakeB2Vec2(e.Y, -e.X)
normal.Normalize()
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
numerator := B2Vec2Dot(normal, B2Vec2Sub(v1, p1))
denominator := B2Vec2Dot(normal, d)
if denominator == 0.0 {
return false
}
t := numerator / denominator
if t < 0.0 || input.MaxFraction < t {
return false
}
q := B2Vec2Add(p1, B2Vec2MulScalar(t, d))
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
r := B2Vec2Sub(v2, v1)
rr := B2Vec2Dot(r, r)
if rr == 0.0 {
return false
}
s := B2Vec2Dot(B2Vec2Sub(q, v1), r) / rr
if s < 0.0 || 1.0 < s {
return false
}
output.Fraction = t
if numerator > 0.0 {
output.Normal = B2RotVec2Mul(xf.Q, normal).OperatorNegate()
} else {
output.Normal = B2RotVec2Mul(xf.Q, normal)
}
return true
}
func (edge B2EdgeShape) ComputeAABB(aabb *B2AABB, xf B2Transform, childIndex int) {
v1 := B2TransformVec2Mul(xf, edge.M_vertex1)
v2 := B2TransformVec2Mul(xf, edge.M_vertex2)
lower := B2Vec2Min(v1, v2)
upper := B2Vec2Max(v1, v2)
r := MakeB2Vec2(edge.M_radius, edge.M_radius)
aabb.LowerBound = B2Vec2Sub(lower, r)
aabb.UpperBound = B2Vec2Sub(upper, r)
}
func (edge B2EdgeShape) ComputeMass(massData *B2MassData, density float64) {
massData.Mass = 0.0
massData.Center = B2Vec2MulScalar(0.5, B2Vec2Add(edge.M_vertex1, edge.M_vertex2))
massData.I = 0.0
}