def global_space(L, oL, oR):
oR = a2(oR) * pi / 180
tL = a3([0, 0, 0])
tL[0], tL[2] = rotate2D(L[0], L[2], oR[1])
tL[1], tL[2] = rotate2D(L[1], tL[2], oR[0])
tL = a3(tL) + a3(oL)
return tL
def predict_CarSim(L0, V0, dt, ept=1 / 120, g=GRAVITY, cf=CAR_FRICTION):
st = CarState(Vector3(*L0), Vector3(*V0))
pt = []
for i in range(int(dt / ept)):
st = PhyLib.carStep(st, ept, g, cf) # calling the step function
pt.append([a3(st.Location), a3(st.Velocity)])
# if dt%ept>0:
st = PhyLib.carStep(st, dt % ept, g, cf)
pt.append([a3(st.Location), a3(st.Velocity)])
return pt
def approx_step(L0, V0, dt):
g = a3([0, 0, gc]) * (d3(V0) > 0)
A = g - r * V0
nV = V0 + A * dt
nL = L0 + V0 * dt + .5 * A * dt**2
return nL, nV
def local_space(tL, oL, oR):
L = a3(tL) - a3(oL)
oR = a2(oR) * pi / 180
y, z = rotate2D(L[1], L[2], -oR[0])
x, z = rotate2D(L[0], z, -oR[1])
return x, y, z
def step(L0, V0, aV0, dt):
g = a3([0, 0, gc]) * (d3(V0) > 0)
# don't apply gravity if the ball is floating
A = g - r * V0
nV = V0 + A * dt
nL = L0 + V0 * dt + .5 * A * dt**2
naV = aV0
if not CollisionFree(nL):
Cl = Collision_R(nL)
if Cl[0] == True:
# transorforming velocities to local space
xv, yv, zv = local_space(V0, [0, 0, 0], Cl[1])
xav, yav, zav = local_space(aV0, [0, 0, 0], Cl[1])
# if rolling
if abs(zv) < 50:
total_v2 = d2([xv, yv])
if total_v2 > 565:
# sliding friction
sf = abs(1 - 1500 * dt / total_v2)
xv, yv = sf * a2([xv, yv])
else:
# bounce angle
ang = abs(math.atan2(zv, math.sqrt(xv**2 + yv**2))) / pi * 180
# some more magic numbers
e = (e1 - 1) / (28) * ang + 1
# limiting e to range [e1, 1]
if e < e1: e = e1
# bounce calculations
xv, yv = (xv + yav * R * a) * e, (yv - xav * R * a) * e
zv = abs(zv) * e2 + 5
xav, yav = -yv / R, xv / R
# limiting ball spin
total_av = math.sqrt(xav**2 + yav**2 + zav**2)
if total_av > 6:
xav, yav, zav = 6 * xav / total_av, 6 * yav / total_av, 6 * zav / total_av
# transorforming velocities back to global/world space
nV = global_space([xv, yv, zv], [0, 0, 0], Cl[1])
naV = global_space([xav, yav, zav], [0, 0, 0], Cl[1])
# redoing the step with the new velocity
nL = nL + nV * dt + (-r * nV) * dt**2
if L0[2] > R: nL += g * dt**2
# limiting ball speed
total_v = d3(nV)
if total_v > 6000:
nV[0], nV[1], nV[
2] = 6 * nV[0] / total_v, 6 * nV[1] / total_v, 6 * nV[2] / total_v
return nL, nV, naV
def state_ca3(st): # nested numpy array
return [a3(st.Location), a3(st.Velocity)]
def state_ba3(st): # nested numpy array
return [a3(st.Location), a3(st.Velocity), a3(st.AngularVelocity)]