'handles the point offsets
Public poff(15, 4) As Integer


Public Sub BoxOnBox(p1 As D3DVECTOR, r1 As D3DMATRIX, s1 As D3DVECTOR, p2 As D3DVECTOR, r2 As D3DMATRIX, s2 As D3DVECTOR)
    'deals with collision
    'note : collision codes
    '       1,2,3 = box 2 intersects with a face of box 1
    '       4,5,6 = box 1 intersects with a face of box 2
    '       7..15 = edge-edge contact
    
    Dim p As D3DVECTOR, pp As D3DVECTOR4, normalC As D3DVECTOR
    Dim a As D3DVECTOR, b As D3DVECTOR
    
    Dim r As D3DMATRIX
    Dim q As D3DMATRIX
    
    Dim s As Double, ss As Double, l As Double
    Dim i As Long, invert_normal As Long

    'get the vector from box 1 to box 2
    D3DXVec3Subtract p, p2, p1

    'get pp relative to the rotation of box 1
    D3DXVec3Transform pp, p, r1
    
    'get side lengths / 2
    D3DXVec3Scale a, s1, 0.5
    D3DXVec3Scale b, s2, 0.5

    'work out r = r1'*r2
    D3DXMatrixTranspose r, r1
    D3DXMatrixMultiply r, r, r2

    'now make q the absolute version (no negatives)
    q.m11 = Abs(r.m11)
    q.m12 = Abs(r.m12)
    q.m13 = Abs(r.m13)
    q.m21 = Abs(r.m21)
    q.m22 = Abs(r.m22)
    q.m23 = Abs(r.m23)
    q.m31 = Abs(r.m31)
    q.m32 = Abs(r.m32)
    q.m33 = Abs(r.m33)

    'check for 15 possible separating axis.
    'if we find one which is not separating, then work out how deep we
    'penetrate along this axis.
    'e.g. there is no collision if the axis keep the boxes seperate

    'first reset the collision flags
    g_s = -999999
    g_invert_normal = 0
    g_code = 0

    'separating axis = u1,u2,u3
    check_faces (pp.x), (a.x + b.x * q.m11 + b.y * q.m12 + b.z * q.m13), r1, 0, 1
    check_faces (pp.y), (a.y + b.x * q.m21 + b.y * q.m22 + b.z * q.m23), r1, 1, 2
    check_faces (pp.z), (a.z + b.x * q.m31 + b.y * q.m32 + b.z * q.m33), r1, 2, 3

    'separating axis = v1,v2,v3
    check_faces mvdot(r2, 0, p), (a.x * q.m11 + a.y * q.m21 + a.z * q.m31 + b.x), r2, 0, 4
    check_faces mvdot(r2, 1, p), (a.x * q.m12 + a.y * q.m22 + a.z * q.m32 + b.y), r2, 1, 5
    check_faces mvdot(r2, 2, p), (a.x * q.m13 + a.y * q.m23 + a.z * q.m33 + b.z), r2, 2, 6
    
    'separating axis = u1 x (v1,v2,v3)
    check_edges pp.z * r.m21 - pp.y * r.m31, (a.y * q.m31 + a.z * q.m21 + b.y * q.m13 + b.z * q.m12), 0, -r.m31, (r.m21), 7
    check_edges pp.z * r.m22 - pp.y * r.m32, (a.y * q.m32 + a.z * q.m22 + b.x * q.m13 + b.z * q.m11), 0, -r.m32, (r.m22), 8
    check_edges pp.z * r.m23 - pp.y * r.m33, (a.y * q.m33 + a.z * q.m23 + b.x * q.m12 + b.y * q.m11), 0, -r.m33, (r.m23), 9

    'separating axis = u2 x (v1,v2,v3)
    check_edges pp.x * r.m31 - pp.z * r.m11, (a.x * q.m31 + a.z * q.m11 + b.y * q.m23 + b.z * q.m22), (r.m31), 0, -r.m11, 10
    check_edges pp.x * r.m32 - pp.z * r.m12, (a.x * q.m32 + a.z * q.m12 + b.x * q.m23 + b.z * q.m21), (r.m32), 0, -r.m12, 11
    check_edges pp.x * r.m33 - pp.z * r.m13, (a.x * q.m33 + a.z * q.m13 + b.x * q.m22 + b.y * q.m21), (r.m33), 0, -r.m13, 12

    'separating axis = u3 x (v1,v2,v3)
    check_edges pp.y * r.m11 - pp.x * r.m21, (a.x * q.m21 + a.y * q.m11 + b.y * q.m33 + b.z * q.m32), -r.m21, (r.m11), 0, 13
    check_edges pp.y * r.m12 - pp.x * r.m22, (a.x * q.m22 + a.y * q.m12 + b.x * q.m33 + b.z * q.m31), -r.m22, (r.m12), 0, 14
    check_edges pp.y * r.m13 - pp.x * r.m23, (a.x * q.m23 + a.y * q.m13 + b.x * q.m32 + b.y * q.m31), -r.m23, (r.m13), 0, 15

    'if no collisions found, then dump out now...
    If g_code = 0 Then Exit Sub

    Dim n As D3DVECTOR4
    'boxes interpenetrate. compute the normal in global coordinates.
    If g_code > 6 Then
        'its an edge normal
        'so transform relative to r1
        D3DXVec3Transform n, g_normal, r1
    Else
        n.x = g_normal.x
        n.y = g_normal.y
        n.z = g_normal.z
    End If
    
    If g_invert_normal Then
        n.x = -n.x
        n.y = -n.y
        n.z = -n.z
    End If
    
    g_normal.x = n.x
    g_normal.y = n.y
    g_normal.z = n.z
    

End Sub
Private Function mvdot(min As D3DMATRIX, c As Integer, vin As D3DVECTOR) As Double
    'the dot product of a matrix and a vector....
    Dim o As D3DVECTOR
    If c = 0 Then
        o.x = min.m11
        o.y = min.m21
        o.z = min.m31
    ElseIf c = 1 Then
        o.x = min.m12
        o.y = min.m22
        o.z = min.m32
    ElseIf c = 2 Then
        o.x = min.m13
        o.y = min.m23
        o.z = min.m33
    End If
    mvdot = D3DXVec3Dot(o, vin)
End Function


Public Sub check_edges(e1 As Double, e2 As Double, n1 As Double, n2 As Double, n3 As Double, cc As Long)
    'this function checks the edges for axis of separation
    'it updates the globals normal, code and invert as req
    Dim s2 As Double
    Dim l As Double
    
    s2 = Abs(e1) - e2
    If s2 > 0 Then Exit Sub
    l = Sqr(n1 ^ 2 + n2 ^ 2 + n3 ^ 2)
    If l > 0 Then
        s2 = s2 / l
        If s2 > g_s Then
            g_s = s2
            g_normal.x = n1 / l
            g_normal.y = n2 / l
            g_normal.z = n3 / l
            g_invert_normal = e1 < 0
            g_code = cc
        End If
    End If
End Sub
Public Sub check_faces(e1 As Double, e2 As Double, mat As D3DMATRIX, col As Integer, cc As Long)
    'this function checks the faces for axis of separation
    'it updates the globals normal, code and invert as req
    Dim s2 As Double
    
    s2 = Abs(e1) - e2
    If s2 > 0 Then Exit Sub
    If s2 > g_s Then
        g_s = s2
        If col = 0 Then
            g_normal.x = mat.m11
            g_normal.y = mat.m21
            g_normal.z = mat.m31
        ElseIf col = 1 Then
            g_normal.x = mat.m12
            g_normal.y = mat.m22
            g_normal.z = mat.m32
        ElseIf col = 2 Then
            g_normal.x = mat.m13
            g_normal.y = mat.m23
            g_normal.z = mat.m33
        End If
        g_invert_normal = e1 < 0
        g_code = cc
    End If
End Sub

Public Sub lineClosestApproach(pa As D3DVECTOR, ua As D3DVECTOR, pb As D3DVECTOR, ub As D3DVECTOR, alpha As Double, beta As Double)
    Dim p As D3DVECTOR
    Dim uaub As Double, q1 As Double, q2 As Double, d As Double
    ' given two lines
    '    qa = pa + alpha* ua
    '    qb = pb + beta * ub
    ' where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
    ' beta go from [-inf,inf], return alpha and beta such that qa and qb are
    ' as close as possible

    D3DXVec3Subtract p, pb, pa
    uaub = D3DXVec3Dot(ua, ub)
    q1 = D3DXVec3Dot(ua, p)
    q2 = -D3DXVec3Dot(ub, p)
    d = 1 - uaub ^ 2
    If d <= 0 Then
        alpha = 0
        beta = 0
    Else
        d = 1 / d
        alpha = (q1 + uaub * q2) * d
        beta = (uaub * q1 + q2) * d
    End If

End Sub

Public Sub config_poff()
    
    'first the six faces
    poff(1, 0) = 4 ' 4 points
    poff(1, 1) = 4
    poff(1, 2) = 5
    poff(1, 3) = 6
    poff(1, 4) = 7
    
    poff(2, 0) = 4 ' 4 points
    poff(2, 1) = 0
    poff(2, 2) = 1
    poff(2, 3) = 4
    poff(2, 4) = 5
    
    poff(3, 0) = 4 ' 4 points
    poff(3, 1) = 0
    poff(3, 2) = 1
    poff(3, 3) = 2
    poff(3, 4) = 3
    
    poff(4, 0) = 4 ' 4 points
    poff(4, 1) = 3
    poff(4, 2) = 2
    poff(4, 3) = 6
    poff(4, 4) = 7
    
    poff(5, 0) = 4 ' 4 points
    poff(5, 1) = 0
    poff(5, 2) = 4
    poff(5, 3) = 7
    poff(5, 4) = 3
    
    poff(6, 0) = 4 ' 4 points
    poff(6, 1) = 1
    poff(6, 2) = 2
    poff(6, 3) = 5
    poff(6, 4) = 6
    
    'now the 9 edges
    
    poff(7, 0) = 2 ' 2 points
    poff(7, 1) = 2
    poff(7, 2) = 6
    
    poff(8, 0) = 2 ' 2 points
    poff(8, 1) = 1
    poff(8, 2) = 2
        
    poff(9, 0) = 2 ' 2 points
    poff(9, 1) = 4
    poff(9, 2) = 7
    
        
    poff(10, 0) = 2 ' 2 points
    poff(10, 1) = 2
    poff(10, 2) = 3
    
    poff(11, 0) = 2 ' 2 points
    poff(11, 1) = 0
    poff(11, 2) = 1
        
    poff(12, 0) = 2 ' 2 points
    poff(12, 1) = 6
    poff(12, 2) = 7
    
    
    poff(13, 0) = 2 ' 2 points
    poff(13, 1) = 5
    poff(13, 2) = 4
    
    poff(14, 0) = 2 ' 2 points
    poff(14, 1) = 3
    poff(14, 2) = 7
        
    poff(15, 0) = 2 ' 2 points
    poff(15, 1) = 0
    poff(15, 2) = 3
    
End Sub