def rollTask(self, task):
dt = globalClock.getDt()
# if dt is large, then there has been a # hiccup that could cause the ball to leave the field if this functions runs, so ignore the frame
if dt > .2:
return Task.cont
# collision handler
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
name = entry.getIntoNode().getName()
if name == "wall_collide":
self.wallCollideHandler(entry)
elif name == "ground_collide":
self.groundCollideHandler(entry)
# move the ball
# update the velocity based on acceleration
self.ballV += self.accelV * dt * ACCEL
# prevent velocity from going above max velocity
if self.ballV.lengthSquared() > MAX_SPEED_SQ:
self.ballV.normalize()
self.ballV *= MAX_SPEED
# update the position based on the velocity
self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
# rotate the ball
prevRot = LRotationf(self.ball.getQuat())
axis = LVector3.up().cross(self.ballV)
newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
self.ball.setQuat(prevRot * newRot)
return Task.cont
def start(self):
#startPos = self.maze.find("**/start").getPos()
#self.ballRoot.setPos(0.5, 0, 0)
#self.ballV = LVector3(0, 0.5, 0) # Initial velocity is 0
#self.accelV = LVector3(0, 0, 0) # Initial acceleration is 0
self.ballVs = []
self.accelVs = []
for ballIndex in xrange(len(self.balls)):
self.ballVs.append(LVector3(0, 0, 0))
self.accelVs.append(LVector3(0, 0, 0))
continue
self.ballRoots[0].setPos(0.2, 1.05, -0.1)
#self.ballVs[0] = LVector3(0, 0.0, 0)
self.ballRoots[1].setPos(0.32, 1.2, -0.1)
#self.ballRoots[2].setHpr(0, 0, 90)
self.ballRoots[2].setPos(-0.4, 1.1, 0.4)
axis = LVector3.up()
prevRot = LRotationf(self.balls[2].getQuat())
newRot = LRotationf(axis, 90)
self.balls[2].setQuat(prevRot * newRot)
# Create the movement task, but first make sure it is not already
# running
taskMgr.remove("rollTask")
#taskMgr.remove("mouseTask")
self.mainLoop = taskMgr.add(self.rollTask, "rollTask")
#self.mainLoop = taskMgr.add(self.mouseTask, "mouseTask")
return
def rollTask(self, task):
# Standard technique for finding the amount of time since the last frame
dt = task.time - task.last
task.last = task.time
# If dt is large, then there has been a # hiccup that could cause the ball
# to leave the field if this functions runs, so ignore the frame
if dt > .2: return Task.cont
# The collision handler collects the collisions. We dispatch which function
# to handle the collision based on the name of what was collided into
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
name = entry.getIntoNode().getName()
if name == "wall_collide": self.wallCollideHandler(entry)
elif name == "ground_collide": self.groundCollideHandler(entry)
elif name == "loseTrigger": self.loseGame(entry)
# Read the mouse position and tilt the maze accordingly
#if base.mouseWatcherNode.hasMouse():
# mpos = base.mouseWatcherNode.getMouse() # get the mouse position
linha = arduino.readline()
if linha != '':
ax, ay, nx, ny = arduino.readline().split(',') # 70 - 182
print ax, ay
ax = float(ax)
ay = float(ay)
rato.y = translate(ay, 0, 255, -1, 1)
rato.x = translate(ax, 0, 255, -1, 1)
self.maze.setR(rato.x * 10)
self.maze.setP(rato.y * -10)
#if base.mouseWatcherNode.hasMouse():
# mpos = base.mouseWatcherNode.getMouse() # get the mouse position
# print mpos.getY(), mpos.getX()
# self.maze.setP(mpos.getY() * -10)
# self.maze.setR(mpos.getX() * 10)
# Finally, we move the ball
# Update the velocity based on acceleration
self.ballV += self.accelV * dt * ACCEL
# Clamp the velocity to the maximum speed
if self.ballV.lengthSquared() > MAX_SPEED_SQ:
self.ballV.normalize()
self.ballV *= MAX_SPEED
# Update the position based on the velocity
self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
# This block of code rotates the ball. It uses something called a quaternion
# to rotate the ball around an arbitrary axis. That axis perpendicular to
# the balls rotation, and the amount has to do with the size of the ball
# This is multiplied on the previous rotation to incrimentally turn it.
prevRot = LRotationf(self.ball.getQuat())
axis = UP.cross(self.ballV)
newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
self.ball.setQuat(prevRot * newRot)
return Task.cont # Continue the task indefinitely
def rollTask(self, task):
# Get the most recent frame
frame = self.leap.frame()
hands = frame.hands
numHands = len(hands)
if numHands >= 1:
# Get the normal vector of the first hand
hand = hands[0]
handNormal = hand.palm_normal
if handNormal is not None:
# Standard technique for finding the amount of time since the last frame
dt = task.time - task.last
task.last = task.time
# If dt is large, then there has been a # hiccup that could cause the ball
# to leave the field if this functions runs, so ignore the frame
if dt > .2: return Task.cont
# The collision handler collects the collisions. We dispatch which function
# to handle the collision based on the name of what was collided into
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
name = entry.getIntoNode().getName()
if name == "wall_collide": self.wallCollideHandler(entry)
elif name == "ground_collide": self.groundCollideHandler(entry)
elif name == "loseTrigger": self.loseGame(entry)
# Read the hands normal vector and tilt the maze accordingly
self.maze.setP(handNormal.z * -30)
self.maze.setR(handNormal.x * -30)
# Finally, we move the ball
# Update the velocity based on acceleration
self.ballV += self.accelV * dt * ACCEL
# Clamp the velocity to the maximum speed
if self.ballV.lengthSquared() > MAX_SPEED_SQ:
self.ballV.normalize()
self.ballV *= MAX_SPEED
# Update the position based on the velocity
self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
# This block of code rotates the ball. It uses something called a quaternion
# to rotate the ball around an arbitrary axis. That axis perpendicular to
# the balls rotation, and the amount has to do with the size of the ball
# This is multiplied on the previous rotation to incrimentally turn it.
prevRot = LRotationf(self.ball.getQuat())
axis = UP.cross(self.ballV)
newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
self.ball.setQuat(prevRot * newRot)
return Task.cont # Continue the task indefinitely
def add_ramp(self, color, base, top, width, thickness, rot=None):
midpoint = Point3((top + base) / 2.0)
rot = LRotationf(0, 0, 0) if rot is None else rot
# Temporarily move `base` and `top` to positions relative to a midpoint
# at (0, 0, 0).
if midpoint != Point3(0, 0, 0):
base = Point3(base - (midpoint - Point3(0, 0, 0)))
top = Point3(top - (midpoint - Point3(0, 0, 0)))
p3 = Point3(top.get_x(), top.get_y(), top.get_z() - thickness)
p4 = Point3(base.get_x(), base.get_y(), base.get_z() - thickness)
# Use three points to calculate an offset vector we can apply to `base`
# and `top` in order to find the required vertices.
offset = (Point3(top + Vec3(0, 0, -1000)) - base).cross(top - base)
offset.normalize()
offset *= (width / 2.0)
vertices = (
Point3(top - offset),
Point3(base - offset),
Point3(base + offset),
Point3(top + offset),
Point3(p3 + offset),
Point3(p3 - offset),
Point3(p4 - offset),
Point3(p4 + offset),
)
vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]
faces = (
# Top and bottom.
[vertices[0], vertices[1], vertices[2], vertices[3]],
[vertices[7], vertices[6], vertices[5], vertices[4]],
# Back and front.
[vertices[0], vertices[3], vertices[4], vertices[5]],
[vertices[6], vertices[7], vertices[2], vertices[1]],
# Left and right.
[vertices[0], vertices[5], vertices[6], vertices[1]],
[vertices[7], vertices[4], vertices[3], vertices[2]],
)
if width and (p3 - base).length():
self._commit_polygon(Polygon(faces[0]), color)
self._commit_polygon(Polygon(faces[1]), color)
if width and thickness:
self._commit_polygon(Polygon(faces[2]), color)
self._commit_polygon(Polygon(faces[3]), color)
if thickness and (p3 - base).length():
self._commit_polygon(Polygon(faces[4]), color)
self._commit_polygon(Polygon(faces[5]), color)
return self
def add_ramp(self, color, base, top, width, thickness, rot=None):
midpoint = Point3((top + base) / 2.0)
rot = LRotationf(0, 0, 0) if rot is None else rot
# Temporarily move `base` and `top` to positions relative to a midpoint
# at (0, 0, 0).
if midpoint != Point3(0, 0, 0):
base = Point3(base - (midpoint - Point3(0, 0, 0)))
top = Point3(top - (midpoint - Point3(0, 0, 0)))
p3 = Point3(top.get_x(), top.get_y() - thickness, top.get_z())
p4 = Point3(base.get_x(), base.get_y() - thickness, base.get_z())
# Use three points to calculate an offset vector we can apply to `base`
# and `top` in order to find the required vertices.
offset = (Point3(top + Vec3(0, -1000, 0)) - base).cross(top - base)
offset.normalize()
offset *= (width / 2.0)
vertices = (
Point3(top - offset),
Point3(base - offset),
Point3(base + offset),
Point3(top + offset),
Point3(p3 + offset),
Point3(p3 - offset),
Point3(p4 - offset),
Point3(p4 + offset),
)
vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]
faces = (
# Top and bottom.
[vertices[0], vertices[1], vertices[2], vertices[3]],
[vertices[7], vertices[6], vertices[5], vertices[4]],
# Back and front.
[vertices[0], vertices[3], vertices[4], vertices[5]],
[vertices[6], vertices[7], vertices[2], vertices[1]],
# Left and right.
[vertices[0], vertices[5], vertices[6], vertices[1]],
[vertices[7], vertices[4], vertices[3], vertices[2]],
)
if width and (p3 - base).length():
self._commit_polygon(Polygon(faces[0]), color)
self._commit_polygon(Polygon(faces[1]), color)
if width and thickness:
self._commit_polygon(Polygon(faces[2]), color)
self._commit_polygon(Polygon(faces[3]), color)
if thickness and (p3 - base).length():
self._commit_polygon(Polygon(faces[4]), color)
self._commit_polygon(Polygon(faces[5]), color)
return self
def rollTask(self, task):
self.gesture_controler.track()
dt = globalClock.getDt()
if dt > 0.2:
return Task.cont
if self.gesture_controler.getPause():
return Task.cont
# if self.gesture_controler.getQuit():
# print("Gesture: Quit. You will quit from the game.")
# sys.exit()
# if self.gesture_controler.getReset():
# self.resetGame()
# return Task.cont
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
name = entry.getIntoNode().getName()
if name == "wall_collide":
self.wallCollideHandler(entry)
elif name == "ground_collide":
self.groundCollideHander(entry)
elif name == "loseTriggers":
self.loseGame(entry)
if self.gesture_controler.isMoving():
cur_pos = self.gesture_controler.getPos()
mpos = LPoint2f(cur_pos[0], cur_pos[1])
# print(mpos)
self.maze.setP(mpos.getY() * -10)
self.maze.setR(mpos.getX() * 10)
self.ballV += self.accelV * dt * ACCEL
if self.ballV.lengthSquared() > MAX_SPEED_SQ:
self.ballV.normalize()
self.ballV *= MAX_SPEED
self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
prevRot = LRotationf(self.ball.getQuat())
axis = LVector3.up().cross(self.ballV)
newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
self.ball.setQuat(prevRot * newRot)
return Task.cont
def add_dome(self, color, center, radius, samples, planes, rot=None):
two_pi = pi * 2
half_pi = pi / 2
azimuths = [(two_pi * i) / samples for i in range(samples + 1)]
elevations = [(half_pi * i) / (planes - 1) for i in range(planes)]
rot = LRotationf(0, 0, 0) if rot is None else rot
# Generate polygons for all but the top tier. (Quads)
for i in range(0, len(elevations) - 2):
for j in range(0, len(azimuths) - 1):
x1, y1, z1 = to_cartesian(azimuths[j], elevations[i], radius)
x2, y2, z2 = to_cartesian(azimuths[j], elevations[i + 1],
radius)
x3, y3, z3 = to_cartesian(azimuths[j + 1], elevations[i + 1],
radius)
x4, y4, z4 = to_cartesian(azimuths[j + 1], elevations[i],
radius)
vertices = (
Point3(x1, y1, z1),
Point3(x2, y2, z2),
Point3(x3, y3, z3),
Point3(x4, y4, z4),
)
vertices = [
rot.xform(v) + LVector3f(*center) for v in vertices
]
self._commit_polygon(Polygon(vertices), color)
# Generate polygons for the top tier. (Tris)
for k in range(0, len(azimuths) - 1):
x1, y1, z1 = to_cartesian(azimuths[k],
elevations[len(elevations) - 2], radius)
x2, y2, z2 = Vec3(0, radius, 0)
x3, y3, z3 = to_cartesian(azimuths[k + 1],
elevations[len(elevations) - 2], radius)
vertices = (
Point3(x1, y1, z1),
Point3(x2, y2, z2),
Point3(x3, y3, z3),
)
vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]
self._commit_polygon(Polygon(vertices), color)
return self
def add_solid(self, node):
mesh = BulletConvexHullShape()
mesh.add_geom(GeomBuilder().add_ramp(self.color, self.base, self.top,
self.width, self.thickness,
LRotationf(*self.hpr)).get_geom())
node.add_shape(mesh)
return node
def add_solid(self, node):
mesh = BulletConvexHullShape()
mesh.add_geom(GeomBuilder().add_dome(self.color, self.center,
self.radius,
self.samples, self.planes,
LRotationf(*self.hpr)).get_geom())
node.add_shape(mesh)
return node
def add_block(self, color, center, size, rot=None):
x_shift = size[0] / 2.0
y_shift = size[1] / 2.0
z_shift = size[2] / 2.0
rot = LRotationf(0, 0, 0) if rot is None else rot
vertices = (
Point3(-x_shift, +y_shift, +z_shift),
Point3(-x_shift, -y_shift, +z_shift),
Point3(+x_shift, -y_shift, +z_shift),
Point3(+x_shift, +y_shift, +z_shift),
Point3(+x_shift, +y_shift, -z_shift),
Point3(+x_shift, -y_shift, -z_shift),
Point3(-x_shift, -y_shift, -z_shift),
Point3(-x_shift, +y_shift, -z_shift),
)
vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]
faces = (
# XY
[vertices[0], vertices[1], vertices[2], vertices[3]],
[vertices[4], vertices[5], vertices[6], vertices[7]],
# XZ
[vertices[0], vertices[3], vertices[4], vertices[7]],
[vertices[6], vertices[5], vertices[2], vertices[1]],
# YZ
[vertices[5], vertices[4], vertices[3], vertices[2]],
[vertices[7], vertices[6], vertices[1], vertices[0]],
)
if size[0] and size[1]:
self._commit_polygon(Polygon(faces[0]), color)
self._commit_polygon(Polygon(faces[1]), color)
if size[0] and size[2]:
self._commit_polygon(Polygon(faces[2]), color)
self._commit_polygon(Polygon(faces[3]), color)
if size[1] and size[2]:
self._commit_polygon(Polygon(faces[4]), color)
self._commit_polygon(Polygon(faces[5]), color)
return self
def add_block(self, color, center, size, rot=None):
x_shift = size[0] / 2.0
y_shift = size[1] / 2.0
z_shift = size[2] / 2.0
rot = LRotationf(0, 0, 0) if rot is None else rot
vertices = (
Point3(-x_shift, +y_shift, +z_shift),
Point3(-x_shift, -y_shift, +z_shift),
Point3(+x_shift, -y_shift, +z_shift),
Point3(+x_shift, +y_shift, +z_shift),
Point3(+x_shift, +y_shift, -z_shift),
Point3(+x_shift, -y_shift, -z_shift),
Point3(-x_shift, -y_shift, -z_shift),
Point3(-x_shift, +y_shift, -z_shift),
)
vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]
faces = (
# XY
[vertices[0], vertices[1], vertices[2], vertices[3]],
[vertices[4], vertices[5], vertices[6], vertices[7]],
# XZ
[vertices[0], vertices[3], vertices[4], vertices[7]],
[vertices[6], vertices[5], vertices[2], vertices[1]],
# YZ
[vertices[5], vertices[4], vertices[3], vertices[2]],
[vertices[7], vertices[6], vertices[1], vertices[0]],
)
if size[0] and size[1]:
self._commit_polygon(Polygon(faces[0]), color)
self._commit_polygon(Polygon(faces[1]), color)
if size[0] and size[2]:
self._commit_polygon(Polygon(faces[2]), color)
self._commit_polygon(Polygon(faces[3]), color)
if size[1] and size[2]:
self._commit_polygon(Polygon(faces[4]), color)
self._commit_polygon(Polygon(faces[5]), color)
return self
def add_dome(self, color, center, radius, samples, planes, rot=None):
two_pi = pi * 2
half_pi = pi / 2
azimuths = [(two_pi * i) / samples for i in range(samples + 1)]
elevations = [(half_pi * i) / (planes - 1) for i in range(planes)]
rot = LRotationf(0, 0, 0) if rot is None else rot
# Generate polygons for all but the top tier. (Quads)
for i in range(0, len(elevations) - 2):
for j in range(0, len(azimuths) - 1):
x1, y1, z1 = to_cartesian(azimuths[j], elevations[i], radius)
x2, y2, z2 = to_cartesian(azimuths[j], elevations[i + 1], radius)
x3, y3, z3 = to_cartesian(azimuths[j + 1], elevations[i + 1], radius)
x4, y4, z4 = to_cartesian(azimuths[j + 1], elevations[i], radius)
vertices = (
Point3(x1, y1, z1),
Point3(x2, y2, z2),
Point3(x3, y3, z3),
Point3(x4, y4, z4),
)
vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]
self._commit_polygon(Polygon(vertices), color)
# Generate polygons for the top tier. (Tris)
for k in range(0, len(azimuths) - 1):
x1, y1, z1 = to_cartesian(azimuths[k], elevations[len(elevations) - 2], radius)
x2, y2, z2 = Vec3(0, radius, 0)
x3, y3, z3 = to_cartesian(azimuths[k + 1], elevations[len(elevations) - 2], radius)
vertices = (
Point3(x1, y1, z1),
Point3(x2, y2, z2),
Point3(x3, y3, z3),
)
vertices = [rot.xform(v) + LVector3f(*center) for v in vertices]
self._commit_polygon(Polygon(vertices), color)
return self
def add_to(self, geom_builder):
geom_builder.add_wedge(self.color, self.base, self.top, self.width,
LRotationf(*self.hpr))
def updatePhysics(self, task):
'''
Use the motor PWM values calculated by the controller to apply forces
to the simulated vehicle.
This runs at every frame, so it needs to complete quickly.
'''
outputs = shm.kalman.get()
self.tm.update(outputs)
passive_wrench = vehicle.passive_forces(outputs, self.tm)
passive_forces, passive_torques = passive_wrench[:3], \
passive_wrench[3:]
# Get motor thrusts
thrusts = np.array(self.tm.get_thrusts())
# Add passive forces and torques to that produced by thrusters,
# converting them to sub space first.
force = self.tm.total_thrust(thrusts) + \
self.tm.orientation.conjugate() * passive_forces
torque = self.tm.total_torque(thrusts) + \
self.tm.orientation.conjugate() * passive_torques
# Finally apply forces and torques to the model
# we also need to account for panda3d's strange coordinate system
# (x and y are flipped and z points up (instead of down))
self.linearForce.setVector(force_subspace[1], \
force_subspace[0], \
-force_subspace[2])
# We're supposed to use axis angle here, but I'm being sneaky
# and using the torque vector directly, i.e. non normalized axis angle
# with the hopes that this LRotationf constructor will figure it out
self.angularForce.setQuat(\
LRotationf(LVector3f(torque_subspace[1], \
torque_subspace[0], \
-torque_subspace[2]), 1))
# Update shared variables for controller
outputs.heading = self.getHeading()
outputs.pitch = self.myPath.getP()
outputs.roll = self.myPath.getR()
# This velocity is in world space
# We need to put it into THRUST CONVENTION SPACE
# which we assume kalman outputs in...
velocity = self.getPhysicsObject().getVelocity()
# Bring the velocity into THRUST CONVENTION SPACE
# Don't forget to account for panda's coordinate system
velocity = self.tm.heading_quat.conjugate() * \
np.array((velocity.getY(), velocity.getX(), -velocity.getZ()))
outputs.velx = velocity[0]
outputs.vely = velocity[1]
outputs.depth_rate = velocity[2]
outputs.depth = self.getDepth()
outputs.north = self.myPath.getY()
outputs.east = self.myPath.getX()
dX = self.myPath.getX() - self.previousXY[0]
dY = self.myPath.getY() - self.previousXY[1]
# Forward and sway are in THRUST CONVENTION SPACE
# don't forget to account for panda's coordinate system
dF, dS, dD = self.tm.heading_quat.conjugate() * np.array((dY, dX, 0.0))
outputs.forward += dF
outputs.sway += dS
# Output some quaternions, accounting for Panda's coordinate system
outputs.q0, outputs.q2, outputs.q1, outputs.q3 = self.myPath.getQuat()
outputs.q3 *= -1.0
shm.kalman.set(outputs)
svHeadingInt.set(self.getHeading())
svDepth.set(self.getDepth())
#XXX: Approximate altitude assuming that the pool is 12 feet deep
svAltitude.set(3.6 - self.getDepth())
svDvlDmgNorth.set(self.myPath.getY())
svDvlDmgEast.set(self.myPath.getX())
self.previousXY = (self.myPath.getX(), self.myPath.getY()) #update
self.output_hydro_data()
return Task.cont
def add_wedge(self, color, base, top, width, rot=None):
delta_y = top.get_y() - base.get_y()
midpoint = Point3((top + base) / 2.0)
rot = LRotationf(0, 0, 0) if rot is None else rot
# Temporarily move `base` and `top` to positions relative to a midpoint
# at (0, 0, 0).
if midpoint != Point3(0, 0, 0):
base = Point3(base - (midpoint - Point3(0, 0, 0)))
top = Point3(top - (midpoint - Point3(0, 0, 0)))
p3 = Point3(top.get_x(), base.get_y(), top.get_z())
# Use three points to calculate an offset vector we can apply to `base`
# and `top` in order to find the required vertices. Ideally we'd use
# `p3` as the third point, but `p3` can potentially be the same as `top`
# if delta_y is 0, so we'll just calculate a new point relative to top
# that differs in elevation by 1000, because that sure seems unlikely.
# The "direction" of that point relative to `top` does depend on whether
# `base` or `top` is higher. Honestly, I don't know why that's important
# for wedges but not for ramps.
if base.get_y() > top.get_y():
direction = Vec3(0, 1000, 0)
else:
direction = Vec3(0, -1000, 0)
offset = (Point3(top + direction) - base).cross(top - base)
offset.normalize()
offset *= (width / 2.0)
vertices = (
Point3(top - offset),
Point3(base - offset),
Point3(base + offset),
Point3(top + offset),
Point3(p3 + offset),
Point3(p3 - offset),
)
vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]
faces = (
# The slope.
[vertices[0], vertices[1], vertices[2], vertices[3]],
# The bottom.
[vertices[5], vertices[4], vertices[2], vertices[1]],
# The back.
[vertices[0], vertices[3], vertices[4], vertices[5]],
# The sides.
[vertices[5], vertices[1], vertices[0]],
[vertices[4], vertices[3], vertices[2]],
)
if width or delta_y:
self._commit_polygon(Polygon(faces[0]), color)
if width and (p3 - base).length():
self._commit_polygon(Polygon(faces[1]), color)
if width and delta_y:
self._commit_polygon(Polygon(faces[2]), color)
if delta_y and (p3 - base).length():
self._commit_polygon(Polygon(faces[3]), color)
self._commit_polygon(Polygon(faces[4]), color)
return self
def add_to(self, geom_builder):
rot = LRotationf(*self.hpr)
geom_builder.add_dome(self.color, self.center, self.radius,
self.samples, self.planes, rot)
def add_to(self, geom_builder):
rot = LRotationf()
rot.set_hpr(self.hpr)
geom_builder.add_block(self.color, self.center, self.size, rot)
def add_wedge(self, color, base, top, width, rot=None):
delta_y = top.get_y() - base.get_y()
midpoint = Point3((top + base) / 2.0)
rot = LRotationf(0, 0, 0) if rot is None else rot
# Temporarily move `base` and `top` to positions relative to a midpoint
# at (0, 0, 0).
if midpoint != Point3(0, 0, 0):
base = Point3(base - (midpoint - Point3(0, 0, 0)))
top = Point3(top - (midpoint - Point3(0, 0, 0)))
p3 = Point3(top.get_x(), base.get_y(), top.get_z())
# Use three points to calculate an offset vector we can apply to `base`
# and `top` in order to find the required vertices. Ideally we'd use
# `p3` as the third point, but `p3` can potentially be the same as `top`
# if delta_y is 0, so we'll just calculate a new point relative to top
# that differs in elevation by 1000, because that sure seems unlikely.
# The "direction" of that point relative to `top` does depend on whether
# `base` or `top` is higher. Honestly, I don't know why that's important
# for wedges but not for ramps.
if base.get_y() > top.get_y():
direction = Vec3(0, 1000, 0)
else:
direction = Vec3(0, -1000, 0)
offset = (Point3(top + direction) - base).cross(top - base)
offset.normalize()
offset *= (width / 2.0)
vertices = (
Point3(top - offset),
Point3(base - offset),
Point3(base + offset),
Point3(top + offset),
Point3(p3 + offset),
Point3(p3 - offset),
)
vertices = [rot.xform(v) + LVector3f(*midpoint) for v in vertices]
faces = (
# The slope.
[vertices[0], vertices[1], vertices[2], vertices[3]],
# The bottom.
[vertices[5], vertices[4], vertices[2], vertices[1]],
# The back.
[vertices[0], vertices[3], vertices[4], vertices[5]],
# The sides.
[vertices[5], vertices[1], vertices[0]],
[vertices[4], vertices[3], vertices[2]],
)
if width or delta_y:
self._commit_polygon(Polygon(faces[0]), color)
if width and (p3 - base).length():
self._commit_polygon(Polygon(faces[1]), color)
if width and delta_y:
self._commit_polygon(Polygon(faces[2]), color)
if delta_y and (p3 - base).length():
self._commit_polygon(Polygon(faces[3]), color)
self._commit_polygon(Polygon(faces[4]), color)
return self
def add_to(self, geom_builder):
geom_builder.add_ramp(self.color, self.base, self.top, self.width,
self.thickness, LRotationf(*self.hpr))
def rollTask(self, task):
# Standard technique for finding the amount of time since the last
# frame
dt = globalClock.getDt()
# If dt is large, then there has been a # hiccup that could cause the ball
# to leave the field if this functions runs, so ignore the frame
if dt > .2:
return Task.cont
# if base.mouseWatcherNode.is_button_down('a'):
# self.holeRoot.setH(self.holeRoot.getH() + 1)
# print(self.holeRoot.getHpr())
# pass
# if base.mouseWatcherNode.is_button_down('s'):
# self.holeRoot.setP(self.holeRoot.getP() + 1)
# print(self.holeRoot.getHpr())
# pass
# if base.mouseWatcherNode.is_button_down('d'):
# self.holeRoot.setR(self.holeRoot.getR() + 1)
# print(self.holeRoot.getHpr())
# pass
# go through different visualizations
if base.mouseWatcherNode.is_button_down(
'space') and self.showing == 'none':
self.showing = 'parts'
self.showingProgress = 0
pass
#print(self.showing)
#print(self.showing)
if self.showing == 'none':
return Task.cont
if self.showing == 'parts':
self.showingProgress += 0.01
#self.showingProgress += 1
#print(self.showingProgress)
scale = 2 - self.showingProgress
scaleY = 1 + (scale - 1) * 0.5
for planeIndex, planeNP in enumerate(self.planeNPs):
center = self.planeCenters[planeIndex]
planeNP.setPos(center[0] * scale, center[1] * scaleY,
center[2] * scale)
planeNP.reparentTo(self.render)
planeNP.setTwoSided(True)
continue
if self.showingProgress > 1:
self.showing = 'moving'
for planeIndex, planeNP in enumerate(self.planeNPs):
planeNP.removeNode()
continue
self.planeScene.show()
self.showingProgress = 1
return Task.cont
if self.showing == 'moving':
self.showingProgress += 0.005
#self.showingProgress += 1
#print(self.showingProgress, np.sign(self.showingProgress - 0.5) * min(self.showingProgress % 0.5, 0.5 - self.showingProgress % 0.5) * 4)
self.camera.setPos(
np.sign(self.showingProgress - 0.5) *
min(self.showingProgress % 0.5,
0.5 - self.showingProgress % 0.5) * 3, 0, 0)
#self.camera.setHpr(angleDegrees, 0, 0)
#self.camera.lookAt(0, 0, 0)
self.camera.lookAt(0, 3, 0)
if self.showingProgress > 1:
self.showing = 'geometry'
self.camera.setPos(0, 0, 0)
#self.planeScene.removeNode()
# for triNP in self.triNPs:
# triNP.show()
# continue
self.showingProgress = 1
return Task.cont
if self.showing == 'geometry':
self.showingProgress += 0.02
if self.showingProgress > 1:
#self.showing = 'image'
self.showing = 'placement'
self.showingProgress = 0
self.holeRoot.show()
self.inPortalRoot.show()
self.outPortalRoot.show()
self.inPortalTube.show()
self.outPortalTube.show()
for ballRoot in self.ballRoots:
ballRoot.show()
continue
self.showingProgress = 0
pass
return Task.cont
# if self.showing == 'placement':
# self.showingProgress += 0.005
# continue
# mouse pose
if self.mouseWatcherNode.hasMouse():
mpos = self.mouseWatcherNode.getMouse()
self.mpos = mpos
self.pickerRay.setFromLens(self.camNode, mpos.getX(), mpos.getY())
pass
#if base.mouseWatcherNode.is_button_down('space') and self.showing == 'placement':
if self.showing == 'placement':
self.card.show()
self.planeScene.removeNode()
self.showing = 'image'
pass
# if base.mouseWatcherNode.is_button_down('space') and self.showing == 'image':
# for triNP in self.triNPs:
# triNP.hide()
# continue
# self.showing = 'start'
# pass
# for each plane, check which horizontal plane it is sitting on
self.ballGroundMap = {}
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
ballName = entry.getFromNode().getName()
groundName = entry.getIntoNode().getName()
if 'ball_ray_' not in ballName:
continue
if 'ground_' not in groundName:
continue
ballIndex = int(ballName[9:])
groundIndex = int(groundName[7:])
norm = -entry.getSurfaceNormal(render)
if norm.length() == 0:
continue
norm = norm / norm.length()
distance = norm.dot(
entry.getSurfacePoint(render) -
self.ballRoots[ballIndex].getPos())
#print(distance)
if distance < 0:
continue
if ballIndex not in self.ballGroundMap or distance < self.ballGroundMap[
ballIndex][1]:
self.ballGroundMap[ballIndex] = (groundIndex, distance)
pass
continue
# The collision handler collects the collisions. We dispatch which function
# to handle the collision based on the name of what was collided into
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
fromName = entry.getFromNode().getName()
#if 'mouseRay' in fromName:
#continue
name = entry.getIntoNode().getName()
#if name == "plane_collide":
if 'tri_' in name:
self.planeCollideHandler(entry)
#elif name == "wall_collide":
#self.wallCollideHandler(entry)
#elif name == "ground_collide":
#self.groundCollideHandler(entry)
elif 'ball_' in name:
self.ballCollideHandler(entry)
elif 'ground_' in name:
self.groundCollideHandler(entry)
elif 'hole' in name:
self.score(entry)
elif 'portal_' in name:
self.portal(entry)
pass
continue
# Read the mouse position and tilt the maze accordingly
if base.mouseWatcherNode.hasMouse():
mpos = base.mouseWatcherNode.getMouse() # get the mouse position
#self.maze.setP(mpos.getY() * -10)
#self.maze.setR(mpos.getX() * 10)
pass
# if base.mouseWatcherNode.is_button_down('mouse1'):
# print(base.mouseWatcherNode.getMouseX())
# print(base.mouseWatcherNode.getMouseY())
# exit(1)
# Finally, we move the ball
# Update the velocity based on acceleration
for ballIndex in xrange(len(self.balls)):
if self.ballVs[ballIndex].length(
) < 1e-4 and self.ballVs[ballIndex].dot(
self.accelVs[ballIndex]) < -1e-4:
self.ballVs[ballIndex] = LVector3(0, 0, 0)
self.accelVs[ballIndex] = LVector3(0, 0, 0)
else:
self.ballVs[ballIndex] += self.accelVs[ballIndex] * dt * ACCEL
pass
#print('current speed', self.ballVs[ballIndex], self.accelVs[ballIndex])
# Clamp the velocity to the maximum speed
if self.ballVs[ballIndex].lengthSquared() > MAX_SPEED_SQ:
self.ballVs[ballIndex].normalize()
self.ballVs[ballIndex] *= MAX_SPEED
pass
#print(self.ballVs[ballIndex], self.accelVs[ballIndex], self.ballRoots[ballIndex].getPos())
# Update the position based on the velocity
self.ballRoots[ballIndex].setPos(
self.ballRoots[ballIndex].getPos() +
(self.ballVs[ballIndex] * dt))
# This block of code rotates the ball. It uses something called a quaternion
# to rotate the ball around an arbitrary axis. That axis perpendicular to
# the balls rotation, and the amount has to do with the size of the ball
# This is multiplied on the previous rotation to incrimentally turn it.
prevRot = LRotationf(self.balls[ballIndex].getQuat())
axis = LVector3.up().cross(self.ballVs[ballIndex])
newRot = LRotationf(
axis,
np.rad2deg(dt * self.ballVs[ballIndex].length() /
self.ballSize))
self.balls[ballIndex].setQuat(prevRot * newRot)
continue
self.cueRoot.setPos(self.cuePos[0], self.cuePos[1], self.cuePos[2])
return Task.cont # Continue the task indefinitely
def add_to(self, geom_builder):
rot = LRotationf()
rot.set_hpr(self.hpr)
geom_builder.add_block(self.color, self.center, self.size, rot)
def rollTask(self, task):
# Standard technique for finding the amount of time since the last
# frame
#print("\r",self.maze.getR(), self.maze.getP(), self.ballRoot.getPos(), end="")
dt = globalClock.getDt()
print("\r{:.3} fps ".format(1 / dt), end="")
# If dt is large, then there has been a # hiccup that could cause the ball
# to leave the field if this functions runs, so ignore the frame
if dt > .2:
return Task.cont
#print(action)
if action == "start":
a = 1
elif action == "stop":
while action == "stop":
a = 0
# elif action == "restart": in Line 531
elif action == "coord":
a = 1
#ALGUNA CRIDA A METODE DE NARCIS/MARC
key_down = base.mouseWatcherNode.is_button_down
if self.ready_to_solve:
if key_down(KeyboardButton.ascii_key('d')):
screenshot = self.camera2_buffer.getScreenshot()
if screenshot:
v = memoryview(screenshot.getRamImage()).tolist()
img = np.array(v, dtype=np.uint8)
img = img.reshape(
(screenshot.getYSize(), screenshot.getXSize(), 4))
img = img[::-1]
#self.digitizer.set_src_img(img)
#self.digitizer.digitalize_source()
cv2.imshow('img', img)
#cv2.waitKey(0)
if key_down(KeyboardButton.ascii_key('s')):
print("Screenshot!")
self.camera2_buffer.saveScreenshot("screenshot.jpg")
# The collision handler collects the collisions. We dispatch which function
# to handle the collision based on the name of what was collided into
for i in range(self.cHandler.getNumEntries()):
entry = self.cHandler.getEntry(i)
name = entry.getIntoNode().getName()
if action == "restart":
self.loseGame(entry)
if name == "wall_col":
self.wallCollideHandler(entry)
elif name == "ground_col":
self.groundCollideHandler(entry)
elif name == "loseTrigger":
vr.restart = 1
global th
th = threading.Thread(target=listenVoice)
th.start()
self.loseGame(entry)
# Read the mouse position and tilt the maze accordingly
# Rotation axes use (roll, pitch, heave)
"""
if base.mouseWatcherNode.hasMouse():
mpos = base.mouseWatcherNode.getMouse() # get the mouse position
self.maze.setP(mpos.getY() * -10)
self.maze.setR(mpos.getX() * 10)
"""
ballPos = self.get_ball_position()
#print("BALL POS: ", ballPos)
posFPixel = self.path[self.indexPuntActual]
xFinal = posFPixel[1] #posFPixel[1]/np.shape(laberint)[0]*13 - 6.5
yFinal = posFPixel[
0] #-(posFPixel[0]/np.shape(laberint)[1]*13.5 - 6.8)
dist = math.sqrt((xFinal - ballPos[1])**2 +
(yFinal - ballPos[0])**2)
if (dist < self.minDist):
if (self.indexPuntActual == len(self.path) - 1):
print("SOLVED!!", end="")
while (self.aStar.distance(
(ballPos[0], ballPos[1]), self.path[self.indexPuntActual])
< self.pas):
if (self.indexPuntActual < len(self.path) - 1):
self.indexPuntActual += 1
else:
break
# ball pos (y,x)
#print("END POS: ", self.digitizer.endPos)
if voice_solving:
p_rotation = dir_veu[0]
r_rotation = dir_veu[1]
if p_rotation == 0 and r_rotation == 0 and ballPos is not None:
p_rotation, r_rotation = self.pid.getPR(
ballPos[1], ballPos[0], ballPos[1], ballPos[0],
self.maze.getP(), self.maze.getR(), dt)
else:
p_rotation = 0
r_rotation = 0
#print(ballPos, dist)
#print(ballPos)
if ballPos is not None:
p_rotation, r_rotation = self.pid.getPR(
ballPos[1], ballPos[0], xFinal, yFinal,
self.maze.getP(), self.maze.getR(), dt)
#print(p_rotation, r_rotation)
if key_down(KeyboardButton.up()):
p_rotation = -1
elif key_down(KeyboardButton.down()):
p_rotation = 1
if key_down(KeyboardButton.left()):
r_rotation = -1
elif key_down(KeyboardButton.right()):
r_rotation = 1
self.rotateMaze(p_rotation, r_rotation)
# Finally, we move the ball
# Update the velocity based on acceleration
self.ballV += self.accelV * dt * ACCEL
# Clamp the velocity to the maximum speed
if self.ballV.lengthSquared() > MAX_SPEED_SQ:
self.ballV.normalize()
self.ballV *= MAX_SPEED
# Update the position based on the velocity
self.ballRoot.setPos(self.ballRoot.getPos() + (self.ballV * dt))
#print(self.ballRoot.getPos())
# This block of code rotates the ball. It uses something called a quaternion
# to rotate the ball around an arbitrary axis. That axis perpendicular to
# the balls rotation, and the amount has to do with the size of the ball
# This is multiplied on the previous rotation to incrimentally turn it.
prevRot = LRotationf(self.ball.getQuat())
axis = LVector3.up().cross(self.ballV)
newRot = LRotationf(axis, 45.5 * dt * self.ballV.length())
self.ball.setQuat(prevRot * newRot)
elif key_down(KeyboardButton.ascii_key('1')):
self.solve()
if key_down(KeyboardButton.ascii_key('i')):
self.light.setY(self.light.getY() + 10 * dt)
elif key_down(KeyboardButton.ascii_key('k')):
self.light.setY(self.light.getY() - 10 * dt)
if key_down(KeyboardButton.ascii_key('j')):
self.light.setX(self.light.getX() - 10 * dt)
elif key_down(KeyboardButton.ascii_key('l')):
self.light.setX(self.light.getX() + 10 * dt)
if key_down(KeyboardButton.ascii_key('u')):
self.lightColor += 10000 * dt
self.light.node().setColor(
(self.lightColor, self.lightColor, self.lightColor, 1))
elif key_down(KeyboardButton.ascii_key('o')):
self.lightColor -= 10000 * dt
self.light.node().setColor(
(self.lightColor, self.lightColor, self.lightColor, 1))
if key_down(KeyboardButton.ascii_key('8')):
self.alightColor += 1 * dt
self.ambientL.node().setColor(
(self.alightColor, self.alightColor, self.alightColor, 1))
elif key_down(KeyboardButton.ascii_key('9')):
self.alightColor -= 1 * dt
self.ambientL.node().setColor(
(self.alightColor, self.alightColor, self.alightColor, 1))
if key_down(KeyboardButton.ascii_key('r')):
base.trackball.node().set_pos(0, 200, 0)
base.trackball.node().set_hpr(0, 60, 0)
return Task.cont # Continue the task indefinitely
