Im actually in a project in the university about robot, and im developing a robotic simulator, which programmers can test their robotics algorithms before test on a real robot which is too risk.
I started working with the Roamming Ralph model of program, I made my own model of robot(who is a car-like robot), and I loaded the roaming ralph world example to start working around the code. I made it walk, introduced a socket, but actually my problem is Rotating according to the ground, the uneven world of ralph roaming was the best map that I had to start with it.
I though too much and I conclude that the rotation with ground code come with the collision check, exactly the same place where we set the Z position of the robot with the Z position of the ground. I made a lot of calculation to get the angle rotation of terrain based on Surface normal, but all I got is a vec3 that is normalized and I found the exactly angle of about the vector (0,0,1) which is orthogonal to the world.
I found the angle, but i dont know how to split it in Hpr, not H, just P and R, because H is controlled with the arrows…
Someone have some idea how do I solve this problem?
I know that SurfaceNormal exists, I know that I can find his angle with by using an vec3 method that returns the angle between 2 vectors, but I really dont know how to calculate the rotation, coz this angle, i can split in P and R rotation, so maybe there is some mathematical formula that I dont know, or perhaps the engine by itself gives a fast solution.
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