2D Car Controller for BGE

car.py

from bge import logic, types, events, render
from mathutils import Vector
import math

def lerp(a, b, t):
	return (1-t) * a + b * t


class Car(types.KX_GameObject):
	def __init__(self, ob):
		self.__force = Vector((0, 0))
		self.heading = 0
		self.steer = 0
		self.speed = 0
		self.braking = 0
		self.wheelBase = 3
		
		self.maxSpeed = 80
		self.maxSteer = 1.15
		self.maxStickSpeed = 1.5
		self.driftFactorSlip = 0.7
		self.driftFactorStick = 0.8
		self.maxBrake = 40
		
	def forwardVelocity(self):
		fwd = self.getAxisVect(Vector((1, 0, 0))).normalized()
		return fwd * self.linearVelocity.dot(fwd)
	
	def rightVelocity(self):
		rgt = self.getAxisVect(Vector((0, -1, 0))).normalized()
		return rgt * self.linearVelocity.dot(rgt)
	
	def turnLeft(self, force=1.0):
		self.steer += self.maxSteer * force
	
	def turnRight(self, force=1.0):
		self.steer += -self.maxSteer * force

	def accelerate(self, force=1.0):
		self.speed += self.maxSpeed * force

	def reverse(self, force=1.0):
		self.speed += -self.maxSpeed * force
	
	def brake(self, force=1.0):
		self.braking = self.maxBrake * force

	def update(self):
#		K = logic.keyboard.events
#		
#		if K[events.WKEY]:
#			self.speed = self.maxSpeed
#		elif K[events.SKEY]:
#			self.speed = -self.maxSpeed

#		if K[events.AKEY]:
#			self.steer = self.maxSteer
#		elif K[events.DKEY]:
#			self.steer = -self.maxSteer
		
		df = self.driftFactorStick
		if self.rightVelocity().length > self.maxStickSpeed:
			df = self.driftFactorSlip
		
		F = self.speed * Vector((1, 0, 0))
		self.applyForce(F, True)
		B = self.braking * Vector((-1, 0, 0))
		self.applyForce(B, True)
		
		steerF = lerp(0, self.steer, self.linearVelocity.length / 10)
		self.angularVelocity.z = lerp(self.angularVelocity.z, steerF, 0.8)
		
		V = self.forwardVelocity() + self.rightVelocity() * df
		self.applyForce(-V, True)
		
		self.steer *= 0.2
		self.speed = 0


class CarAI(Car):
	def __init__(self, ob):
		super(CarAI, self).__init__(ob)
		
		self.lap = 1
		self.progress = 0
		self.nextWaypoint = 0
		self.lastWaypoint = -1
		self.nextLerpedWaypoint = Vector((0, 0, 0))
		self.accelFactor = 1.0
		
		self.__way_max_dist = 8

		debug.init()

	def get_position(self):
		cars = [ob for ob in self.scene.objects if "Car" in ob]
		cars.sort(key=lambda x: x.progress, reverse=True)
		return cars.index(self)

	def update(self):
		waypointsOB = [ob for ob in self.scene.objects if ob.name.startswith("WAYPOINT")]
		waypointsOB.sort(key=lambda x: x.name)
		
		waypoints = [Vector((ob.position.x, ob.position.y, 0)) for ob in waypointsOB]

		if len(waypoints) > 0:
			mypos = Vector((self.position.x, self.position.y, 0))
			lastWaypoint = waypoints[self.lastWaypoint]
			nextWaypoint = waypoints[self.nextWaypoint]
			
			## Lerp the next way point position to avoid flickering and
			## give a nicer behaviour
			if self.nextLerpedWaypoint.length <= 0:
				self.nextLerpedWaypoint = nextWaypoint
			else:
				self.nextLerpedWaypoint = self.nextLerpedWaypoint.lerp(nextWaypoint, 0.025)

			myright = self.getAxisVect(Vector((0, 1, 0))).normalized()
			
			## Track waypoints			
			self.diff = (self.nextLerpedWaypoint - mypos)
			angle = myright.dot(self.diff.normalized())
						
			if angle < 0:
				self.turnRight()
			elif angle > 0:
				self.turnLeft()
			
			sharp_turn = abs(angle) > 0.45
			straight = abs(angle) <= 0.05
			
			## Don't go too fast on sharp turns
			if sharp_turn:
				self.accelFactor = lerp(self.accelFactor, 0.7, 0.1)
			elif straight: ## Accelerate a little bit more on straight lines
				self.accelFactor = lerp(self.accelFactor, 1.05, 0.1)
			else:
				self.accelFactor = lerp(self.accelFactor, 1.0, 0.1)
			self.accelerate(self.accelFactor)

			## It's approaching a way point, so slow down a bit
			D = (mypos - nextWaypoint).length
			if D <= self.__way_max_dist*2:
				self.brake(0.6)
			
			## Go to the next way point
			if D <= self.__way_max_dist:
				self.lastWaypoint = self.nextWaypoint
				self.nextWaypoint = waypoints.index(nextWaypoint)+1
								
				if self.nextWaypoint > len(waypoints)-1:
					self.nextWaypoint = 0
					self.lap += 1

			## Calculate current progress of this car
			displacement = mypos - lastWaypoint
			segment = nextWaypoint - lastWaypoint
			lsqr = segment.length**2
			fraction = (displacement.dot(segment) / lsqr)+(self.__way_max_dist/segment.length)
			self.progress = fraction + (self.lastWaypoint+1) + self.lap * 10

		Car.update(self)


def main(cont):
	o = cont.owner
	if "init" not in o:
		o = CarAI(o)
		o["init"] = 1
	else:
		o.update()