def processActions(self, deltaT):
for c in self.gs.characters:
# calculate character's required acceleration for the desired velocity
acceleration = c.getAction().getDesiredAcceleration(c.maxSpeed) - c.getVelocity()
# calculate required force from the acceleration
force = vec.clampMaxLength(vec.scale(acceleration, c.mass, self.n), c.maxForce, self.t)
# re-calculate acceleration for new (possibly) clamped force
c.setVelocity(vec.clampMaxLength(c.getVelocity() + vec.scale(force, deltaT/c.mass, self.n), c.maxSpeed, self.t))
def processActions(self, deltaT):
for c in self.gs.characters:
# calculate character's required acceleration for the desired velocity
acceleration = c.getAction().getDesiredAcceleration(c.maxSpeed) - c.getVelocity()
# calculate required force from the acceleration
force = vec.clampMaxLength(vec.scale(acceleration, c.mass, self.n), c.maxForce, self.t)
# re-calculate acceleration for new (possibly) clamped force
c.setVelocity(vec.clampMaxLength(c.getVelocity() + vec.scale(force, deltaT/c.mass, self.n), c.maxSpeed, self.t))
def drawArrow(begin, lvec, lineSize):
assert util.isAlmostEq(1.0, vec.length(lvec)), "unnormalized input"
headSize = 0.5 * lineSize
lAngle = 2.5
rAngle = -2.5
# TODO: get rid of all these constructor calls
hvec1 = zeros((util.numDim, ), float)
hvec2 = zeros((util.numDim, ), float)
tmp = zeros((util.numDim, ), float)
hvec1 = vec.scale(
array([
lvec[0] * math.cos(lAngle) - lvec[1] * math.sin(lAngle),
lvec[0] * math.sin(lAngle) + lvec[1] * math.cos(lAngle)
]), headSize, hvec1)
hvec2 = vec.scale(
array([
lvec[0] * math.cos(rAngle) - lvec[1] * math.sin(rAngle),
lvec[0] * math.sin(rAngle) + lvec[1] * math.cos(rAngle)
]), headSize, hvec2)
tmp = vec.scale(lvec, lineSize, tmp)
rightWay = True
glPushMatrix()
if rightWay:
glTranslate(begin[0], begin[1], 0.0)
else:
glTranslate(begin[0] - tmp[0], begin[1] - tmp[1], 0.0)
glBegin(GL_LINE_STRIP)
glVertex(0.0, 0.0)
glVertex(tmp[0], tmp[1])
glVertex(tmp[0] + hvec1[0], tmp[1] + hvec1[1])
glEnd()
glPopMatrix()
glPushMatrix()
if rightWay:
glTranslate(begin[0] + tmp[0], begin[1] + tmp[1], 0.0)
else:
glTranslate(begin[0], begin[1], 0.0)
glBegin(GL_LINES)
glVertex(0.0, 0.0)
glVertex(hvec2[0], hvec2[1])
glEnd()
glPopMatrix()
def render(self, c):
RendererCircle.render(self, c)
start = c.getPosition() + vec.scale(c.getOrientation(), c.shape.radius,
self.tmp)
gui.drawArrow(start, c.getOrientation(),
1.5 * c.shape.getRadius() * (c.getSpeed() / c.maxSpeed))
def nearestIntersection(c0, c1, q):
p = c1.position
v = c1.orientation
rp = p - c0.position
k0 = vec.lengthSq(rp) - c0.getRadius() * c0.getRadius()
k1 = vec.dot(v, rp)
roots = []
k = k1 * k1 - k0
if util.isAlmostZero(k):
roots.append(-k1)
elif 0.0 < k:
kSqrt = math.sqrt(k)
roots.append(-k1 - kSqrt)
roots.append(-k1 + kSqrt)
assert roots[0] < roots[1]
vec.set(Inf, q)
for root in roots:
if util.isAlmostZero(root) or 0 < root:
q = vec.scale(v, root, q)
q = q + p
break
return q
def nearestIntersection(c0, c1, q):
p = c1.position
v = c1.orientation
rp = p - c0.position
k0 = vec.lengthSq(rp) - c0.getRadius() * c0.getRadius()
k1 = vec.dot(v, rp)
roots = []
k = k1 * k1 - k0
if util.isAlmostZero(k):
roots.append(-k1)
elif 0.0 < k:
kSqrt = math.sqrt(k)
roots.append(-k1 - kSqrt)
roots.append(-k1 + kSqrt)
assert roots[0] < roots[1]
vec.set(Inf, q)
for root in roots:
if util.isAlmostZero(root) or 0 < root:
q = vec.scale(v, root, q)
q = q + p
break
return q
def balls_collisions(balls):
collided = False
for b1 in balls:
for b2 in balls:
if b1 is b2:
continue
d = vec.sub(b1.pos, b2.pos)
penetration = (b1.radius + b2.radius) - vec.len(d)
if penetration < 0:
continue
collided = True
n = vec.unit(d)
b1.pos = vec.add(b1.pos, vec.scale(n, penetration / 2 + 1))
b2.pos = vec.add(b2.pos, vec.scale(n, -penetration / 2 - 1))
j = vec.dot(vec.sub(b2.speed, b1.speed), n)
b1.speed = vec.add(b1.speed, vec.scale(n, j))
b2.speed = vec.add(b2.speed, vec.scale(n, -j))
return collided
def render(self, c):
RendererCircle.render(self, c)
start = c.getPosition() + vec.scale(c.getOrientation(), c.shape.radius, self.tmp)
gui.drawArrow(start, c.getOrientation(), 1.5 * c.shape.getRadius() * (c.getSpeed()/c.maxSpeed))
def resolveCollisions(self):
e = 0.75 # coefficient of restitution
minTagInterval = 3.0 # minimum time allowed between re-tagging
now = timer.getTime()
loopCount = 0
while True:
isCollision = False
for i in self.gs.characters:
for j in self.gs.characters:
if i == j:
continue
if not i.isColliding(j):
continue
isCollision = True
# We have to keep computing these in case they changed in a previous collision
uc = i.getVelocity()
mc = i.mass
self.t = i.normalTo(j, self.t)
self.n = util2D.perpendicularTo(self.t, self.n, self.nTmp)[0]
uct = dot(uc, self.t)
ucn = dot(uc, self.n)
mo = j.mass
uo = j.getVelocity()
uot = dot(uo, self.t)
uon = dot(uo, self.n)
k = (uct - uot)/(mc + mo)
vct = uct - (1 + e) * mo * k
vot = uot + (1 + e) * mc * k
i.setVelocity(vec.scale(self.t, vct, self.vt) + vec.scale(self.n, ucn, self.vn))
j.setVelocity(vec.scale(self.t, vot, self.vt) + vec.scale(self.n, uon, self.vn))
# TODO: limit re-tagging restriction to previously tagged character only
if j.tagged and minTagInterval < now - self.gs.lastTagTime:
self.gs.setTagged(i, j, now)
elif i.tagged and minTagInterval < now - self.gs.lastTagTime:
self.gs.setTagged(j, i, now)
for j in self.gs.nonCharacterObstacles:
if not i.isColliding(j):
continue
isCollision = True
uc = i.getVelocity()
self.t = i.normalTo(j, self.t)
self.n = util2D.perpendicularTo(self.t, self.n, self.nTmp)[0]
uct = dot(uc, self.t)
ucn = dot(uc, self.n)
vct = -e * uct
i.setVelocity(vec.scale(self.t, vct, self.vt) + vec.scale(self.n, ucn, self.vn))
if not isCollision:
break
assert loopCount < 1000, "something probably went wrong"
loopCount += 1
def resolveCollisions(self):
e = 0.75 # coefficient of restitution
minTagInterval = 3.0 # minimum time allowed between re-tagging
now = timer.getTime()
loopCount = 0
while True:
isCollision = False
for i in self.gs.characters:
for j in self.gs.characters:
if i == j:
continue
if not i.isColliding(j):
continue
isCollision = True
# We have to keep computing these in case they changed in a previous collision
uc = i.getVelocity()
mc = i.mass
self.t = i.normalTo(j, self.t)
self.n = util2D.perpendicularTo(self.t, self.n, self.nTmp)[0]
uct = dot(uc, self.t)
ucn = dot(uc, self.n)
mo = j.mass
uo = j.getVelocity()
uot = dot(uo, self.t)
uon = dot(uo, self.n)
k = (uct - uot)/(mc + mo)
vct = uct - (1 + e) * mo * k
vot = uot + (1 + e) * mc * k
i.setVelocity(vec.scale(self.t, vct, self.vt) + vec.scale(self.n, ucn, self.vn))
j.setVelocity(vec.scale(self.t, vot, self.vt) + vec.scale(self.n, uon, self.vn))
# TODO: limit re-tagging restriction to previously tagged character only
if j.tagged and minTagInterval < now - self.gs.lastTagTime:
self.gs.setTagged(i, j, now)
elif i.tagged and minTagInterval < now - self.gs.lastTagTime:
self.gs.setTagged(j, i, now)
for j in self.gs.nonCharacterObstacles:
if not i.isColliding(j):
continue
isCollision = True
uc = i.getVelocity()
self.t = i.normalTo(j, self.t)
self.n = util2D.perpendicularTo(self.t, self.n, self.nTmp)[0]
uct = dot(uc, self.t)
ucn = dot(uc, self.n)
vct = -e * uct
i.setVelocity(vec.scale(self.t, vct, self.vt) + vec.scale(self.n, ucn, self.vn))
if not isCollision:
break
assert loopCount < 1000, "something probably went wrong"
loopCount += 1
def updateGameState(self, deltaT):
for c in self.gs.characters:
c.setPosition(vec.wrap(c.getPosition() + vec.scale(c.getVelocity(), deltaT, self.t), self.gs.worldDim, self.n))
c.setActualVelocity(c.getVelocity())
def getVelocity(self):
# TODO: cache to avoid recomputing
return vec.scale(self.shape.orientation, self.speed, self.velocity)
def integrate(bs):
for b in bs:
b.speed = vec.add(b.speed, b.forces)
b.speed = vec.scale(b.speed, 0.99)
b.pos = vec.add(b.pos, b.speed)
def updateGameState(self, deltaT):
for c in self.gs.characters:
c.setPosition(vec.wrap(c.getPosition() + vec.scale(c.getVelocity(), deltaT, self.t), self.gs.worldDim, self.n))
def getVelocity(self):
# TODO: cache to avoid recomputing
return vec.scale(self.shape.orientation, self.speed, self.velocity)
pos = vec.vec(400, 300)
speed = vec.vec(0, 0)
while True:
screen.fill((0, 0, 0))
pygame.event.pump()
if pygame.event.peek(pygame.QUIT):
break
keys = pygame.key.get_pressed()
forces = vec.vec(0, 0)
if keys[pygame.K_LEFT]:
forces.x -= 1
if keys[pygame.K_RIGHT]:
forces.x += 1
if keys[pygame.K_UP]:
forces.y -= 1
if keys[pygame.K_DOWN]:
forces.y += 1
speed = vec.add(speed, forces)
speed = vec.scale(speed, 0.95)
pos = vec.add(pos, speed)
pygame.draw.circle(screen, (255, 255, 255), (int(pos.x), int(pos.y)), 50)
pygame.display.flip()
time.sleep(1 / 60)
def getDesiredAcceleration(self, maxSpeed):
return vec.scale(self.direction, self.speed * maxSpeed, self.tmp)
def getDesiredAcceleration(self, maxSpeed):
return vec.scale(self.direction, self.speed * maxSpeed, self.tmp)
