func circle2segmentFunc(contacts []*Contact, circle *CircleShape, segment *SegmentShape) int {
rsum := circle.Radius + segment.Radius
//Calculate normal distance from segment
dn := vect.Dot(segment.Tn, circle.Tc) - vect.Dot(segment.Ta, segment.Tn)
dist := vect.FAbs(dn) - rsum
if dist > 0.0 {
return 0
}
//Calculate tangential distance along segment
dt := -vect.Cross(segment.Tn, circle.Tc)
dtMin := -vect.Cross(segment.Tn, segment.Ta)
dtMax := -vect.Cross(segment.Tn, segment.Tb)
// Decision tree to decide which feature of the segment to collide with.
if dt < dtMin {
if dt < (dtMin - rsum) {
return 0
} else {
return segmentEncapQuery(circle.Tc, segment.Ta, circle.Radius, segment.Radius, contacts[0], segment.A_tangent)
}
} else {
if dt < dtMax {
n := segment.Tn
if dn >= 0.0 {
n.Mult(-1)
}
con := &contacts[0]
pos := vect.Add(circle.Tc, vect.Mult(n, circle.Radius+dist*0.5))
(*con).reset(pos, n, dist, 0)
return 1
} else {
if dt < (dtMax + rsum) {
return segmentEncapQuery(circle.Tc, segment.Tb, circle.Radius, segment.Radius, contacts[0], segment.B_tangent)
} else {
return 0
}
}
}
panic("Never reached")
}
func Proximity(a, b AABB) vect.Float {
return vect.FAbs(a.Lower.X+a.Upper.X-b.Lower.X-b.Upper.X) + vect.FAbs(a.Lower.Y+a.Upper.Y-b.Lower.Y-b.Upper.Y)
}
func (S1 *Segment) Intersects(S2 *Segment) bool {
u := vect.Sub(S1.Point2, S1.Point1)
v := vect.Sub(S2.Point2, S2.Point1)
w := vect.Sub(S1.Point1, S2.Point1)
D := vect.Cross(u, v)
// test if they are parallel (includes either being a point)
if vect.FAbs(D) < 0.0000001 { // S1 and S2 are parallel
if vect.Cross(u, w) != 0 || vect.Cross(v, w) != 0 {
return false // they are NOT collinear
}
// they are collinear or degenerate
// check if they are degenerate points
du := vect.Dot(u, u)
dv := vect.Dot(v, v)
if du == 0 && dv == 0 { // both segments are points
if !vect.Equals(S1.Point1, S2.Point1) { // they are distinct points
return false
}
return true
}
if du == 0 { // S1 is a single point
if !S2.Contains(S1.Point1) { // but is not in S2
return false
}
return true
}
if dv == 0 { // S2 a single point
if !S1.Contains(S2.Point1) { // but is not in S1
return false
}
return true
}
// they are collinear segments - get overlap (or not)
var t0, t1 vect.Float // endpoints of S1 in eqn for S2
w2 := vect.Sub(S1.Point2, S2.Point1)
if v.X != 0 {
t0 = w.X / v.X
t1 = w2.X / v.X
} else {
t0 = w.Y / v.Y
t1 = w2.Y / v.Y
}
if t0 > t1 { // must have t0 smaller than t1
t0, t1 = t1, t0 // swap if not
}
if t0 > 1 || t1 < 0 {
return false // NO overlap
}
return true
}
// the segments are skew and may intersect in a point
// get the intersect parameter for S1
sI := vect.Cross(v, w) / D
if sI < 0 || sI > 1 { // no intersect with S1
return false
}
// get the intersect parameter for S2
tI := vect.Cross(u, w) / D
if tI < 0 || tI > 1 { // no intersect with S2
return false
}
return true
}