CollisionDetection3D.bas

 Attribute VB_Name = "CollisionDetection"
'//-------------------------------------------------------------------------
'// Colision Detection in 3D
'// by Stephanie
'// version not optimized
'//-------------------------------------------------------------------------
Option Explicit
Const Infini = 4294967296#
Const CONST_MAXCOLLISIONCOUNT = 10
Public Function CollDetec_obj(point As D3DVECTOR, move As D3DVECTOR, obj As obj, resvector As D3DVECTOR) As Long
Dim t As Long
Dim CollisionCount As Long
Dim SurfOK As Long
Dim m As D3DMATRIX, tmpm As D3DMATRIX
SurfOK = -1
CollisionCount = 0
resvector.x = 0
resvector.y = 0
resvector.z = 0
With obj
    For t = 0 To .IC - 1 Step 3
        If t = SurfOK Then GoTo L_NextT
        If Test(point, move, .v(), .I(), .IC, t, resvector) = True Then
            CollDetec_obj = True
            SurfOK = t
            t = -3
            move = resvector
            CollisionCount = CollisionCount + 1
            If CollisionCount >= CONST_MAXCOLLISIONCOUNT Then
                resvector = MakeVector(0, 0, 0)
                Exit For
            End If
        End If
L_NextT:
    Next t
End With
End Function

Private Function Test(point As D3DVECTOR, move As D3DVECTOR, v() As D3DVERTEX, I() As Integer, IC As Long, IStart As Long, resvector As D3DVECTOR) As Boolean
Dim ang(2) As Single, t As Long
Dim Point2 As D3DVECTOR, move2 As D3DVECTOR
Dim FZRapXA As Single
Dim FZRapXC As Single
Dim FZRapYA As Single
Dim FZRapYC As Single
Dim Fvertex(2) As D3DVECTOR
Dim Fvertexdist As Single
Dim tridist As Single
Dim x As Single, y As Single
Dim b As Single
Dim tmpcal As Single
Dim m As D3DMATRIX
Fvertexdist = 0
For t = 0 To 2
    Fvertex(t).x = v(I(t + IStart)).x
    Fvertex(t).y = v(I(t + IStart)).y
    Fvertex(t).z = v(I(t + IStart)).z
    'Fvertexdist = Fvertexdist + Sqr(Sqr((Fvertex(t).x - point.x) ^ 2 + (Fvertex(t).y - point.y) ^ 2) ^ 2 + (Fvertex(t).z - point.z) ^ 2)
Next t
'tridist = Sqr(Sqr((Fvertex(0).x - Fvertex(1).x) ^ 2 + (Fvertex(0).y - Fvertex(1).y) ^ 2) ^ 2 + (Fvertex(0).z - Fvertex(1).z) ^ 2)
'tridist = tridist + Sqr(Sqr((Fvertex(1).x - Fvertex(2).x) ^ 2 + (Fvertex(1).y - Fvertex(2).y) ^ 2) ^ 2 + (Fvertex(1).z - Fvertex(2).z) ^ 2)
'tridist = tridist + Sqr(Sqr((Fvertex(2).x - Fvertex(0).x) ^ 2 + (Fvertex(2).y - Fvertex(0).y) ^ 2) ^ 2 + (Fvertex(2).z - Fvertex(0).z) ^ 2)
'If Fvertexdist + 20 > tridist Then Exit Function
Point2 = point
G_dx.VectorAdd move2, move, Point2
G_dx.IdentityMatrix m
m.rc41 = -Fvertex(0).x
m.rc42 = -Fvertex(0).y
m.rc43 = -Fvertex(0).z
Vector_TransMatrix Fvertex(0), m
Vector_TransMatrix Fvertex(1), m
Vector_TransMatrix Fvertex(2), m
Vector_TransMatrix Point2, m
Vector_TransMatrix move2, m
G_dx.IdentityMatrix m
If t = 2 Or t = 3 Then
    ang(0) = 0
End If
ang(0) = Atn(1) * 2 - Rad2p(0, 0, Fvertex(1).z, Fvertex(1).y)
G_dx.RotateXMatrix m, ang(0)
tmpcal = Sqr(Fvertex(1).z ^ 2 + Fvertex(1).y ^ 2)
If tmpcal = 0 Then
    ang(1) = Atn(1) * 8
Else
    ang(1) = Atn(1) * 6 + Atn(Fvertex(1).x / tmpcal)
End If
Vector_TransMatrix Fvertex(0), m
Vector_TransMatrix Fvertex(1), m
Vector_TransMatrix Fvertex(2), m
Vector_TransMatrix Point2, m
Vector_TransMatrix move2, m
G_dx.IdentityMatrix m
ang(1) = Rad2p(0, 0, Fvertex(1).x, Fvertex(1).y)
G_dx.RotateZMatrix m, ang(1)
Vector_TransMatrix Fvertex(0), m
Vector_TransMatrix Fvertex(1), m
Vector_TransMatrix Fvertex(2), m
Vector_TransMatrix Point2, m
Vector_TransMatrix move2, m
G_dx.IdentityMatrix m
ang(2) = -(Rad2p(0, 0, Fvertex(2).z, Fvertex(2).y) - Atn(1) * 2)
G_dx.RotateXMatrix m, ang(2)
Vector_TransMatrix Fvertex(0), m
Vector_TransMatrix Fvertex(1), m
Vector_TransMatrix Fvertex(2), m
Vector_TransMatrix Point2, m
Vector_TransMatrix move2, m
b = (move2.x - Point2.x)
If b = 0 Then
    FZRapXA = Infini
    FZRapXC = 0
Else
    FZRapXA = (move2.z - Point2.z) / b
    FZRapXC = Point2.z - FZRapXA * Point2.x
End If
b = (move2.y - Point2.y)
If b = 0 Then
    FZRapYA = Infini
    FZRapYC = 0
Else
    FZRapYA = (move2.z - Point2.z) / b
    FZRapYC = Point2.z - FZRapYA * Point2.y
End If
If FZRapXA = 0 Then
    x = Infini
Else
    If FZRapXA = Infini Then
        x = Point2.x
    Else
        x = -FZRapXC / FZRapXA
    End If
End If
If FZRapYA = 0 Then
    y = Infini
Else
    If FZRapYA = Infini Then
        y = Point2.y
    Else
        y = -FZRapYC / FZRapYA
    End If
End If
If Sgn(y) = Sgn(Fvertex(2).y) Then
    If intDroitY(Fvertex(2).x, Fvertex(2).y, Fvertex(1).x, Fvertex(1).y, y) >= x Then
        If intDroitY(Fvertex(0).x, Fvertex(0).y, Fvertex(2).x, Fvertex(2).y, y) <= x Then
            If Sgn(Point2.z) > 0 Then
                If move2.z <= 0.1 Then
                    Test = True
                    coll_CalcResvector ang(), move, resvector
                End If
            Else
                If move2.z >= -0.1 Then
                    coll_CalcResvector ang(), move, resvector
                    Test = True
                End If
            End If
        End If
    End If
End If
End Function

Private Function coll_CalcResvector(ang() As Single, move As D3DVECTOR, resvector As D3DVECTOR)
Dim m As D3DMATRIX, Movement As D3DVECTOR
Dim tmpm As D3DMATRIX
Dim normal As D3DVECTOR
Dim resvector2 As D3DVECTOR
normal = MakeVector(0, 0, 1)
G_dx.IdentityMatrix m
G_dx.VectorSubtract Movement, MakeVector(0, 0, 0), move
G_dx.RotateXMatrix m, -ang(2)
G_dx.RotateZMatrix tmpm, -ang(1)
G_dx.MatrixMultiply m, m, tmpm
Vector_TransMatrix normal, m
G_dx.IdentityMatrix m
G_dx.RotateXMatrix m, -ang(0)
Vector_TransMatrix normal, m
G_dx.VectorReflect Movement, Movement, normal
G_dx.VectorAdd resvector, resvector, Movement
resvector.x = resvector.x * 0.6
resvector.y = resvector.y * 0.6
resvector.z = resvector.z * 0.6
End Function

Public Function Vector_TransMatrix(vector As D3DVECTOR, m As D3DMATRIX) As Long
Dim x As Single, y As Single, z As Single
x = vector.x * m.rc11 + vector.y * m.rc21 + vector.z * m.rc31 + m.rc41
y = vector.x * m.rc12 + vector.y * m.rc22 + vector.z * m.rc32 + m.rc42
z = vector.x * m.rc13 + vector.y * m.rc23 + vector.z * m.rc33 + m.rc43
vector.x = x
vector.y = y
vector.z = z
End Function

Public Function Rad2p(ByVal x1 As Double, ByVal y1 As Double, ByVal x2 As Double, ByVal y2 As Double) As Variant
If y1 - y2 = 0 Then
    If x2 < x1 Then
        Rad2p = PI
    Else
        Rad2p = 0
    End If
    Exit Function
End If
Rad2p = Atn(Abs(x1 - x2) / Abs(y1 - y2))
If x2 >= x1 Then
    If y2 > y1 Then
        Rad2p = PI / 2 - Rad2p
    Else
        Rad2p = PI * 1.5 + Rad2p
    End If
Else
    If y2 > y1 Then
        Rad2p = PI / 2 + Rad2p
    Else
        Rad2p = PI * 1.5 - Rad2p
    End If
End If
End Function
Private Function intDroitY(ByVal x1 As Single, ByVal y1 As Single, ByVal x2 As Single, ByVal y2 As Single, ByVal PY As Single) As Single
Dim b As Single, a As Single, c As Single, y As Single
b = (x1 - x2)
If b = 0 Then b = 0.00001
a = (y1 - y2) / b
c = y1 - a * x1
If a = 0 Then
    intDroitY = y
Else
    intDroitY = (PY - c) / a
End If
End Function

Project Homepage: