Ich habe 2 Demos gebaut um das 'Separating Axis Theorem' umzusetzten bzw zu verstehen, für Kollisionserkennung zwischen rotierten Rechtecken (2d, erster Code) oder Quadern (3d, zweiter Code).
Unter Windows müßt ihr vielleicht einen anderen Font angeben. Unter Linux subsystem gtk2 verwenden um Bugs von DrawText() zu vermeiden, siehe Kommentar am Anfang der 3d Version.
2d version:
Code: Alles auswählen
EnableExplicit
Define ww, wh, style, win, canvas, event, quit
Define mouseButtonLeft, mouseButtonRight
ww=800
wh=600
style | #PB_Window_ScreenCentered
style | #PB_Window_SystemMenu
style | #PB_Window_MinimizeGadget
win = OpenWindow(#PB_Any, 50,100, ww,wh, "", style)
AddKeyboardShortcut(win, #PB_Shortcut_Escape, 10)
canvas = CanvasGadget(#PB_Any, 0, 0, ww, wh, #PB_Canvas_Keyboard)
SetGadgetAttribute(canvas, #PB_Canvas_Cursor, #PB_Cursor_Invisible)
SetWindowTitle(win, "press mousebuttons to rotate boxes")
Structure sVec
x.f
y.f
EndStructure
Structure sBox
; box position and origin of rotation
pos.sVec
; corner positions (relative to box position and unrotated)
p1.sVec ; top left
p2.sVec ; top right
p3.sVec ; bottom right
p4.sVec ; bottom left
; rotation
a.f
EndStructure
NewList b.sBox()
Define.sBox *b1, *b2
Declare.i newBox(posx.f, posY.f, p1x.f, p1y.f, p2x.f, p2y.f, p3x.f, p3y.f, p4x.f, p4y.f, a.f)
*b1 = newBox(400, 300, -60, -10, 60, -10, 60, 40, -60, 40, Radian(22))
*b2 = newBox(500, 200, -20, -50, 20, -50, 20, 20, -20, 20, Radian(-10))
Procedure.i newBox(posx.f, posY.f, p1x.f, p1y.f, p2x.f, p2y.f, p3x.f, p3y.f, p4x.f, p4y.f, a.f)
Shared b()
AddElement(b())
b()\pos\x = posx
b()\pos\y = posy
b()\p1\x = p1x
b()\p1\y = p1y
b()\p2\x = p2x
b()\p2\y = p2y
b()\p3\x = p3x
b()\p3\y = p3y
b()\p4\x = p4x
b()\p4\y = p4y
b()\a = a
ProcedureReturn b()
EndProcedure
Procedure rotate(*p.sVec, a.f, *pOut.sVec)
Define l.f
l = Sqr( (*p\x * *p\x) + (*p\y * *p\y) )
a = ATan2(*p\x, *p\y) + a
*pOut\x = l * Cos(a)
*pOut\y = l * Sin(a)
EndProcedure
Procedure.i boxCollisionUnrotated(*b1.sBox, *b2.sBox)
If (*b1\pos\x + *b1\p2\x) >= (*b2\pos\x + *b2\p1\x) And
(*b1\pos\x + *b1\p1\x) <= (*b2\pos\x + *b2\p2\x) And
(*b1\pos\y + *b1\p3\y) >= (*b2\pos\y + *b2\p1\y) And
(*b1\pos\y + *b1\p2\y) <= (*b2\pos\y + *b2\p3\y)
ProcedureReturn #True
EndIf
ProcedureReturn #False
EndProcedure
Procedure.f min(a.f, b.f)
If a < b
ProcedureReturn a
EndIf
ProcedureReturn b
EndProcedure
Procedure.f max(a.f, b.f)
If a > b
ProcedureReturn a
EndIf
ProcedureReturn b
EndProcedure
Procedure.i boxCollisionRotated(*b1.sBox, *b2.sBox)
Define.sBox b1rot, b2rot, bTmpRot
Define.f b1size
Define countIntersection
; copy boxes and apply rotation directly to corner points 1-4
CopyStructure(*b1, @ b1rot, sBox)
rotate(b1rot\p1, b1rot\a, @ b1rot\p1)
rotate(b1rot\p2, b1rot\a, @ b1rot\p2)
rotate(b1rot\p3, b1rot\a, @ b1rot\p3)
rotate(b1rot\p4, b1rot\a, @ b1rot\p4)
CopyStructure(*b2, @ b2rot, sBox)
rotate(b2rot\p1, b2rot\a, @ b2rot\p1)
rotate(b2rot\p2, b2rot\a, @ b2rot\p2)
rotate(b2rot\p3, b2rot\a, @ b2rot\p3)
rotate(b2rot\p4, b2rot\a, @ b2rot\p4)
countIntersection = 0
Define.i swapBoxes
; 1 = test box 1 normals with projected box 2 corners
; 2 = test box 2 normals with projected box 1 corners
For swapBoxes = 1 To 2
; test the other way round in second run
If swapBoxes = 2
Swap *b1, *b2 ; swap unrotated boxes
bTmpRot = b1rot ; swap rotated copies
b1rot = b2rot
b2rot = bTmpRot
EndIf
Define.i b1edge
Define.sVec *b1pA, *b1pB
; test for 2 edges per box (top and right)
For b1edge = 1 To 2
Select b1edge
Case 1 : ; box 1 top edge
*b1pA = b1rot\p1 ; top left
*b1pB = b1rot\p2 ; top right
b1size = *b1\p4\y - *b1\p1\y ; box height (using unrotated coordinates for simpler calculation)
Case 2 : ; box 1 right edge
*b1pA = b1rot\p2 ; top right
*b1pB = b1rot\p3 ; bottom right
b1size = *b1\p2\x - *b1\p1\x ; box width (using unrotated coordinates for simpler calculation)
EndSelect
Define.sVec edge, normal
Define.f f
Define.f dx, dy
Define.f b2pProjected
Define.f b2pProjectedMin
Define.f b2pProjectedMax
; get edge (point B - point A)
edge\x = *b1pB\x - *b1pA\x
edge\y = *b1pB\y - *b1pA\y
LineXY(b1rot\pos\x + *b1pA\x +1, ; ( +1 offset to be visible)
b1rot\pos\y + *b1pA\y +1,
b1rot\pos\x + *b1pA\x +1 + edge\x,
b1rot\pos\y + *b1pA\y +1 + edge\y,
$ff00bbdd) ; yellow
; get edge normal
normal\x = - edge\y
normal\y = edge\x
f = -1 / Sqr(normal\x * normal\x + normal\y * normal\y)
normal\x * f ; normalize magnitude, factor = reciprocal of length
normal\y * f
LineXY(b1rot\pos\x + *b1pA\x + edge\x/2 - normal\x * 200,
b1rot\pos\y + *b1pA\y + edge\y/2 - normal\y * 200,
b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * 200,
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * 200,
$aaff3333) ; blue
; project all 4 corners of box 2 onto current box 1 edge normal
Define.i b2p
Define.sVec *b2p
b2pProjectedMin = 999999
b2pProjectedMax = -999999
; each corner of box 2
For b2p = 1 To 4
Select b2p
Case 1 : *b2p = b2rot\p1
Case 2 : *b2p = b2rot\p2
Case 3 : *b2p = b2rot\p3
Case 4 : *b2p = b2rot\p4
EndSelect
; get vector from any point on edge (b1pA) to box 2 corner
dx = (b2rot\pos\x + *b2p\x) - (b1rot\pos\x + *b1pA\x)
dy = (b2rot\pos\y + *b2p\y) - (b1rot\pos\y + *b1pA\y)
; dot product of vector and normal (project corner vector onto normal vector)
b2pProjected = normal\x * dx + normal\y * dy
Circle(b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * b2pProjected,
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * b2pProjected,
2, $66dd33aa) ; purple
LineXY(b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * b2pProjected,
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * b2pProjected,
b2rot\pos\x + *b2p\x,
b2rot\pos\y + *b2p\y,
$66dd33aa) ; purple
; get min/max of projected range
If b2pProjected <= b2pProjectedMin
b2pProjectedMin = b2pProjected
EndIf
If b2pProjected >= b2pProjectedMax
b2pProjectedMax = b2pProjected
EndIf
Next
LineXY(b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * b2pProjectedMin,
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * b2pProjectedMin,
b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * b2pProjectedMax,
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * b2pProjectedMax,
$dd999999) ; gray
; test intersection of projection and box size
If (b2pProjectedMin <= 0) And (b2pProjectedMax >= -b1size)
countIntersection + 1
LineXY(b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * max(b2pProjectedMin, -b1size),
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * max(b2pProjectedMin, -b1size),
b1rot\pos\x + *b1pA\x + edge\x/2 + normal\x * min(0, b2pProjectedMax),
b1rot\pos\y + *b1pA\y + edge\y/2 + normal\y * min(0, b2pProjectedMax),
$aa3333ff) ; red
Else
; no intersection in one of the 4 projections means no collision
;Procedure #False ; (would cancel drawing)
EndIf
Next
Next
; for a collision all 4 cases must cause intersections
If countIntersection = 4
ProcedureReturn #True
EndIf
ProcedureReturn #False
EndProcedure
Procedure drawBox(*b.sBox, colorUnrotated, colorRotated)
Define.sVec p1, p2, p3, p4
;Shared *b1, *b2
; draw position
Circle(*b\pos\x, *b\pos\y, 2, colorUnrotated)
; draw unrotated
rotate(*b\p1, 0, @ p1) : p1\x + *b\pos\x : p1\y + *b\pos\y
rotate(*b\p2, 0, @ p2) : p2\x + *b\pos\x : p2\y + *b\pos\y
rotate(*b\p3, 0, @ p3) : p3\x + *b\pos\x : p3\y + *b\pos\y
rotate(*b\p4, 0, @ p4) : p4\x + *b\pos\x : p4\y + *b\pos\y
LineXY(p1\x, p1\y, p2\x, p2\y, colorUnrotated)
LineXY(p2\x, p2\y, p3\x, p3\y, colorUnrotated)
LineXY(p3\x, p3\y, p4\x, p4\y, colorUnrotated)
LineXY(p4\x, p4\y, p1\x, p1\y, colorUnrotated)
; draw rotated
rotate(*b\p1, *b\a, @ p1) : p1\x + *b\pos\x : p1\y + *b\pos\y
rotate(*b\p2, *b\a, @ p2) : p2\x + *b\pos\x : p2\y + *b\pos\y
rotate(*b\p3, *b\a, @ p3) : p3\x + *b\pos\x : p3\y + *b\pos\y
rotate(*b\p4, *b\a, @ p4) : p4\x + *b\pos\x : p4\y + *b\pos\y
LineXY(p1\x, p1\y, p2\x, p2\y, colorRotated)
LineXY(p2\x, p2\y, p3\x, p3\y, colorRotated)
LineXY(p3\x, p3\y, p4\x, p4\y, colorRotated)
LineXY(p4\x, p4\y, p1\x, p1\y, colorRotated)
EndProcedure
Procedure p()
Shared win, mouseButtonLeft, mouseButtonRight
Shared *b1, *b2
Define colorUnrotated, colorRotated
If WindowMouseX(win) >= 0 And WindowMouseY(win) >= 0
*b2\pos\x = WindowMouseX(win)
*b2\pos\y = WindowMouseY(win)
Else
*b2\pos\x = 500
*b2\pos\y = 200
EndIf
If mousebuttonLeft
*b1\a + 0.001
EndIf
If mouseButtonRight
*b2\a - 0.001
EndIf
; check and draw simple box collision
colorUnrotated = $4400ff00 ; dimmed green
If boxCollisionUnrotated(*b1, *b2)
colorUnrotated = $440000ff ; dimmed red (collision)
EndIf
; check and draw rotated box collision
colorRotated = $aa00ff00 ; green
If boxCollisionRotated(*b1, *b2)
colorRotated = $aa0000ff ; red (collision)
EndIf
; draw boxes only
drawBox(*b1, colorUnrotated, colorRotated)
drawBox(*b2, colorUnrotated, colorRotated)
EndProcedure
Repeat
If IsWindow(win) ;{
Repeat
event = WindowEvent()
Select event
Case #PB_Event_CloseWindow
quit = #True
Case #PB_Event_Menu
Select EventMenu()
Case 10
quit = #True
EndSelect
Case #PB_Event_Gadget
Select EventGadget()
Case canvas
Select EventType()
Case #PB_EventType_LeftButtonDown : mouseButtonLeft = #True
Case #PB_EventType_LeftButtonUp : mouseButtonLeft = #False
Case #PB_EventType_RightButtonDown : mouseButtonRight = #True
Case #PB_EventType_RightButtonUp : mouseButtonRight = #False
EndSelect
EndSelect
EndSelect
Until Not event
;}
EndIf
StartDrawing(CanvasOutput(canvas))
DrawingMode(#PB_2DDrawing_AllChannels)
Box(0, 0, OutputWidth(), OutputHeight(), $00000000)
DrawingMode(#PB_2DDrawing_AlphaBlend)
p()
StopDrawing()
Until quit
Code: Alles auswählen
; use subsystem gtk2 to avoid:
; pb 5.61, memory leak of DrawText(), program might be killed by the system after a while.
; pb 5.46 lts, wrong text displayed with DrawText().
EnableExplicit
Define ww, wh, style, win, canvas, event, quit
Define mouseButtonLeft
Define mouseButtonRight
Define mouseButtonMiddle
Define mouseWheelDelta
Define mouseX
Define mouseY
Define mouseXDelta
Define mouseYDelta
Define keyUpR
Define keyDownLeft
Define keyDownRight
Define control = 1
Define showStep = 1
Define frameTime, perSecond.f
Define font
font = LoadFont(#PB_Any, "monospace", 8)
;font = LoadFont(#PB_Any, "Lucida Console", 8)
ww=1200
wh=900
style | #PB_Window_ScreenCentered
style | #PB_Window_SystemMenu
style | #PB_Window_MinimizeGadget
win = OpenWindow(#PB_Any, 50,100, ww,wh, "", style)
AddKeyboardShortcut(win, #PB_Shortcut_Escape, 10)
canvas = CanvasGadget(#PB_Any, 0, 0, ww, wh, #PB_Canvas_Keyboard)
SetGadgetAttribute(canvas, #PB_Canvas_Cursor, #PB_Cursor_Invisible)
SetActiveGadget(canvas)
; -------------------------------------------------------------------
Structure sVec3d
x.f
y.f
z.f
EndStructure
Structure sBox3d
; box position and origin of rotation
pos.sVec3d
; corner positions. relative to box position and unrotated.
; boxCollisionRotated() will internally use temporary copies of the boxes with the box position and
; rotation applied to corner vertices directly so they will be absolute positions in global space in
; that context)
p.sVec3d[8]
; .
; 0________1 .
; /| /| .
; 3/_|_____2/ | .
; | | | | .
; | 4|_____|_5| .
; | / | / .
; |/_______|/ .
; 7 6 .
; .
; rotation. will be applied on global axis each, not on successively rotated local axes.
ax.f ; pitch
ay.f ; roll
az.f ; yaw
; .
; z .
; | y .
; | / .
; | / .
; |/ .
; +--------x .
; .
EndStructure
; -------------------------------------------------------------------
Define.f camDepth
Define.f camX, camY, camZ
Define.f camAZ
NewList b.sBox3d()
Define.sBox3d *b1, *b2
; -------------------------------------------------------------------
Procedure.i newBox(posX.f, posY.f, posZ.f,
p0x.f, p0y.f, p0z.f,
p1x.f, p1y.f, p1z.f,
p2x.f, p2y.f, p2z.f,
p3x.f, p3y.f, p3z.f,
p4x.f, p4y.f, p4z.f,
p5x.f, p5y.f, p5z.f,
p6x.f, p6y.f, p6z.f,
p7x.f, p7y.f, p7z.f,
ax.f, ay.f, az.f)
; verify planes
If Not(p0x = p3x And p0x = p4x And p0x = p7x) : CallDebugger : EndIf
If Not(p1x = p2x And p1x = p5x And p1x = p6x) : CallDebugger : EndIf
If Not(p0y = p1y And p0y = p4y And p0y = p5y) : CallDebugger : EndIf
If Not(p3y = p2y And p3y = p6y And p3y = p7y) : CallDebugger : EndIf
If Not(p0z = p1z And p0z = p2z And p0z = p3z) : CallDebugger : EndIf
If Not(p4z = p5z And p4z = p6z And p4z = p7z) : CallDebugger : EndIf
Shared b()
AddElement(b())
b()\pos\x = posX
b()\pos\y = posY
b()\pos\z = posZ
b()\p[0]\x = p0x : b()\p[0]\y = p0y : b()\p[0]\z = p0z
b()\p[1]\x = p1x : b()\p[1]\y = p1y : b()\p[1]\z = p1z
b()\p[2]\x = p2x : b()\p[2]\y = p2y : b()\p[2]\z = p2z
b()\p[3]\x = p3x : b()\p[3]\y = p3y : b()\p[3]\z = p3z
b()\p[4]\x = p4x : b()\p[4]\y = p4y : b()\p[4]\z = p4z
b()\p[5]\x = p5x : b()\p[5]\y = p5y : b()\p[5]\z = p5z
b()\p[6]\x = p6x : b()\p[6]\y = p6y : b()\p[6]\z = p6z
b()\p[7]\x = p7x : b()\p[7]\y = p7y : b()\p[7]\z = p7z
b()\ax = ax
b()\ay = ay
b()\az = az
ProcedureReturn b()
EndProcedure
Procedure rotate(dx.f, dy.f, dz.f, ax.f, ay.f, az.f, *dxOut.Float, *dyOut.Float, *dzOut.Float)
Define lx.f
Define ly.f
Define lz.f
; each rotation is applied to absolute/global axis, not to successively rotated local axis
; rotate around x axis (up/down, pitch)
lx = Sqr(dz * dz + dy * dy)
ax = ATan2( dz, dy) + ax
*dzOut\f = lx * Cos(ax)
*dyOut\f = lx * Sin(ax)
dz = *dzOut\f
dy = *dyOut\f
; rotate around y axis (left/right, roll)
ly = Sqr(dx * dx + dz * dz)
ay = ATan2( dx, dz) + ay
*dxOut\f = ly * Cos(ay)
*dzOut\f = ly * Sin(ay)
dx = *dxOut\f
dz = *dzOut\f
; rotate around z axis (turn, yaw)
lz = Sqr(dx * dx + dy * dy)
az = ATan2( dx, dy) + az
*dxOut\f = lz * Cos(az)
*dyOut\f = lz * Sin(az)
;dx = *dxOut\f
;dy = *dyOut\f
EndProcedure
Procedure.i boxCollisionUnrotated(*b1.sBox3d, *b2.sBox3d)
If (*b1\pos\x + *b1\p[1]\x) >= (*b2\pos\x + *b2\p[0]\x) And
(*b1\pos\x + *b1\p[0]\x) <= (*b2\pos\x + *b2\p[1]\x) And
(*b1\pos\y + *b1\p[0]\y) >= (*b2\pos\y + *b2\p[3]\y) And
(*b1\pos\y + *b1\p[3]\y) <= (*b2\pos\y + *b2\p[0]\y) And
(*b1\pos\z + *b1\p[0]\z) >= (*b2\pos\z + *b2\p[4]\z) And
(*b1\pos\z + *b1\p[4]\z) <= (*b2\pos\z + *b2\p[0]\z)
ProcedureReturn #True
EndIf
ProcedureReturn #False
EndProcedure
Procedure.f min(a.f, b.f)
If a < b
ProcedureReturn a
EndIf
ProcedureReturn b
EndProcedure
Procedure.f max(a.f, b.f)
If a > b
ProcedureReturn a
EndIf
ProcedureReturn b
EndProcedure
Procedure.i spaceToView(x.f, y.f, z.f, *x2d.Float, *y2d.Float)
Shared camX, camY, camZ, camDepth, camAZ
; get position in space relative to cam
x = x - camX
y = y - camY
z = z - camZ
; apply cam rotation around z axis
rotate(x, y, z, 0, 0, -camAZ, @ x, @ y, @ z)
; only if before cam
If y > 0
*x2d\f = ( x / y) * camDepth
*y2d\f = (-z / y) * camDepth
ProcedureReturn #True
EndIf
; position is behind cam
ProcedureReturn #False
EndProcedure
Procedure circle3d(x.f, y.f, z.f, radius, color)
Shared ww, wh, camY
Define.f x2d, y2d
If spaceToView(x, y, z, @ x2d, @ y2d)
x2d + (ww / 2)
y2d + (wh / 2)
Circle(x2d, y2d, radius, color)
EndIf
EndProcedure
Procedure line3d(x1.f, y1.f, z1.f, x2.f, y2.f, z2.f, color)
Shared ww, wh, camY
Define.f p1x, p1y
Define.f p2x, p2y
If spaceToView(x1, y1, z1, @ p1x, @ p1y)
If spaceToView(x2, y2, z2, @ p2x, @ p2y)
p1x + (ww / 2)
p1y + (wh / 2)
p2x + (ww / 2)
p2y + (wh / 2)
LineXY(p1x, p1y, p2x, p2y, color)
EndIf
EndIf
EndProcedure
Procedure line3dByVec(*p1.sVec3d, *p2.sVec3d, color, *position.sVec3d = #Null)
If *position
line3d(*position\x + *p1\x, *position\y + *p1\y, *position\z + *p1\z,
*position\x + *p2\x, *position\y + *p2\y, *position\z + *p2\z,
color)
Else
line3d(*p1\x, *p1\y, *p1\z,
*p2\x, *p2\y, *p2\z,
color)
EndIf
EndProcedure
Procedure text3d(x.f, y.f, z.f, text.s, color, *position.sVec3d = #Null)
Shared ww, wh, camY, font
Define.f x2d, y2d
If *position
x + *position\x
y + *position\y
z + *position\z
EndIf
If spaceToView(x, y, z, @ x2d, @ y2d)
x2d + (ww / 2)
y2d + (wh / 2)
DrawingFont(FontID(font))
DrawText(x2d, y2d, text, color)
EndIf
EndProcedure
Procedure vecMinus(*a.sVec3d, *b.sVec3d, *out.sVec3d)
*out\x = *a\x - *b\x
*out\y = *a\y - *b\y
*out\z = *a\z - *b\z
EndProcedure
Procedure vecCrossProduct(*a.sVec3d, *b.sVec3d, *out.sVec3d)
*out\x = (*a\y * *b\z) - (*a\z * *b\y)
*out\y = (*a\z * *b\x) - (*a\x * *b\z)
*out\z = (*a\x * *b\y) - (*a\y * *b\x)
EndProcedure
Procedure.f vecDotProduct(*a.sVec3d, *b.sVec3d)
ProcedureReturn *a\x * *b\x + *a\y * *b\y + *a\z * *b\z
EndProcedure
Procedure vecNormalizeMagnitude(*v.sVec3d)
Define.f f
f = Sqr(*v\x * *v\x + *v\y * *v\y + *v\z * *v\z)
; avoid division-by-zero if v is a zero-vector {0,0,0} (can happen for normals of parallel edges)
If f
f = 1 / f
*v\x * f
*v\y * f
*v\z * f
EndIf
EndProcedure
Procedure.i boxCollisionRotated(*b1source.sBox3d, *b2source.sBox3d)
Define.sBox3d box1
Define.sBox3d box2
Define countIntersection
Define i, iNormal
Define b
Define.sBox3d *b
Shared showStep
Define.sVec3d offset ; little offset to make edge markers visible
offset\x = -0.6
offset\y = -0.6
offset\z = -0.6
; copy boxes and apply box rotation and position directly to corner vertices of the copy
CopyStructure(*b1source, box1, sBox3d)
For i=0 To 7
With box1
rotate(\p[i]\x, \p[i]\y, \p[i]\z, \ax, \ay, \az, @ \p[i]\x, @ \p[i]\y, @ \p[i]\z)
\p[i]\x + \pos\x
\p[i]\y + \pos\y
\p[i]\z + \pos\z
EndWith
Next
CopyStructure(*b2source, box2, sBox3d)
For i=0 To 7
With box2
rotate(\p[i]\x, \p[i]\y, \p[i]\z, \ax, \ay, \az, @ \p[i]\x, @ \p[i]\y, @ \p[i]\z)
\p[i]\x + \pos\x
\p[i]\y + \pos\y
\p[i]\z + \pos\z
EndWith
Next
countIntersection = 0
For iNormal = 1 To 15
Define.sVec3d *edge1start
Define.sVec3d *edge1end
Define.sVec3d *edge2start
Define.sVec3d *edge2end
Define.sVec3d normal
Define.sVec3d edge1Direction
Define.sVec3d edge2Direction
Define.sVec3d normalOrigin
; for each of the 15 cases we get 2 edges, then we get the normal for those 2 edges, then we project each boxes corners onto the normal
; and check if the projected ranges of each box intersect. if all 15 cases create an intersection then the boxes are colliding.
; case 1-6 will use 2 edges of the same box to get the face normals.
; case 9-15 will use one edge of each box to test for edge-to-edge collisions.
; possible optimizations:
; - instead of calculating the face normals from 2 egdes connected to a corner, we could simply use the third edge that is connected
; to that corner as a normal directly.
; - instead of projecting each corner of the box that belongs to the face normal we could get that boxes range on the normal by just
; using the width, height or depth (whichever is parallel to the normal)
Select iNormal
; box1 edge pairs (3 faces/normals)
Case 1 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[3] : *edge2start = @ box1\p[0] : *edge2end = @ box1\p[1]
Case 2 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[1] : *edge2start = @ box1\p[0] : *edge2end = @ box1\p[4]
Case 3 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[4] : *edge2start = @ box1\p[0] : *edge2end = @ box1\p[3]
; box2 edge pairs (3 faces/normals)
Case 4 : *edge1start = @ box2\p[0] : *edge1end = @ box2\p[3] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[1]
Case 5 : *edge1start = @ box2\p[0] : *edge1end = @ box2\p[1] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[4]
Case 6 : *edge1start = @ box2\p[0] : *edge1end = @ box2\p[4] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[3]
; box1 edge 1-3 with box2 edge 1
Case 7 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[1] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[1]
Case 8 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[4] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[1]
Case 9 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[3] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[1]
; box1 edge 1-3 with box2 edge 2
Case 10 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[1] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[4]
Case 11 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[4] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[4]
Case 12 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[3] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[4]
; box1 edge 1-3 with box2 edge 3
Case 13 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[1] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[3]
Case 14 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[4] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[3]
Case 15 : *edge1start = @ box1\p[0] : *edge1end = @ box1\p[3] : *edge2start = @ box2\p[0] : *edge2end = @ box2\p[3]
EndSelect
; edge-end minus edge-start = edge direction vector
vecMinus(*edge1end, *edge1start, @ edge1Direction)
vecMinus(*edge2end, *edge2start, @ edge2Direction)
; get the normal for the 2 edges.
; for test 1-6 this will be the face-normal.
; for edge-to-edge tests 9-15 those edges might not form a plane but we can still get the normal. parallel non-identical edges
; don't have a common normal and will result in a zero-vector (see box setup test 'Case 2' before the main loop).
; also normalizing the normal magnitude to 1 is only for visualization, projected ranges and their
; intersection will still work if we comment it out.
vecCrossProduct(edge1Direction, edge2Direction, @ normal)
vecNormalizeMagnitude(@ normal)
; we want the normal origin at face center for the face-normal tests, and between the edges for edge-to-edge tests.
; this is only for the visualization to easily see where normal comes from. we could simply use no origin (meaning
; origin at global {0,0,0}) and the projected ranges and their intersection would still work.
If iNormal <= 6
; align normal at face center
normalOrigin\x = *edge1start\x + edge1Direction\x/2 + edge2Direction\x/2
normalOrigin\y = *edge1start\y + edge1Direction\y/2 + edge2Direction\y/2
normalOrigin\z = *edge1start\z + edge1Direction\z/2 + edge2Direction\z/2
Else
; align normal between edges
normalOrigin\x = (*edge1start\x + edge1Direction\x/2 + *edge2start\x + edge2Direction\x/2) / 2
normalOrigin\y = (*edge1start\y + edge1Direction\y/2 + *edge2start\y + edge2Direction\y/2) / 2
normalOrigin\z = (*edge1start\z + edge1Direction\z/2 + *edge2start\z + edge2Direction\z/2) / 2
EndIf
; test without origin
;normalOrigin\x = 0
;normalOrigin\y = 0
;normalOrigin\z = 0
If showStep = iNormal
line3dByVec(*edge1start, *edge1end, $cc88eeff, @ offset) ; yellow (edge)
line3dByVec(*edge2start, *edge2end, $cc88eeff, @ offset) ; yellow (edge)
circle3d(normalOrigin\x,
normalOrigin\y,
normalOrigin\z,
4,
$eeff3333) ; blue (normal origin)
line3d(normalOrigin\x,
normalOrigin\y,
normalOrigin\z,
normalOrigin\x + normal\x * 150,
normalOrigin\y + normal\y * 150,
normalOrigin\z + normal\z * 150,
$eeff3333) ; blue (normal, positive direction)
line3d(normalOrigin\x,
normalOrigin\y,
normalOrigin\z,
normalOrigin\x - normal\x * 150,
normalOrigin\y - normal\y * 150,
normalOrigin\z - normal\z * 150,
$88ff3333) ; dim blue (normal, negative direction)
EndIf
Define.sVec3d p
Define.f pProjected
Define.f box1ProjectedMin
Define.f box1ProjectedMax
Define.f box2ProjectedMin
Define.f box2ProjectedMax
box1ProjectedMin = 99999999999
box1ProjectedMax = -99999999999
box2ProjectedMin = 99999999999
box2ProjectedMax = -99999999999
; project all 8 corners of each of the 2 boxes onto the current normal to get a range per box
For b = 1 To 2
For i = 0 To 7
Select b
Case 1 :
vecMinus(box1\p[i], normalOrigin, @ p) ; get corner position realtive to normal origin
pProjected = vecDotProduct( @ normal, p) ; get position on normal
box1ProjectedMin = min(box1ProjectedMin, pProjected) ; update range
box1ProjectedMax = max(box1ProjectedMax, pProjected)
Case 2 :
vecMinus(box2\p[i], normalOrigin, @ p) ; get corner position realtive to normal origin
pProjected = vecDotProduct( @ normal, p) ; get position on normal
box2ProjectedMin = min(box2ProjectedMin, pProjected) ; update range
box2ProjectedMax = max(box2ProjectedMax, pProjected)
EndSelect
If showStep = iNormal
circle3d(normalOrigin\x + normal\x * pProjected,
normalOrigin\y + normal\y * pProjected,
normalOrigin\z + normal\z * pProjected,
2,
$55dd33aa) ; purple (corner position on normal)
line3d(normalOrigin\x + p\x,
normalOrigin\y + p\y,
normalOrigin\z + p\z,
normalOrigin\x + normal\x * pProjected,
normalOrigin\y + normal\y * pProjected,
normalOrigin\z + normal\z * pProjected,
$66dd33aa) ; purple (corner-to-normal line)
EndIf
Next
Next
If showStep = iNormal
line3d(normalOrigin\x + normal\x * box1ProjectedMin,
normalOrigin\y + normal\y * box1ProjectedMin,
normalOrigin\z + normal\z * box1ProjectedMin,
normalOrigin\x + normal\x * box1ProjectedMax,
normalOrigin\y + normal\y * box1ProjectedMax,
normalOrigin\z + normal\z * box1ProjectedMax,
$dd999999) ; gray (projected range box 1)
line3d(normalOrigin\x + normal\x * box2ProjectedMin,
normalOrigin\y + normal\y * box2ProjectedMin,
normalOrigin\z + normal\z * box2ProjectedMin,
normalOrigin\x + normal\x * box2ProjectedMax,
normalOrigin\y + normal\y * box2ProjectedMax,
normalOrigin\z + normal\z * box2ProjectedMax,
$dd999999) ; gray (projected range box 2)
EndIf
; check if the ranges projected onto the normal intersect
If (box1ProjectedMax >= box2ProjectedMin) And (box1ProjectedMin <= box2ProjectedMax)
countIntersection + 1
If showStep = iNormal
line3d(normalOrigin\x + normal\x * max(box1ProjectedMin, box2ProjectedMin),
normalOrigin\y + normal\y * max(box1ProjectedMin, box2ProjectedMin),
normalOrigin\z + normal\z * max(box1ProjectedMin, box2ProjectedMin),
normalOrigin\x + normal\x * min(box1ProjectedMax, box2ProjectedMax),
normalOrigin\y + normal\y * min(box1ProjectedMax, box2ProjectedMax),
normalOrigin\z + normal\z * min(box1ProjectedMax, box2ProjectedMax),
$cc3333ff) ; red (intersection of projected range)
EndIf
Else
; any non-intersection will reveal a non-collision of the 2 boxes, so we could
; stop here (but would cause incomplete drawing of the collision test process)
;ProcedureReturn #False
EndIf
Next
; all 15 cases must result in an intersection for a box collision
If countIntersection = 15
ProcedureReturn #True
EndIf
ProcedureReturn #False
EndProcedure
Procedure drawBox(*b.sBox3d, posText.s, colorUnrotated, colorRotated)
Define i
Define.sBox3d boxRotated
; draw unrotated
With *b
; draw position
circle3d(\pos\x, \pos\y, \pos\z, 2, colorUnrotated)
text3d(\pos\x, \pos\y, \pos\z, " "+posText, colorUnrotated)
; top quad
line3dByVec(\p[0], \p[1], colorUnrotated, \pos)
line3dByVec(\p[1], \p[2], colorUnrotated, \pos)
line3dByVec(\p[2], \p[3], colorUnrotated, \pos)
line3dByVec(\p[3], \p[0], colorUnrotated, \pos)
; bottom quad
line3dByVec(\p[4], \p[5], colorUnrotated, \pos)
line3dByVec(\p[5], \p[6], colorUnrotated, \pos)
line3dByVec(\p[6], \p[7], colorUnrotated, \pos)
line3dByVec(\p[7], \p[4], colorUnrotated, \pos)
; 4 vertical edges
line3dByVec(\p[0], \p[4], colorUnrotated, \pos)
line3dByVec(\p[1], \p[5], colorUnrotated, \pos)
line3dByVec(\p[2], \p[6], colorUnrotated, \pos)
line3dByVec(\p[3], \p[7], colorUnrotated, \pos)
For i=0 To 7
text3d(\p[i]\x, \p[i]\y, \p[i]\z, Str(i), $44ffffff, \pos)
Next
EndWith
; draw rotated
; make a copy and apply rotation to corner vertices. box position is not applied but passed to line3dByVec()
CopyStructure(*b, @ boxRotated, sBox3d)
With boxRotated
For i=0 To 7
rotate(\p[i]\x, \p[i]\y, \p[i]\z, \ax, \ay, \az, @ \p[i]\x, @ \p[i]\y, @ \p[i]\z)
Next
; top quad
line3dByVec(\p[0], \p[1], colorRotated, \pos)
line3dByVec(\p[1], \p[2], colorRotated, \pos)
line3dByVec(\p[2], \p[3], colorRotated, \pos)
line3dByVec(\p[3], \p[0], colorRotated, \pos)
; bottom quad
line3dByVec(\p[4], \p[5], colorRotated, \pos)
line3dByVec(\p[5], \p[6], colorRotated, \pos)
line3dByVec(\p[6], \p[7], colorRotated, \pos)
line3dByVec(\p[7], \p[4], colorRotated, \pos)
; 4 vertical edges
line3dByVec(\p[0], \p[4], colorRotated, \pos)
line3dByVec(\p[1], \p[5], colorRotated, \pos)
line3dByVec(\p[2], \p[6], colorRotated, \pos)
line3dByVec(\p[3], \p[7], colorRotated, \pos)
For i=0 To 7
text3d(\p[i]\x, \p[i]\y, \p[i]\z, Str(i), $aaffffff, \pos)
Next
EndWith
EndProcedure
Procedure processInput()
Shared win, mouseButtonLeft, mouseButtonRight, mouseButtonMiddle
Shared perSecond
Shared *b1, *b2
Shared keyUpR, keyDownLeft, keyDownRight
Shared control, camX, camY, camZ, camAZ, mouseXDelta, mouseYDelta, mouseWheelDelta
Static.f ax, ay, az, animate
Static.sVec3d move
; move x/z
If mouseButtonMiddle
Select control
Case 0
move\x = mouseXDelta
move\y = 0
move\z = - mouseYDelta
rotate(move\x, move\y, move\z, 0, 0, camAZ, @ move\x, @ move\y, @ move\z)
camX + move\x
camY + move\y
camZ + move\z
Case 1
*b1\pos\x + mouseXDelta
*b1\pos\z - mouseYDelta
Case 2
*b2\pos\x + mouseXDelta
*b2\pos\z - mouseYDelta
EndSelect
EndIf
; move y
Select control
Case 0
move\x = 0
move\y = mouseWheelDelta * 12
move\z = 0
rotate(move\x, move\y, move\z, 0, 0, camAZ, @ move\x, @ move\y, @ move\z)
camX + move\x
camY + move\y
camZ + move\z
Case 1
*b1\pos\y + mouseWheelDelta * 12
Case 2
*b2\pos\y + mouseWheelDelta * 12
EndSelect
; get random rotation
If keyUpR
Select control
Case 1, 2
ax = 1.0 * (Random(1000) / 1000.0 ) * 2 * #PI
ay = 1.0 * (Random(1000) / 1000.0 ) * 2 * #PI
az = 1.0 * (Random(1000) / 1000.0 ) * 2 * #PI
animate = 1.0
EndSelect
EndIf
; rotate cam
If keyDownLeft
camAZ + 0.01
EndIf
If keyDownRight
camAZ - 0.01
EndIf
; animate random rotation
If animate > 0
animate - 0.05 * perSecond
If animate < 0
animate = 0
EndIf
Select control
Case 1
*b1\ax = (ax * (1-animate)) + (*b1\ax * animate)
*b1\ay = (ay * (1-animate)) + (*b1\ay * animate)
*b1\az = (az * (1-animate)) + (*b1\az * animate)
Case 2
*b2\ax = (ax * (1-animate)) + (*b2\ax * animate)
*b2\ay = (ay * (1-animate)) + (*b2\ay * animate)
*b2\az = (az * (1-animate)) + (*b2\az * animate)
EndSelect
EndIf
EndProcedure
Procedure checkCollisonsAndDraw()
Shared *b1, *b2
Define colorUnrotated, colorRotated
; check and draw unrotated box collision
colorUnrotated = $4400ff00 ; dimmed green
If boxCollisionUnrotated(*b1, *b2)
colorUnrotated = $440000ff ; dimmed red (collision)
EndIf
; check and draw rotated box collision
colorRotated = $aa00ff00 ; green
If boxCollisionRotated(*b1, *b2)
colorRotated = $aa0000ff ; red (collision)
EndIf
; draw boxes only
drawBox(*b1, "box 1", colorUnrotated, colorRotated)
drawBox(*b2, "box 2", colorUnrotated, colorRotated)
EndProcedure
; -------------------------------------------------------------------
camX = 0
camY = -300
camZ = 30
camDepth = 500
*b1 = newBox(000, 000, 00,
-60, 10, 10, 60, 10, 10, 60, -40, 10, -60, -40, 10,
-60, 10, -10, 60, 10, -10, 60, -40, -10, -60, -40, - 10,
Radian(-8), Radian(-8), Radian(-8))
*b2 = newBox(100, -10, 40,
-20, -10, -20, 20, -10, -20, 20, -30, -20, -20, -30, -20,
-20, -10, -100, 20, -10, -100, 20, -30, -100, -20, -30, - 100,
Radian(4), Radian(4), Radian(4))
Select 0
Case 1 :
; check edge-to-edge collision that would fail if we would test only with face normals (step 1-6).
; (edge-to-edge test step 7 is the only one revealing the non-collision)
showStep = 7
*b1\pos\x = 100
*b1\pos\y = 0
*b1\pos\z = 45
*b1\ax = 2.54469
*b1\ay = 5.8370
*b1\az = 2.3247
*b2\pos\x = 100
*b2\pos\y = -10
*b2\pos\z = 40
*b2\ax = 5.8433
*b2\ay = 0.2450
*b2\az = 1.3948
Case 2 :
; check edge-to-edge collision test of parallel edges, which results in a zero-vector normal. all projections of that test step
; will collapse into a single point (on my machine), which means this case will be counted as intersection/collision, but since
; the two edges are parallel, an corresponding face-normal test step (1-6) will reveal the non-collision correctly.
showStep = 15
*b1\pos\x = 100
*b1\pos\y = 0
*b1\pos\z = 45
*b1\ax = Radian(0)
*b1\ay = Radian(0)
*b1\az = Radian(0)
*b2\pos\x = 100
*b2\pos\y = -10
*b2\pos\z = 40
*b2\ax = Radian(0)
*b2\ay = Radian(4)
*b2\az = Radian(0)
EndSelect
; -------------------------------------------------------------------
frameTime = ElapsedMilliseconds()
Repeat
perSecond = 1.0 * (ElapsedMilliseconds() - frameTime) / 1000.0
frameTime = ElapsedMilliseconds()
mouseWheelDelta = 0
mouseXDelta = 0
mouseYDelta = 0
keyUpR = #False
keyDownLeft = #False
keyDownRight = #False
If IsWindow(win) ;{
Repeat
event = WindowEvent()
Select event
Case #PB_Event_CloseWindow
quit = #True
Case #PB_Event_Menu
Select EventMenu()
Case 10
quit = #True
EndSelect
Case #PB_Event_Gadget
Select EventGadget()
Case canvas
Select EventType()
Case #PB_EventType_MiddleButtonDown : mouseButtonMiddle = #True
Case #PB_EventType_MiddleButtonUp : mouseButtonMiddle = #False
Case #PB_EventType_LeftButtonDown : mouseButtonLeft = #True
Case #PB_EventType_LeftButtonUp : mouseButtonLeft = #False
Case #PB_EventType_RightButtonDown : mouseButtonRight = #True
Case #PB_EventType_RightButtonUp : mouseButtonRight = #False
Case #PB_EventType_MouseWheel
mouseWheelDelta + GetGadgetAttribute(canvas, #PB_Canvas_WheelDelta)
Case #PB_EventType_MouseMove
mouseXDelta = GetGadgetAttribute(canvas, #PB_Canvas_MouseX) - mouseX
mouseYDelta = GetGadgetAttribute(canvas, #PB_Canvas_MouseY) - mouseY
mouseX = GetGadgetAttribute(canvas, #PB_Canvas_MouseX)
mouseY = GetGadgetAttribute(canvas, #PB_Canvas_MouseY)
Case #PB_EventType_KeyDown
Select GetGadgetAttribute(canvas, #PB_Canvas_Key)
Case #PB_Shortcut_R : keyUpR = #True
Case #PB_Shortcut_Left : keyDownLeft = #True
Case #PB_Shortcut_Right : keyDownRight = #True
EndSelect
Case #PB_EventType_KeyUp
; pb bug: http://www.purebasic.fr/english/viewtopic.php?f=23&t=56366
Define shortcut = GetGadgetAttribute(canvas, #PB_Canvas_Key)
If shortcut >= 97 And shortcut <= 122
shortcut - 32
EndIf
Select shortcut
Case #PB_Shortcut_C : control = 0
Case #PB_Shortcut_V : control = 1
Case #PB_Shortcut_B : control = 2
Case #PB_Shortcut_R : keyUpR = #True
Case #PB_Shortcut_0 : showStep = 0
EndSelect
If GetGadgetAttribute(canvas, #PB_Canvas_Modifiers) & #PB_Canvas_Control
Select shortcut
Case #PB_Shortcut_1 : showStep = 6 + 1
Case #PB_Shortcut_2 : showStep = 6 + 2
Case #PB_Shortcut_3 : showStep = 6 + 3
Case #PB_Shortcut_4 : showStep = 6 + 4
Case #PB_Shortcut_5 : showStep = 6 + 5
Case #PB_Shortcut_6 : showStep = 6 + 6
Case #PB_Shortcut_7 : showStep = 6 + 7
Case #PB_Shortcut_8 : showStep = 6 + 8
Case #PB_Shortcut_9 : showStep = 6 + 9
EndSelect
Else
Select shortcut
Case #PB_Shortcut_1 : showStep = 1
Case #PB_Shortcut_2 : showStep = 2
Case #PB_Shortcut_3 : showStep = 3
Case #PB_Shortcut_4 : showStep = 4
Case #PB_Shortcut_5 : showStep = 5
Case #PB_Shortcut_6 : showStep = 6
EndSelect
EndIf
EndSelect
EndSelect
EndSelect
Until Not event
;}
EndIf
StartDrawing(CanvasOutput(canvas))
DrawingMode(#PB_2DDrawing_AllChannels)
Box(0, 0, OutputWidth(), OutputHeight(), $00000000)
;DrawingMode(#PB_2DDrawing_AlphaBlend)
DrawingMode(#PB_2DDrawing_AlphaBlend | #PB_2DDrawing_Transparent)
DrawingFont(FontID(font))
Define txt.s
Select control
Case 0 : txt = "cam"
Case 1 : txt = "box 1"
Case 2 : txt = "box 2"
EndSelect
Define y = 10
DrawText(10, y, "c : control cam " + Left("(*)", 99*Bool(control=0))) : y + 10
DrawText(10, y, "v : control box 1 " + Left("(*)", 99*Bool(control=1))) : y + 10
DrawText(10, y, "b : control box 2 " + Left("(*)", 99*Bool(control=2))) : y + 10
y + 10
DrawText(10, y, "move in x/z by moving mouse while pressing middle mouse button") : y + 10
DrawText(10, y, "move in y with mousewheel ") : y + 10
DrawText(10, y, "← → : rotate cam ") : y + 10
If control > 0
DrawText(10, y, "r : random rotate box " + control) : y + 10
EndIf
y + 10
DrawText(10, y, " 1-6 : show step 1-6 (face normals) " + Left("("+showStep+")", 99*Bool(showStep>=1 And showStep<=6 ))) : y + 10
DrawText(10, y, "ctrl 1-9 : show step 7-15 (edge to edge normals) " + Left("("+showStep+")", 99*Bool(showStep>=7 And showStep<=15))) : y + 10
processInput()
checkCollisonsAndDraw()
StopDrawing()
Until quit