I have some progress right here.
def checkCollisions(self):
self.cTrav.traverse(self.render)
entries = []
for i in range(self.roboGroundHandler.getNumEntries()):
entry = self.roboGroundHandler.getEntry(i)
entries.append(entry)
entries.sort(lambda x,y: cmp(y.getSurfacePoint(self.render).getZ(),
x.getSurfacePoint(self.render).getZ()))
if (len(entries)>0) and (entries[0].getIntoNode().getName() == "terrain"):
self.robo.setZ(entries[0].getSurfacePoint(self.render).getZ()+0.40)
#Calcula a rotacao no eixo X
b = entries[0].getSurfaceNormal(self.render).getX() / fabs(entries[0].getSurfaceNormal(self.render).getX())
#b=b*-1
p = fabs(self.robo.getH() % 360)
hP = 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(0,1,0))
hR = 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(1,0,0))
if (p >= 0 and p < 90):
self.robo.setP(hP)
self.robo.setR(hR)
if (p >= 90 and p < 180):
self.robo.setR(hR)
self.robo.setP(hP)
if (p >= 180 and p < 270):
self.robo.setP(360 - hP)
self.robo.setR(360 - hR)
if (p >= 270 and p <= 359):
self.robo.setR(360 - hR)
self.robo.setP(360 - hP)
print self.robo.getHpr()
print 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(0,1,0))
print 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(1,0,0))
else:
self.robo.setPos(self.bpos)
This is my collision function, some part of it extracted from the roaming ralph example, the rotation is going good but still need some improved