Hi!
I’ve made a simple car game in panda3D. Right now im working with some AI for the cars driving against me. I have a path with waypoints that the AI-cars should follow. The way i do that is shown in the code below.
My problem is that i just can’t get the AI-car to follow the waypoints correctly. When its near the first waypoint it just drive around in a circle. Can you figure out what i am doing wrong in the code? Hints are also welcome.
#automaticcontrol.py
import driveinterface
import Factory
from pandac.PandaModules import *
from direct.interval.IntervalGlobal import *
import cmath
class AIControl(driveinterface.DriveInterface):
def __init__(self):
driveinterface.DriveInterface.__init__(self)
def doPhysics(self,task):
self.doAI()
pos = self.vehicle.getPos()
R=Mat3.rotateMat(self.vehicle.getH()+90)
v = Vec3(0,1,0)
v = R.xform(v)
self.vehicle.setPos(pos + v * self.vehicle.throttle)
#if self.vehicle.throttle > 0:
# self.vehicle.setH(self.vehicle.getH()+self.vehicle.turn)
#else:
# self.vehicle.setH(self.vehicle.getH())
return Task.cont
def doAI(self):
#calculate what to do referring to the current location.
#Steer the car using self.vehicle.right(),brake(),left(), drive()
#etc.
# waypoints
a =Point3(-41.5200386047 ,-2.4629440308,0)
b =Vec3(-61.8333015442 ,26.2737121582,0)
c =Vec3(-128.173110962 ,45.6150665283,0)
d =Vec3(-151.851867676 ,64.0680847168,0)
e =Vec3(-159.6615448 ,84.8736724854,0)
f =Vec3(-159.917602539 ,108.900344849,0)
g =Vec3(-153.291610718 ,120.254852295,0)
h =Vec3(-138.410324097 ,125.322944641,0)
i =Vec3(-113.70740509 ,124.90096283,0)
j =Vec3(-78.744430542 ,120.945602417,0)
k =Vec3(-52.6699905396 ,118.700958252,0)
l =Vec3(-26.1722412109 ,120.467704773,0)
m =Vec3(-8.09318733215 ,125.630355835,0)
n =Vec3(18.0730571747 ,143.843185425,0)
o =Vec3(41.2357902527 ,177.912857056,0)
p =Vec3(62.5629272461 ,206.668563843,0)
q =Vec3(95.6246337891 ,215.226669312,0)
r =Vec3(116.539146423 ,186.554489136,0)
s =Vec3(102.333610535 ,122.015609741,0)
t =Vec3(77.2887420654 ,70.7077102661,0)
u =Vec3( 38.4174346924,35.4624137878,0)
self.WPnumber=0
# waypoints list
self.waypoint = [b,c,d,e,f,g,h,i,j,k,l,m,n,o,p,q,r,s,t,u]
h= ((self.vehicle.getH()-90)*math.pi/180)
angle = self.CalculateAngle(self.vehicle.getPos(), self.waypoint[self.WPnumber] )
self.steerCar(angle,self.vehicle)
# check if the car has reached its waypoint if(self.waypointReached(self.waypoint[self.WPnumber],self.vehicle)==1):
self.WPnumber=self.WPnumber+1
#print self.WPnumber
#print "(x,y)" + str(self.vehicle.getX())+","+ str(self.vehicle.getY())
def waypointReached(self,way, z):
xdif=(self.vehicle.getX()-way[0])*(self.vehicle.getX()-way[0])
ydif=(self.vehicle.getY()-way[1])*(self.vehicle.getY()-way[1])
xydif=xdif+ydif
distance=cmath.sqrt(xydif)
if(abs(distance) < 5):
return 1
return 0;
def steerCar(self,angle, y):
self.vehicle.throttle=0.07
if self.waypoint[self.WPnumber][2] > 0:
self.vehicle.setH(self.vehicle.getH() + (abs(angle)/(180/math.pi)))
else:
self.vehicle.setH(self.vehicle.getH() - (abs(angle)/(180/math.pi)))
def CalculateAngle(self,a, b):
# vector set to the orientation of the ai car
R=Mat3.rotateMat(self.vehicle.getH()+90)
v = Vec3(0,1,0)
v = R.xform(v)
v=Vec3(v[0],v[1],v[2])
#current position of the aicar
carvector = Vec3(a.getX(),a.getY(),a.getZ())
#The current Waypoint vector
wpvector =b
# calculate the vector from the ai current position to the next waypoint
x = (wpvector[0] - carvector[0])
y = (wpvector[1] - carvector[1])
aiToWaypoint = Vec3 (x,y,0)
# calculate the dot product for the direction vector of the car and
# the vector to waypoint. This will get output the angle between the to
# vectors
lengthV = v.length()
lengthWP = aiToWaypoint.length()
tmp = v.dot(aiToWaypoint)/(lengthV*lengthWP)
angleRad = (cmath.acos(tmp))
angleDeg = angleRad * (180/cmath.pi)
#print angleDeg
return angleDeg
def createAIControl():
return AIControl()
Factory.Instance.Register("AIControl",createAIControl)Thanks…