'##########################################################################
'##########################################################################
'##########################################################################
Public Type Point
x As Single
y As Single
z As Single
End Type
Public Const Epsilon = 0.000001
Public Function MakePoint(ByVal x As Single, ByVal y As Single, ByVal z As Single) As Point
With MakePoint
.x = x
.y = y
.z = z
End With
End Function
Private Function VectorNormalize(a As Point) As Point
Dim L As Single: L = DistanceEx(MakePoint(0, 0, 0), a)
If L = 0 Then
VectorNormalize = MakePoint(0, 0, 0)
Else
VectorNormalize = MakePoint(a.x / L, a.y / L, a.z / L)
End If
End Function
Public Function VectorDeduction(ByRef p1 As Point, ByRef p2 As Point) As Point
With VectorDeduction
.x = (p1.x - p2.x)
.y = (p1.y - p2.y)
.z = (p1.z - p2.z)
End With
End Function
Public Function VectorCrossProduct(ByRef p1 As Point, ByRef p2 As Point) As Point
With VectorCrossProduct
.x = ((p1.y * p2.z) - (p1.z * p2.y))
.y = ((p1.z * p2.x) - (p1.x * p2.z))
.z = ((p1.x * p2.y) - (p1.y * p2.x))
End With
End Function
Public Function VectorDotProduct(a As Point, B As Point) As Single
VectorDotProduct = a.x * B.x + a.y * B.y + a.z * B.z
End Function
Private Function TriangleNormal(ByRef p1 As Point, ByRef p2 As Point, ByRef p3 As Point) As Point
Dim v1 As Point, v2 As Point
v1 = VectorDeduction(p1, p2)
v2 = VectorDeduction(p1, p3)
TriangleNormal = VectorNormalize(VectorCrossProduct(v1, v2))
End Function
Public Function DistanceEx(ByRef p1 As Point, ByRef p2 As Point) As Single
DistanceEx = (((p1.x - p2.x) ^ 2) + ((p1.y - p2.y) ^ 2) + ((p1.z - p2.z) ^ 2))
If DistanceEx <> 0 Then DistanceEx = DistanceEx ^ (1 / 2)
End Function
Public Function AreParallel(t1p1 As Point, t1p2 As Point, t1p3 As Point, t2p1 As Point, t2p2 As Point, t2p3 As Point) As Boolean
Dim n1 As Point, n2 As Point, cross As Point
n1 = TriangleNormal(t1p1, t1p2, t1p3)
n2 = TriangleNormal(t2p1, t2p2, t2p3)
cross = VectorCrossProduct(n1, n2)
AreParallel = (Abs(cross.x) < Epsilon And Abs(cross.y) < Epsilon And Abs(cross.z) < Epsilon)
End Function
Public Function AreCoplanar(t1p1 As Point, t1p2 As Point, t1p3 As Point, t2p1 As Point, t2p2 As Point, t2p3 As Point) As Boolean
If Not AreParallel(t1p1, t1p2, t1p3, t2p1, t2p2, t2p3) Then
AreCoplanar = False
Exit Function
End If
Dim n1 As Point, d As Single
n1 = TriangleNormal(t1p1, t1p2, t1p3)
d = -(n1.x * t1p1.x + n1.y * t1p1.y + n1.z * t1p1.z)
AreCoplanar = Abs(n1.x * t2p1.x + n1.y * t2p1.y + n1.z * t2p1.z + d) < Epsilon
End Function
' ===== Point-in-triangle test (barycentric) =====
Private Function PointInTriangle(p As Point, V0 As Point, v1 As Point, v2 As Point) As Boolean
Dim u As Point, V As Point, w As Point
u = VectorDeduction(v1, V0)
V = VectorDeduction(v2, V0)
w = VectorDeduction(p, V0)
Dim uu As Single, vv As Single, uv As Single
Dim wu As Single, wv As Single, d As Single
uu = VectorDotProduct(u, u)
vv = VectorDotProduct(V, V)
uv = VectorDotProduct(u, V)
wu = VectorDotProduct(w, u)
wv = VectorDotProduct(w, V)
d = uv * uv - uu * vv
If Abs(d) < Epsilon Then
PointInTriangle = False
Exit Function
End If
Dim s As Single, t As Single
s = (uv * wv - vv * wu) / d
t = (uv * wu - uu * wv) / d
PointInTriangle = (s >= -Epsilon And t >= -Epsilon And (s + t) <= 1 + Epsilon)
End Function
' ===== Edge-plane intersection =====
Private Function EdgePlaneIntersect(p As Point, Q As Point, planePoint As Point, PlaneNormal As Point, x As Point) As Boolean
Dim dir As Point: dir = VectorDeduction(Q, p)
Dim denom As Single: denom = VectorDotProduct(PlaneNormal, dir)
If Abs(denom) < Epsilon Then
EdgePlaneIntersect = False
Exit Function
End If
Dim t As Single
t = VectorDotProduct(PlaneNormal, VectorDeduction(planePoint, p)) / denom
If t < -Epsilon Or t > 1 + Epsilon Then
EdgePlaneIntersect = False
Exit Function
End If
x = VectorAddition(p, MakePoint(dir.x * t, dir.y * t, dir.z * t))
EdgePlaneIntersect = True
End Function
Public Function TriTriSegmentEx(ByRef t1p1 As Point, ByRef t1p2 As Point, ByRef t1p3 As Point, ByRef t2p1 As Point, ByRef t2p2 As Point, ByRef t2p3 As Point, ByRef OutP0 As Point, ByRef OutP1 As Point) As Single
Dim ap As Boolean
Dim ac As Boolean
ap = AreParallel(t1p1, t1p2, t1p3, t2p1, t2p2, t2p3)
ac = AreCoplanar(t1p1, t1p2, t1p3, t2p1, t2p2, t2p3)
Dim l1 As Single
Dim l2 As Single
If ap And Not ac Then
TriTriSegmentEx = 0 'parallel triangles but not on the same plane and/or overlapping
ElseIf ac Then
'potentially parallel, but on the same plane at any rate, return the overlapping difference from a edge view of the mboth
'because colliding triangles below are in the positive specture of a integers max value, this will be in the negative spec
l1 = (DistanceEx(t1p1, t1p2) + DistanceEx(t1p2, t1p3) + DistanceEx(t1p3, t1p1))
l2 = (DistanceEx(t2p1, t2p2) + DistanceEx(t2p2, t2p3) + DistanceEx(t2p3, t2p1))
TriTriSegmentEx = (Least(l1, l2) / Large(l1, l2)) * -32768
Else
'the triangles are certianly colliding, and must be caught
'before two edges have penetrated the other, or vice versa
'and that before this function is called so by time now is
Dim nA As Point, nB As Point
nA = VectorCrossProduct(VectorDeduction(t1p2, t1p1), VectorDeduction(t1p3, t1p1))
nB = VectorCrossProduct(VectorDeduction(t2p2, t2p1), VectorDeduction(t2p3, t2p1))
Dim pts(0 To 5) As Point
Dim c As Integer: c = 0
Dim x As Point
' Intersect edges of A with plane of B
If EdgePlaneIntersect(t1p1, t1p2, t2p1, nB, x) Then If PointInTriangle(x, t2p1, t2p2, t2p3) Then pts(c) = x: c = c + 1
If EdgePlaneIntersect(t1p2, t1p3, t2p1, nB, x) Then If PointInTriangle(x, t2p1, t2p2, t2p3) Then pts(c) = x: c = c + 1
If EdgePlaneIntersect(t1p3, t1p1, t2p1, nB, x) Then If PointInTriangle(x, t2p1, t2p2, t2p3) Then pts(c) = x: c = c + 1
' Intersect edges of B with plane of A
If EdgePlaneIntersect(t2p1, t2p2, t1p1, nA, x) Then If PointInTriangle(x, t1p1, t1p2, t1p3) Then pts(c) = x: c = c + 1
If EdgePlaneIntersect(t2p2, t2p3, t1p1, nA, x) Then If PointInTriangle(x, t1p1, t1p2, t1p3) Then pts(c) = x: c = c + 1
If EdgePlaneIntersect(t2p3, t2p1, t1p1, nA, x) Then If PointInTriangle(x, t1p1, t1p2, t1p3) Then pts(c) = x: c = c + 1
If c < 2 Then
'this shouldn't happen by prequisit input args as being in collision determined by three 2D views using PointInPoly
TriTriSegmentEx = 0
Exit Function
End If
' Choose two extreme points along intersection line direction
Dim dir As Point: dir = VectorNormalize(VectorCrossProduct(nA, nB))
Dim minProj As Single, maxProj As Single
Dim minIdx As Integer, maxIdx As Integer
minProj = VectorDotProduct(dir, pts(0)): maxProj = minProj
minIdx = 0: maxIdx = 0
Dim i As Integer
For i = 1 To c - 1
Dim p As Single: p = VectorDotProduct(dir, pts(i))
If p < minProj Then minProj = p: minIdx = i
If p > maxProj Then maxProj = p: maxIdx = i
Next i
OutP0 = pts(minIdx)
OutP1 = pts(maxIdx)
TriTriSegmentEx = DistanceEx(OutP0, OutP1)
End If
End Function
'##########################################################################
'##########################################################################
'##########################################################################
