Talk:Physics
From WikiManual
Attribute VB_Name = "Physics"
'
' P H Y S I C S
'
Option Explicit
' experimental object: used to store a grid of environmental
' features
Public envir As New EnvGrid
Public envgridpres As Boolean
Public nlink As Single ' links physics constants
Public plink As Single
Public klink As Single
Public mlink As Single
Const cof_E As Single = 0.8
' assigns every robot a new acceleration
' this is the first step in recalculating positions
Public Sub Newaccel()
Dim nd As node
Dim t As Integer
Set nd = rlist.firstnode
While Not nd Is rlist.last
t = nd.robn
With rob(t)
.ax = 0 + Random(-SimOpts.PhysBrown, SimOpts.PhysBrown)
.ay = SimOpts.PhysGrav + Random(-SimOpts.PhysBrown, SimOpts.PhysBrown)
Set nd = rlist.nextnode(nd)
End With
Wend
KeepOff
End Sub
' calculates angle between (x1,y1) and (x2,y2)
Public Function angle(x1 As Long, y1 As Long, x2 As Long, y2 As Long) As Single
Dim an As Single
Dim dx As Long
Dim dy As Long
dx = x2 - x1
dy = y1 - y2
If dx = 0 Then
an = 0
If dy < 0 Then an = pi / 4 * 3
Else
an = Atn(dy / dx)
If dx < 0 Then
an = an + pi
End If
End If
angle = an
End Function
' normalizes angle in 0,2pi
Public Function angnorm(ByVal an As Single) As Single
While an < 0
an = an + 2 * pi
Wend
While an > 2 * pi
an = an - 2 * pi
Wend
angnorm = an
End Function
' calculates difference between two angles
Public Function AngDiff(a1 As Single, a2 As Single) As Single
Dim r As Single
r = a1 - a2
If r > pi Then
r = -(2 * pi - r)
End If
If r < -pi Then
r = r + 2 * pi
End If
AngDiff = r
End Function
' same as above, but using the experimental environment grid
Private Sub NewAccelEnv()
Dim nd As node
Dim t As Integer
Set nd = rlist.firstnode
Dim phv As Variant
While Not nd Is rlist.last
t = nd.robn
With rob(t)
phv = envir.GetMat(.envx, .envy)
.ax = 0 + Random(-phv(1), phv(1))
.ay = phv(2) + Random(-phv(1), phv(1))
Set nd = rlist.nextnode(nd)
End With
Wend
KeepOff
End Sub
Public Sub linklen()
Dim k As Integer
Dim d As Single
Dim m As Single 'Acceleration modifier
Dim dx As Long
Dim dy As Long
Dim dxl As Single
Dim dyl As Single
Dim j As Integer
Dim kconst As Single
Dim TLength As Single 'target length for tie. = existing length + shrink variable
Dim Xdif As Single
Dim Ydif As Single
Dim AvgVX As Single
Dim AvgVY As Single
Dim Stiff As Integer
Dim nd As node
Dim t As Integer
Dim kl As Boolean
Dim dif As Single
m = 1.5
Set nd = rlist.firstnode
While Not nd Is rlist.last
j = 1
t = nd.robn
If Not rob(t).Corpse Then 'new condition added to speed up code by not parsing corpses
If rob(t).Numties > 0 Then 'new condition added to speed up code by not parsing robots without ties.
While rob(t).Ties(j).pnt > 0 'Tie points to something
k = rob(t).Ties(j).pnt 'k is the number of the robot it points to
TLength = rob(t).Ties(j).ln + rob(t).Ties(j).shrink
rob(t).Ties(j).shrink = 0
If k > 0 And k <> t Then 'doesn't point to himself?
dx = (rob(t).x - rob(k).x) 'X distance
dy = (rob(t).Y - rob(k).Y) 'Y distance
d = Sqr(dx ^ 2 + dy ^ 2) 'inter robot distance
If d = 0 Then d = 0.01 'UUH??? prevents division by zero perhaps?
dif = Abs(d - TLength) * 0.1 'sets strength of ties
kconst = dif * klink
'If d <> TLength Then
' If d > TLength Then
' If d > TLength + RobSize Then
' kconst = dif * klink
' Else
' kconst = dif * klink ' 0.01 in other words
' End If
' Else
' If d < TLength / 2 Then
' kconst = dif * plink ' 0.1 that is
' Else
' kconst = dif * klink
' End If
' End If
'End If
dxl = (dx - (TLength * dx) / d) 'gives fractional accelerations
dyl = (dy - (TLength * dy) / d)
rob(t).ax = rob(t).ax - kconst * dxl / rob(t).mass
rob(t).ay = rob(t).ay - kconst * dyl / rob(t).mass
rob(k).ax = rob(k).ax + kconst * dxl / rob(k).mass
rob(k).ay = rob(k).ay + kconst * dyl / rob(k).mass
Xdif = (rob(t).vx - rob(t).ax) - (rob(k).vx + rob(k).ax)
Ydif = (rob(t).vy - rob(t).ay) - (rob(k).vy + rob(k).ay)
'If rob(t).Multibot And rob(t).mem(stifftie) <> 0 Then
If rob(t).mem(stifftie) > 40 Then rob(t).mem(stifftie) = 40
Stiff = 40 - rob(t).mem(stifftie)
If Abs(Xdif) > Stiff Or Abs(Ydif) > Stiff Then
'if this works then we will be normalizing tied robot's
'velocities to one another.
AvgVX = (rob(t).vx + rob(k).vx) / 2
AvgVY = (rob(t).vy + rob(k).vy) / 2
rob(t).vx = (rob(t).vx + AvgVX) / 2
rob(k).vx = (rob(k).vx + AvgVX) / 2
rob(t).vy = (rob(t).vy + AvgVY) / 2
rob(k).vy = (rob(k).vy + AvgVY) / 2
End If
'End If
If Abs(rob(t).Ties(j).last) > 1 Then
rob(t).Ties(j).last = rob(t).Ties(j).last - Sgn(rob(t).Ties(j).last)
If rob(t).Ties(j).last > 47 And rob(t).Ties(j).last < 68 Then 'should copy the memory locations 971 to 990 from parent to child. One per cycle.
If rob(t).mem(1038 - rob(t).Ties(j).last) = 0 And rob(rob(t).Ties(j).pnt).mem(1038 - rob(t).Ties(j).last) <> 0 Then
rob(t).mem(1038 - rob(t).Ties(j).last) = rob(rob(t).Ties(j).pnt).mem(1038 - rob(t).Ties(j).last)
End If
End If
End If
Select Case rob(t).Ties(j).last
Case 1
deltie t, k
Case -2
regang t, j
regang k, srctie(t, k)
End Select
'If kl Then deltie t, k
If d > 1000 Then
deltie t, k ' destroys ties that get too long
End If
End If
j = j + 1
Wend
End If
End If
Set nd = nd.pn
Wend
'If rob(t).ax > 10 Then rob(t).absvel = 100000 'debug crash inducing line
End Sub
' calculates attraction/repulsion forces due to the ties
Private Sub LinkLen3()
Dim k As Integer
Dim d As Single
Dim dx As Long
Dim dy As Long
Dim dxl As Single
Dim dyl As Single
Dim j As Integer
Dim kconst As Single
Dim nd As node
Dim t As Integer
Dim kl As Boolean
Set nd = rlist.firstnode
While Not nd Is rlist.last
j = 1
t = nd.robn
If Not rob(t).Corpse Then 'new condition added to speed up code by not parsing corpses
If rob(t).Numties > 0 Then 'new condition added to speed up code by not parsing robots without ties.
While rob(t).Ties(j).pnt > 0 'Tie points to something
k = rob(t).Ties(j).pnt 'k is the number of the robot it points to
nlink = rob(t).Ties(j).ln + rob(t).Ties(j).shrink
rob(t).Ties(j).shrink = 0
If k > 0 And k <> t Then 'doesn't point to himself?
dx = (rob(t).x - rob(k).x)
dy = (rob(t).Y - rob(k).Y)
d = Sqr(dx ^ 2 + dy ^ 2) 'inter robot distance
If d = 0 Then d = 0.01 'UUH??? prevents division by zero perhaps?
kl = False 'and again with the UUH??
If d > nlink Then
If d > mlink Then
kconst = 5 * klink
If d > nlink * 4 Then kl = True
Else
kconst = klink
End If
Else
If d < RobSize * 2 Then
kconst = plink
Else
kconst = klink
End If
End If
dxl = (dx - (nlink * dx) / d)
dyl = (dy - (nlink * dy) / d)
rob(t).ax = rob(t).ax - kconst * dxl
rob(t).ay = rob(t).ay - kconst * dyl
rob(k).ax = rob(k).ax + kconst * dxl
rob(k).ay = rob(k).ay + kconst * dyl
If Abs(rob(t).Ties(j).last) > 1 Then
rob(t).Ties(j).last = rob(t).Ties(j).last - Sgn(rob(t).Ties(j).last)
End If
Select Case rob(t).Ties(j).last
Case 1
deltie t, k
Case -2
regang t, j
regang k, srctie(t, k)
End Select
'If kl Then deltie t, k
If d > 1000 Then deltie t, k ' destroys ties that get too long
End If
j = j + 1
Wend
End If
End If
Set nd = nd.pn
Wend
'If rob(t).ax > 10 Then rob(t).absvel = 100000 'debug crash inducing line
End Sub
' calculates torque generated by all ties on robots
Public Sub Momenti()
Dim check As Single
Dim anl As Single
Dim dlo As Single
Dim dx As Long
Dim dy As Long
Dim dist As Long
Dim n As Integer
Dim nd As node
Dim t As Integer
Dim j As Integer
Dim mt As Single, mm As Single, m As Single
Dim nax As Long, nay As Long
Set nd = rlist.firstnode
While Not (nd Is rlist.last)
t = nd.robn
rob(t).mt = 0
Set nd = nd.pn
Wend
Set nd = rlist.firstnode
While Not (nd Is rlist.last)
j = 1
mt = 0
t = nd.robn
With rob(t)
If Not .Corpse Then 'condition added to prevent parsing corpses.
If .Numties > 0 Then 'condition added to prevent parsing robots without ties.
If .Ties(1).pnt > 0 Then
While .Ties(j).pnt > 0
If .Ties(j).angreg Then 'if angle is fixed.
n = .Ties(j).pnt
anl = angle(.x, .Y, rob(n).x, rob(n).Y) 'angle of tie in euclidian space
dlo = AngDiff(anl, .aim) 'difference of angle of tie and direction of robot
mm = AngDiff(dlo, .Ties(j).ang + .Ties(j).bend) 'difference of actual angle and requested angle
.Ties(j).bend = 0 'reset bend command .tieang
m = mm * 0.3
dx = rob(n).x - .x
dy = .Y - rob(n).Y
dist = Sqr(dx ^ 2 + dy ^ 2)
nax = -Sin(anl) * m * dist / 10
nay = -Cos(anl) * m * dist / 10
'experimental limits to acceleration
If Abs(nax) > 100 Then nax = 100 * Sgn(nax)
If Abs(nay) > 100 Then nay = 100 * Sgn(nax)
rob(n).ax = rob(n).ax - nax
rob(n).ay = rob(n).ay - nay
.ax = .ax + nax
.ay = .ay + nay
mt = mt + mm 'in other words mt = mm for 1 tie
If t = 10 Then
mt = mt
End If
End If
j = j + 1
Wend
'If rob(t).absvel > 10 Then rob(30000).absvel = 1000000 'crash inducing line for debugging
.ma = angnorm(mt / (j - 1))
.aim = angnorm(.aim + .ma)
.aimx = Cos(.aim)
.aimy = Sin(.aim)
'.mem(setaim) = .aim * 200
End If
End If
End If
End With
Set nd = nd.pn
Wend
End Sub
' calculates acceleration due to the medium action on the links
' (used for swimming)
Public Sub LinkForce()
Dim vxle As Long
Dim vyle As Long
Dim nd As node
Dim t As Integer, p As Integer
Dim j As Byte
Dim anle As Single, anve As Single, ancm As Single
Dim vea As Long, lle As Long
Dim cnorm As Single
Dim fx As Long, fy As Long
Set nd = rlist.firstnode
While Not (nd Is rlist.last)
t = nd.robn
With rob(t)
If .Corpse = False And .Numties > 0 Then 'new conditions to prevent parsing corpses and robots without ties.
j = 1
While .Ties(j).pnt > 0
p = .Ties(j).pnt
vxle = (.vx + rob(p).vx) / 2 'average x velocity
vyle = (.vy + rob(p).vy) / 2 'average y velocity
anle = angle(.x, .Y, rob(p).x, rob(p).Y) 'Angle between robots
anve = angle(0, 0, vxle, vyle) 'Angle of vector velocity
ancm = anve - anle 'Combined angle
vea = Sqr(vxle ^ 2 + vyle ^ 2) 'velocity along vector
lle = Sqr((.x - rob(p).x) ^ 2 + (.Y - rob(p).Y) ^ 2) 'distance between robots
cnorm = Sin(ancm) * vea * lle * SimOpts.PhysSwim 'Swim force?
fx = cnorm * Sin(anle) / 800
fy = cnorm * Cos(anle) / 800
.ax = .ax + fx
.ay = .ay + fy
rob(p).ax = rob(p).ax + fx
rob(p).ay = rob(p).ay + fy
j = j + 1
Wend
End If
End With
Set nd = nd.pn
Wend
End Sub
' updates positions, transforming calculated accelerations
' in velocities, and velocities in new positions
Public Sub updpos(ByVal n As node)
Dim t As Integer
Dim vt As Single, k As Single
Dim Reduce As Single
t = n.robn
With rob(t)
'If t <> moving And Not rob(t).Fixed Then
If t <> moving Then 'plugs loophole and velocity error
Maxspeed = 30 / (.mass / 2) 'Set maximum speed. Absolute max = 60
' friction only applied to existing velocity
k = SimOpts.PhysFriction / 10
If .Veg And Not .Multibot Then
k = 0.1 + SimOpts.PhysFriction / 5 'more friction for veggies. shielded if part of a multibot
End If
'If .Corpse Then k = 0
vt = 0.001 + Sqr(.vx ^ 2 + .vy ^ 2)
'If vt < SimOpts.PhysFriction Then
' .vx = .vx / 10 ' I think there should still be movement possible even in high friction.
' .vy = .vy / 10 ' This allows a robot to move still but only at half the speed applied by acceleration.
'Else
.vx = .vx - (.vx / vt) * k
.vy = .vy - (.vy / vt) * k
'End If
.vx = .vx + .ax
.vy = .vy + .ay
If SimOpts.Bouyancy Then
.vy = .vy - .Bouyancy
End If
' speed normalization
vt = Sqr(.vx ^ 2 + .vy ^ 2)
If vt > Maxspeed Then 'top speed limit routine 2 changed faster speeds for lower mass robots
Reduce = vt / Maxspeed
.vx = .vx / Reduce
.vy = .vy / Reduce
End If
'reset acceleration to zero at end of cycle
.ax = 0
.ay = 0
End If
colls n
bordercolls t
If Not .Fixed Then
.x = .x + .vx
.Y = .Y + .vy
End If
'this is where we will sort of sort the robot into the array
'perhaps use its old position to guess at its new position
'if it's crossed a screen boundary, we have to reset its
'position counter to something so we know we have to really go through the
'array, starting at either end
Dim temp As Integer
On Error Resume Next
'below is bubble-sortish
If .order < UBound(rob()) And .order >= 0 Then
If Roborder(.order + 1) <> -1 Then
If (.x > rob(Roborder(.order + 1)).x) Then
'move it on up.... to the east side!
'we finally got a piece of the piieeeee
While .x > rob(Roborder(.order + 1)).x
temp = Roborder(.order + 1)
Roborder(.order + 1) = t
Roborder(.order) = temp
.order = .order + 1
rob(temp).order = .order - 1
If Roborder(.order + 1) = -1 Then GoTo endloop1
Wend
End If
End If
End If
endloop1:
If .order > 0 And .order <= UBound(rob()) Then
If Roborder(.order - 1) <> -1 Then
If .x < rob(Roborder(.order - 1)).x Then
'move it down the list
While .x < rob(Roborder(.order - 1)).x
temp = Roborder(.order - 1)
Roborder(.order - 1) = t
Roborder(.order) = temp
.order = .order - 1
rob(temp).order = .order + 1
If .order = 0 Then GoTo endloop2
Wend
End If
End If
End If
endloop2:
.mem(velscalar) = Sqr(.vy ^ 2 + .vx ^ 2)
.mem(vel) = Cos(.aim) * .vx + Sin(.aim) * .vy * -1
.mem(veldn) = .mem(vel) * -1
.mem(veldx) = Sin(.aim) * .vx + Cos(.aim) * .vy
.mem(velsx) = .mem(veldx) * -1
End With
End Sub
' collisions with borders, or moves robs from one end of
' the field to the other one, if toroidal is selected
Private Sub bordercolls(t As Integer)
If rob(t).x <= 0 Then
If SimOpts.Dxsxconnected = True Then
ReSpawn t, SimOpts.FieldWidth - RobSize, rob(t).Y
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).x = 0
rob(t).vx = rob(t).vx / 2
End If
End If
If rob(t).x >= SimOpts.FieldWidth - RobSize Then
If SimOpts.Dxsxconnected Then
ReSpawn t, 0, rob(t).Y
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).x = SimOpts.FieldWidth - RobSize
rob(t).vx = rob(t).vx / 2
End If
End If
If rob(t).Y <= 0 Then
If SimOpts.Updnconnected Then
ReSpawn t, rob(t).x, SimOpts.FieldHeight - RobSize
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).Y = 0
rob(t).vy = rob(t).vy / 2
End If
End If
If rob(t).Y >= SimOpts.FieldHeight - RobSize Then
If SimOpts.Updnconnected Then
ReSpawn t, rob(t).x, 0
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).Y = SimOpts.FieldHeight - RobSize
rob(t).vy = rob(t).vy / 2
End If
End If
t = t
End Sub
' collisions with borders, or moves robs from one end of
' the field to the other one, if toroidal is selected
Private Sub bordercolls2(t As Integer) 'new code that doesn't seem to work right.
Dim x As Long
Dim Y As Long
x = rob(t).x + rob(t).vx
Y = rob(t).Y + rob(t).vy
If x < 0 Then
If SimOpts.Dxsxconnected Then
ReSpawn t, SimOpts.FieldWidth - RobSize, rob(t).Y
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).x = 0 '(RobSize + rob(t).Radius) / 2
rob(t).vx = 0 'rob(t).vx / -2
End If
End If
If rob(t).x > SimOpts.FieldWidth - (RobSize + rob(t).Radius) Then
If SimOpts.Dxsxconnected Then
ReSpawn t, 0, rob(t).Y
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).x = SimOpts.FieldWidth - (RobSize + rob(t).Radius)
rob(t).vx = 0 'rob(t).vx / -2
End If
End If
If rob(t).Y < 0 Then
If SimOpts.Updnconnected Then
ReSpawn t, rob(t).x, SimOpts.FieldHeight - RobSize
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).Y = 0 '(RobSize + rob(t).Radius) / 2
rob(t).vy = 0
End If
End If
If rob(t).Y > SimOpts.FieldHeight - RobSize Then
If SimOpts.Updnconnected Then
ReSpawn t, rob(t).x, 0
Else
rob(t).mem(214) = 1 'flags collision with edge of screen in mem location 510
rob(t).Y = SimOpts.FieldHeight - (RobSize + rob(t).Radius)
rob(t).vy = 0
End If
End If
t = t
End Sub
' calculates collisions between the robot pointed by node n
' (of the robots' sorted linked list) and other robots
' and generates accelerations
Private Sub colls(n As node)
Dim nd As node
Dim t As Integer
Dim a As Integer
Dim dist As Long
Dim tvx As Long, tvy As Long
Dim slowdown As Single
Dim angl As Single
Dim aPer As Single
Dim tPer As Single
Dim aM As Single
Dim tM As Single
Dim totM As Single
Dim colldist As Long
Dim colldistsquared As Long
Dim deltax As Long
Dim deltay As Long
Dim deltaxsquare As Long
Dim deltaysquare As Long
Dim maxdist As Integer
Dim otherrobot As Integer
Dim xpos As Long
slowdown = 0.8
On Error Resume Next
t = n.robn
xpos = n.xpos
maxdist = RobSize * 2 + Half
Set nd = rlist.firstprox(n, maxdist)
otherrobot = rob(nd.robn).order
If Roborder(otherrobot) = -1 Then Exit Sub
While rob(Roborder(otherrobot)).x < xpos + maxdist
a = Roborder(otherrobot)
If t <> a Then
colldist = RobSize + rob(t).Radius + rob(a).Radius
colldistsquared = colldist * colldist 'so we don't need the sqr
deltax = rob(t).x - rob(a).x
If Abs(deltax) > colldist Then GoTo bypass1
deltay = rob(t).Y - rob(a).Y
If Abs(deltay) > colldist Then GoTo bypass1
deltaxsquare = deltax * deltax
deltaysquare = deltay * deltay
If deltaxsquare + deltaysquare > colldistsquared Then GoTo bypass1
repel a, t
touch t, rob(a).x + Half, rob(a).Y + Half
touch a, rob(t).x + Half, rob(t).Y + Half
GoTo bypass
'End If
bypass1:
'PLEASE don't delete this
'this was Numsgil's attempt at physics
'there might still be some useful bits here and there
'deltax = deltax + rob(t).vx - rob(a).vx
'If deltax > colldist Then GoTo bypass
'
'deltay = deltay + rob(t).vy - rob(a).vy
'If deltay > colldist Then GoTo bypass
'
'deltaxsquare = deltax * deltax
'deltaysquare = deltay * deltay
'
'If deltaxsquare + deltaysquare <= colldistsquared Then
'
' dist = Sqr(deltaxsquare + deltaysquare)
' If dist = 0 Then GoTo bypass
'
' Dim deltaxsingle As Single
' Dim deltaysingle As Single
'
' deltaxsingle = deltax
' deltaysingle = deltay
'
' deltaxsingle = deltaxsingle / dist
' deltaysingle = deltaysingle / dist
'
' Dim tabx As Single
' Dim taby As Single
'
' tabx = -deltaysingle
' taby = deltaxsingle
'
' Dim vait As Single
' vait = rob(a).vx * tabx + rob(a).vy * taby
' Dim vain As Single
' vain = rob(a).vx * deltaxsingle + rob(a).vy * deltaysingle
' Dim vbit As Single
' vbit = rob(t).vx * tabx + rob(t).vy * taby
' Dim vbin As Single
' vbin = rob(t).vx * deltaxsingle + rob(t).vy * deltaysingle
' Dim ma As Single
' ma = rob(a).mass
' If rob(a).Fixed Then ma = 1000000
' Dim mb As Single
' mb = rob(t).mass
' If rob(t).Fixed Then mb = 1000000
' Dim vafn As Single
' vafn = (mb * vbin * (cof_E + 1) + vain * (ma - cof_E * mb)) / (ma + mb)
' Dim vbfn As Single
' vbfn = (ma * vain * (cof_E + 1) - vbin * (ma - cof_E * mb)) / (ma + mb)
' Dim vaft As Single
' vaft = vait
' Dim vbft As Single
' vbft = vbit
' Dim xfa As Single
' xfa = vafn * deltaxsingle + vaft * tabx
' Dim yfa As Single
' yfa = vafn * deltaysingle + vaft * taby
' Dim xfb As Single
' xfb = vbfn * deltaxsingle + vbft * tabx
' Dim yfb As Single
' yfb = vbfn * deltaysingle + vbft * taby
' If Sqr(xfa ^ 2 + yfa ^ 2) > 60 Then
' 'normalize * maxspeed
' End If
' If Sqr(xfb ^ 2 + yfb ^ 2) > 60 Then
' 'normalize * maxspeed
' End If
' rob(a).vx = xfa
' rob(a).vy = yfa
' rob(t).vx = xfb
' rob(t).vy = yfb
'End If
bypass:
End If
'Set nd = rlist.nextorder(nd)
otherrobot = otherrobot + 1
If Roborder(otherrobot) = -1 Then
Exit Sub
End If
Wend
End Sub
' gives to too near robots an accelaration towards
' opposite directions, inversely prop. to their distance
Private Sub repel(k As Integer, t As Integer)
Dim d As Single
Dim dx As Integer
Dim dy As Integer
Dim dxlk As Single
Dim dylk As Single
Dim dxlt As Single
Dim dylt As Single
Dim kconst As Single
Dim llink As Single
Dim accel As Long 'new acceleration to apply
Dim difKE As Long
Dim maxcel As Single
Dim angl1 As Single
Dim angl2 As Single
Dim colldist As Integer
Dim totx As Single 'total x velocity
Dim toty As Single 'total y velocity
Dim totv As Single 'total absolute velocity
Dim totaccel As Single
Dim decell As Single
Dim kmovx As Single
Dim kmovy As Single
Dim tmovx As Single
Dim tmovy As Single
Dim kKE As Single
Dim tKE As Single
Dim kxm As Single 'robot k x momentum
Dim kym As Single 'robot k y momentum
Dim txm As Single 'robot t x momentum
Dim tym As Single 'robot t y momentum
Dim xmean As Single 'Mean of both x momentums
Dim ymean As Single 'Mean of both y momentums
Dim totKE As Single
Dim kPer As Single 'percentage of acceleration to give to robot k based on mass
Dim tPer As Single 'same for robot t
Dim moveaway As Single 'The amount to directly move a robot away from a collision
'If xmoveaway(k, t) Then GoTo bypass2
'If ymoveaway(k, t) Then GoTo bypass2
dx = (rob(t).x - rob(k).x)
dy = (rob(t).Y - rob(k).Y)
colldist = RobSize + (rob(k).body / factor + rob(t).body / factor) 'amount of overlap based on size of robot
d = Sqr(dx ^ 2 + dy ^ 2) + 0.01 'inter-robot distance
decell = 1
maxcel = 40
'GoTo bypass2
kKE = rob(k).mass * rob(k).vx + rob(k).mass * rob(k).vy
tKE = rob(t).mass * rob(t).vx + rob(t).mass * rob(t).vy
kxm = rob(k).mass * rob(k).vx 'rob(k) x momentum signed
kym = rob(k).mass * rob(k).vy 'rob(k) y momentum signed
txm = rob(t).mass * rob(t).vx 'rob(t) x momentum signed
tym = rob(t).mass * rob(t).vy 'rob(t) y momentum signed
xmean = (Abs(kxm) + Abs(txm)) / 2 'absolute mean of x momentums. Both counted as positive
ymean = (Abs(kym) + Abs(tym)) / 2 'absolute mean of y momentums. Both counted as positive
If rob(k).mass = 0 Then rob(k).mass = 0.01
rob(k).vx = rob(k).vx + (xmean / rob(k).mass) * kxColdir(k, t) * decell
rob(t).vx = rob(t).vx + (xmean / rob(t).mass) * txColdir(k, t) * decell
rob(k).vy = rob(k).vy + (ymean / rob(k).mass) * kyColdir(k, t) * decell
rob(t).vy = rob(t).vy + (ymean / rob(t).mass) * tyColdir(k, t) * decell
Maxspeed = 30 / (rob(t).mass / 2)
totv = Sqr(rob(t).vx ^ 2 + rob(t).vy ^ 2)
If totv > Maxspeed Then 'top speed limit routine 2 changed faster speeds for lower mass robots
maxcel = totv / Maxspeed
rob(t).vx = rob(t).vx / maxcel
rob(t).vy = rob(t).vy / maxcel
End If
Maxspeed = 30 / (rob(k).mass / 2)
totv = Sqr(rob(k).vx ^ 2 + rob(k).vy ^ 2)
If totv > Maxspeed Then 'top speed limit routine 2 changed faster speeds for lower mass robots
maxcel = totv / Maxspeed
rob(k).vx = rob(k).vx / maxcel
rob(k).vy = rob(k).vy / maxcel
End If
bypass2:
'totKE = tKE + kKE 'calculates total momentum of both robots
'difKE = kKE - tKE 'difference in momentum
'tPer = rob(k).mass / (rob(k).mass + rob(t).mass) 'percentage of total momentum in bot k
'kPer = rob(t).mass / (rob(k).mass + rob(t).mass) 'percentage of total momentum in bot t
'If rob(k).vx = 0 Then rob(k).vx = 0.00001
'If rob(t).vx = 0 Then rob(t).vx = 0.00001
'mvanglt = Atn(rob(k).vy / rob(k).vx)
'mvangla = Atn(rob(t).vy / rob(t).vx)
'angledifference = angnorm(AngDiff(mvanglt, angl))
'exitangle = angnorm(angl - angledifference)
'newvx = (rob(k).vx + rob(t).vx) * (rob(t).mass / (rob(k).mass + rob(t).mass))
'newvy = (rob(k).vy + rob(t).vy) * (rob(t).mass / (rob(k).mass + rob(t).mass))
'newvx = newvx * 0.95
'newvy = newvy * 0.95
'llink = 1000
'totx = Abs(rob(k).vx) + Abs(rob(t).vx)
'totx = rob(k).vx + rob(t).vx
'toty = Abs(rob(k).vy) + Abs(rob(t).vy)
'toty = rob(k).vy + rob(t).vy
'totv = totx + toty
angl1 = angle(rob(k).x, rob(k).Y, rob(t).x, rob(t).Y) 'angle from rob k to rob t
angl2 = angle(rob(t).x, rob(t).Y, rob(k).x, rob(k).Y) 'angle from rob t to rob k
'colldist = colldist * 1.2
'dxlk = absx(angl1, totKE, 0, 0, 0) * tPer
'dylk = absy(angl1, totKE, 0, 0, 0) * tPer
'dxlt = absx(angl2, totKE, 0, 0, 0) * kPer
'dylt = absy(angl2, totKE, 0, 0, 0) * kPer
'totaccel = Abs(dxl) + Abs(dyl)
'kconst = 0.01
'dxl = (dx - (llink * dx) / d)
'dyl = (dy - (llink * dy) / d)
moveaway = (colldist - d) / 2 'move away based on half of overlap
If d < colldist Then
kmovx = absx(angl1, moveaway, 0, 0, 0)
kmovy = absy(angl1, moveaway, 0, 0, 0)
tmovx = absx(angl2, moveaway, 0, 0, 0)
tmovy = absy(angl2, moveaway, 0, 0, 0)
If Not rob(t).Fixed Then
rob(t).x = rob(t).x - tmovx
rob(t).Y = rob(t).Y - tmovy
End If
If Not rob(k).Fixed Then
rob(k).x = rob(k).x - kmovx
rob(k).Y = rob(k).Y - kmovy
End If
Else
kmovx = 0
kmovy = 0
tmovx = 0
tmovy = 0
End If
bypass3:
End Sub
Private Function kxColdir(k As Integer, t As Integer)
If rob(k).x < rob(t).x Then
kxColdir = -1
Else
kxColdir = 1
End If
End Function
Private Function kyColdir(k As Integer, t As Integer)
If rob(k).Y < rob(t).Y Then
kyColdir = -1
Else
kyColdir = 1
End If
End Function
Private Function txColdir(k As Integer, t As Integer)
If rob(t).x < rob(k).x Then
txColdir = -1
Else
txColdir = 1
End If
End Function
Private Function tyColdir(k As Integer, t As Integer)
If rob(t).Y < rob(k).Y Then
tyColdir = -1
Else
tyColdir = 1
End If
End Function
Private Function xmoveaway(k As Integer, t As Integer) As Boolean
If Sgn(rob(k).vx) = Sgn(rob(t).vx) Then 'both moving the same way
If rob(k).x < rob(t).x Then 'rob(k) is to the left
If Sgn(rob(k).vx) = 1 Then 'rob(k) moving to the right
If rob(k).vx < rob(t).vx Then 'rob(k) moving slower than rob(t)
xmoveaway = True 'moving away
Else
xmoveaway = False 'not moving away
End If
Else 'rob(k) moving to the left
If rob(k).vx > rob(t).vx Then 'rob(k) moving faster than rob(t)
xmoveaway = True 'moving away
Else
xmoveaway = False 'not moving away
End If
End If
Else 'rob(k) is NOT to the left (right or level)
If Sgn(rob(k).vx) = 1 Then 'rob(k) moving to the right
If rob(k).vx > rob(t).vx Then 'rob(k) moving faster than rob(t)
xmoveaway = True 'moving away
Else
xmoveaway = False 'not moving away
End If
Else 'rob(k) moving to the left
If rob(k).vx < rob(t).vx Then 'rob(k) moving faster than rob(t)
xmoveaway = True 'moving away
Else
xmoveaway = False 'not moving away
End If
End If
End If
Else 'robots moving opposite directions
If rob(k).x < rob(t).x Then 'rob(k) is to the left
If Sgn(rob(k).vx) = 1 Then 'rob(k) moving to the right
xmoveaway = False
Else 'rob(k) moving to the left
xmoveaway = True
End If
Else 'rob(k) is to the right or level
If Sgn(rob(k).vx) = 1 Then 'rob(k) moving to the right
xmoveaway = True 'must be moving away
Else 'rob(k) moving to the left
xmoveaway = False 'must be moving towards
End If
End If
End If
End Function
Private Function ymoveaway(k As Integer, t As Integer) As Boolean
If Sgn(rob(k).vy) = Sgn(rob(t).vy) Then 'both moving the same way
If rob(k).Y < rob(t).Y Then 'rob(k) is to the top
If Sgn(rob(k).vy) = 1 Then 'rob(k) moving to the bottom
If rob(k).vy < rob(t).vy Then 'rob(k) moving slower than rob(t)
ymoveaway = True 'moving away
Else
ymoveaway = False 'not moving away
End If
Else 'rob(k) moving to the left
If rob(k).vy > rob(t).vy Then 'rob(k) moving faster than rob(t)
ymoveaway = True 'moving away
Else
ymoveaway = False 'not moving away
End If
End If
Else 'rob(k) is NOT to the left (right or level)
If Sgn(rob(k).vy) = 1 Then 'rob(k) moving to the bottom
If rob(k).vy > rob(t).vy Then 'rob(k) moving faster than rob(t)
ymoveaway = True 'moving away
Else
ymoveaway = False 'not moving away
End If
Else 'rob(k) moving to the top
If rob(k).vy < rob(t).vy Then 'rob(k) moving faster than rob(t)
ymoveaway = True 'moving away
Else
ymoveaway = False 'not moving away
End If
End If
End If
Else 'robots moving opposite directions
If rob(k).Y < rob(t).Y Then 'rob(k) is to the top
If Sgn(rob(k).vy) = 1 Then 'rob(k) moving to the bottom
ymoveaway = False
Else 'rob(k) moving to the top
ymoveaway = True
End If
Else 'rob(k) is to the right or level
If Sgn(rob(k).vy) = 1 Then 'rob(k) moving to the bottom
ymoveaway = True 'must be moving away
Else 'rob(k) moving to the top
ymoveaway = False 'must be moving towards
End If
End If
End If
End Function
