def __init__(self, terrain, x, z, height, length, color):
# translate x and z coordinates into world coordinates
mx = x * terrain.extent
mz = z * terrain.extent
# figure out a random direction for the grass blade to point in
direction = vec3(random.random() - 0.5, 0, random.random() - 0.5)
try:
direction.normalize()
except ZeroDivisionError:
direction = vec3(1, 0, 0)
direction = direction * length
# figure out vertex coordinates for the random vector
center = vec3(mx, 0, mz)
a = center - direction
b = center + direction
a.y = terrain.getWorldHeightValue(a.x, -a.z)
b.y = terrain.getWorldHeightValue(b.x, -b.z)
c = a + vec3(0, height, 0)
d = b + vec3(0, height, 0)
self.a = a
self.b = b
self.c = c
self.d = d
# store opengl value thingies
self.vertexArray = [a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z, d.x, d.y, d.z]
self.texCoords = [0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0]
self.color = color
def update(self):
RiftApp.update(self)
pressed = pygame.key.get_pressed()
if pressed[pgl.K_r]:
self.reset_camera()
rotation = 0.0
if pressed[pgl.K_q]:
rotation = +1.0
if pressed[pgl.K_e]:
rotation = -1.0
if (rotation != 0.0):
self.camera = self.camera * \
mat4.rotation(rotation * 0.01, vec3(0, 1, 0))
self.recompose_camera()
# Modify direction vectors for key presses
translation = vec3()
if pressed[pgl.K_r]:
self.rift.recenter_pose()
if pressed[pgl.K_w]:
translation.z = -1.0
elif pressed[pgl.K_s]:
translation.z = +1.0
if pressed[pgl.K_a]:
translation.x = -1.0
elif pressed[pgl.K_d]:
translation.x = +1.0
if (vec3.length(translation) > 0.1):
translation = self.camera.getMat3() * (translation * 0.005)
self.camera.translate(translation)
self.recompose_camera()
def updateForces(self, timedelta, maxForce, yaw=0):
#take moveVector, rotate it by yaw, and scale it
strafe_force = -self.moveVector.x
fwd_force = self.moveVector.z
torque_vec = vec3( fwd_force, 0, strafe_force )
self.moveForce = MathUtil.rotateVectorAroundAxis(torque_vec, yaw, vec3(0,1,0), True )
self.moveForce = self.moveForce * 20
def unpack_message(msg):
default_pos = np.array([-2,-15,0.5])
master_pos = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
# Correction by 20 for origin compensation
cmd_values = np.array([msg.pose.position.x, msg.pose.position.z])
current_mag= np.linalg.norm(cmd_values) - 15 if (np.linalg.norm(cmd_values) > 15 ) else 0
vel_st = Twist()
if current_mag>0 :
#convert and remap speed values, with deadzone of 15
vel_st.linear.y = -msg.pose.position.z/20 if (math.fabs(msg.pose.position.z) > 50 and math.fabs(msg.pose.position.x) < 50) else 0
vel_st.linear.x = msg.pose.position.x/20 if (math.fabs(msg.pose.position.x) > 50 and math.fabs(msg.pose.position.z) < 50) else 0
vel_st.linear.z = 0
vel_st.angular.x = 0
vel_st.angular.y = 0
vel_st.angular.z = math.atan2 (-msg.pose.position.z,msg.pose.position.x) if (math.fabs(msg.pose.position.x) > 50 and math.fabs(msg.pose.position.z) > 50) else 0
pub.publish(vel_st)
#~ rospy.loginfo("talker")
current_angle=np.arccos(np.dot(master_pos, default_pos))
v=vec3(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z)
v2=vec3(-2,-15,0.5)
#~ print("length, angle" ,current_mag,current_angle, v.length(),v.angle(v2),master_pos )
rospy.loginfo("pos %s", master_pos)
master_rot = np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])
master_header = np.array([msg.header.frame_id, msg.header.seq, msg.header.stamp])
def makeVertexBuffer(self):
self.vertexbuffer = VertexBuffer.MakeStandardVertexBuffer(
vertices=[vec3(*x) for x in self.vertices_gl],
normals=[vec3(*x) for x in self.normals_gl],
tex_coords=ListFunctions.flattenListOfLists(self.texcoord_gl)
)
self.indexbuffer = VertexBuffer.IndexBufferObject( self.indices_gl )
def update(self):
RiftApp.update(self)
pressed = pygame.key.get_pressed()
if pressed[pgl.K_r]:
self.reset_camera()
rotation = 0.0
if pressed[pgl.K_q]:
rotation = +1.0
if pressed[pgl.K_e]:
rotation = -1.0
if (rotation != 0.0):
self.camera = self.camera * \
mat4.rotation(rotation * 0.01, vec3(0, 1, 0))
self.recompose_camera()
# Modify direction vectors for key presses
translation = vec3()
if pressed[pgl.K_r]:
self.rift.recenter_pose()
if pressed[pgl.K_w]:
translation.z = -1.0
elif pressed[pgl.K_s]:
translation.z = +1.0
if pressed[pgl.K_a]:
translation.x = -1.0
elif pressed[pgl.K_d]:
translation.x = +1.0
if (vec3.length(translation) > 0.1):
translation = self.camera.getMat3() * (translation * 0.005)
self.camera.translate(translation)
self.recompose_camera()
def __init__(self, terrain, x, z, height, length, color):
#translate x and z coordinates into world coordinates
mx = x * terrain.extent
mz = z * terrain.extent
#figure out a random direction for the grass blade to point in
direction = vec3(random.random() - 0.5, 0, random.random() - 0.5)
try:
direction.normalize()
except ZeroDivisionError:
direction = vec3(1, 0, 0)
direction = direction * length
#figure out vertex coordinates for the random vector
center = vec3(mx, 0, mz)
a = center - direction
b = center + direction
a.y = terrain.getWorldHeightValue(a.x, -a.z)
b.y = terrain.getWorldHeightValue(b.x, -b.z)
c = a + vec3(0, height, 0)
d = b + vec3(0, height, 0)
self.a = a
self.b = b
self.c = c
self.d = d
#store opengl value thingies
self.vertexArray = [
a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z, d.x, d.y, d.z
]
self.texCoords = [0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0]
self.color = color
def getInterpolatedHeight( self, fx, fy ): x, y = self.correctXY( fx, fy ) t = self.getTriangleAtPosition( x, y ) #calculate the triangle's plane plane = MathUtil.getTrianglePlane( t[0], t[1], t[2] ) return MathUtil.intersectRayWithPlane( plane, vec3( x,0,y), vec3(0,1,0 ) )
def addContact(self, geom1, geom2, c):
# add a contact between a capped cylinder BP and the ground
(cpos, cnor, cdep, cg1, cg2) = c.getContactGeomParams()
# figure out which cylinder foot this contact describes
if type(geom1) is ode.GeomCCylinder:
cylinder = geom1
elif type(geom2) is ode.GeomCCylinder:
cylinder = geom2
# find endpoints of cylinder
r = mat3(cylinder.getRotation())
p = vec3(cylinder.getPosition())
(radius, length) = cylinder.getParams()
# is collision point c in an endpoint?
ep0 = p + r * vec3(0, 0, -length / 2)
ep1 = p + r * vec3(0, 0, length / 2)
# is cpos in sphere around ep0 or ep1?
for ep in ep0, ep1:
cpos = vec3(cpos)
d2 = (cpos - ep).length()
if (d2 <= radius + 0.1):
epc = ep
# we will get two addContact() calls for each real contact, one for each
# of the joined capped cylinders, so only add a contact for the
# 'furthest out' from the root. If geom is the root then always add the
# contact.
if not cylinder.parent or epc == ep1:
mu = 300
self.points.append((cpos, (0, 1, 1), mu / 1000.0))
c.setMu(mu)
j = ode.ContactJoint(self.world, self.contactGroup, c)
j.attach(geom1.getBody(), geom2.getBody())
# remember contact for touch sensors
self.geom_contact[geom1] = 1
self.geom_contact[geom2] = 1
def test_lookAtAlt(self):
v1 = self.cgm.lookAt(cg.vec3(), self.cgvec3, cg.vec3(0, 1, 0))
v2 = self.cym.lookAtRH(cycg.vec3(), self.cyvec3, cycg.vec3(0, 1, 0))
try:
self.assertEqual(v1, v2)
raise AssertionError('CGkit\'s lookAt and CyCGkit\'s lookAtRH return same result.')
except AssertionError:
return
def __init__(self, shininess = 0.15, dif_refl = cgt.vec3([0.8,0.8,0.8]), amb_refl = cgt.vec3([0.8,0.8,0.8]), spe_refl = cgt.vec3([0.8,0.8,0.8])): self.dif_refl = dif_refl self.amb_refl = amb_refl self.spe_refl = spe_refl self.shininess = shininess
def getInterpolatedHeight(self, fx, fy):
x, y = self.correctXY(fx, fy)
t = self.getTriangleAtPosition(x, y)
#calculate the triangle's plane
plane = MathUtil.getTrianglePlane(t[0], t[1], t[2])
return MathUtil.intersectRayWithPlane(plane, vec3(x, 0, y),
vec3(0, 1, 0))
def updateForces(self, timedelta, maxForce, yaw=0):
#take moveVector, rotate it by yaw, and scale it
strafe_force = -self.moveVector.x
fwd_force = self.moveVector.z
torque_vec = vec3(fwd_force, 0, strafe_force)
self.moveForce = MathUtil.rotateVectorAroundAxis(
torque_vec, yaw, vec3(0, 1, 0), True)
self.moveForce = self.moveForce * 20
def __init__(self,
shininess=0.15,
dif_refl=cgt.vec3([0.8, 0.8, 0.8]),
amb_refl=cgt.vec3([0.8, 0.8, 0.8]),
spe_refl=cgt.vec3([0.8, 0.8, 0.8])):
self.dif_refl = dif_refl
self.amb_refl = amb_refl
self.spe_refl = spe_refl
self.shininess = shininess
def __init__(self,
pos_0=cgtypes.vec3(0,0,0),
ori_0=cgtypes.quat(1,0,0,0),
vel_0=cgtypes.vec3(0,0,0),
):
self.pos_0 = pos_0
self.ori_0 = ori_0
self.vel_0 = vel_0
self.reset()
def randomVec3(ov):
"Create a random normalised vector"
try:
if ov == None:
ov = (None, None, None)
v = vec3(rnd(-1, 1, ov[0]), rnd(-1, 1, ov[1]), rnd(-1, 1,
ov[2])).normalize()
except:
v = vec3(1, 0, 0)
return v
def faceTowards(self, direction_vector):
if direction_vector.length():
#figure out the current direction
current_direction = MathUtil.rotateVectorAroundAxis(
vec3(0, 0, 1), self.yaw, vec3(0, 1, 0))
direction_vector = direction_vector.normalize()
# interpolate between current direction and desired direction
d = MathUtil.interpolateVectors(current_direction,
direction_vector, 0.1)
self.yaw = MathUtil.yawAngle(d)
def test_pose_serial(self):
anim = BVHToolkit.Animation(BVHToolkit.Bone(bvh_serial["root"]))
anim.add_frame(bvh_serial["frames"][0])
anim.add_frame(bvh_serial["frames"][1])
pose = anim.get_pose(0)
self.assertVec3Equal(pose.get_position(0), vec3(0, 0, 0))
self.assertVec3Equal(pose.get_position(3), vec3(0, 60, 0))
pose = anim.get_pose(1)
self.assertVec3Equal(pose.get_position(3), vec3(0, -60, 0))
def test_pose_serial(self):
anim = BVHToolkit.Animation(BVHToolkit.Bone(bvh_serial["root"]))
anim.add_frame(bvh_serial["frames"][0])
anim.add_frame(bvh_serial["frames"][1])
pose = anim.get_pose(0)
self.assertVec3Equal(pose.get_position(0), vec3(0, 0, 0))
self.assertVec3Equal(pose.get_position(3), vec3(0, 60, 0))
pose = anim.get_pose(1)
self.assertVec3Equal(pose.get_position(3), vec3(0, -60, 0))
def _createWall(self, objName, objData):
lx, ly, lz = vec3(objData['lengths'])
pos = vec3(objData['position'])
x, y, z= objData['rotation']
rot = mat3().fromEulerXYZ(radians(x), radians(y), radians(z))
obj = Box(name=objName, lx=lx, ly=ly, lz=lz, pos=pos, rot=rot,
static=True)
logger.info("using box lengths: (%s, %s, %s)" % (lx, ly, lz))
logger.info("using box position: %s" % pos)
return obj
def setup(self):
print "[Geometry] [Extrude #{}] Compiling display list...".format(
self.ref)
self.display_list = glGenLists(1)
glNewList(self.display_list, GL_COMPILE)
glPushMatrix()
glTranslated(*self.position)
if self.texture_info.sides[0]:
glEnable(GL_TEXTURE_2D)
glColor4ubv(self.texture_info.color)
glBindTexture(GL_TEXTURE_2D, self.texture_info.tex_ref)
else:
glDisable(GL_TEXTURE_2D)
glColor4ubv(self.texture_info.color)
tex_increase = self.tex_max_x / float(len(self.points))
glBegin(GL_QUAD_STRIP)
for i, point in enumerate(self.points):
normal = self.vertex_normals[tuple(point)]
normal_down = vec3(normal.x, -0.5 + normal.y, normal.z).normalize()
normal_up = vec3(normal.x, 0.5 + normal.y, normal.z).normalize()
glNormal3fv(list(normal_down))
glTexCoord2d(tex_increase * i, 0)
glVertex3f(point[0], 0, point[1])
glNormal3fv(list(normal_up))
glTexCoord2d(tex_increase * i, self.tex_max_y)
glVertex3f(point[0], self.height, point[1])
glEnd()
tex_increase_x = self.tex_max_x / float(self.size[0])
tex_increase_y = self.tex_max_y / float(self.size[1])
glBegin(GL_POLYGON)
for point in self.points:
normal = self.vertex_normals[tuple(point)]
normal_up = vec3(normal.x, 0.5 + normal.y, normal.z).normalize()
glNormal3fv(list(normal_up))
glTexCoord2d(tex_increase_x * point[0], tex_increase_y * point[1])
glVertex3f(point[0], self.height, point[1])
glEnd()
glBegin(GL_POLYGON)
for point in self.points:
normal = self.vertex_normals[tuple(point)]
normal_up = vec3(normal.x, -0.5 + normal.y, normal.z).normalize()
glNormal3fv(list(normal_up))
glTexCoord2d(tex_increase_x * point[0], tex_increase_y * point[1])
glVertex3f(point[0], 0, point[1])
glEnd()
glPopMatrix()
glEndList()
def walkTo(self, X, Y, Theta):
cur_position = vec3(self.nao.position)
curTheta = self.nao.orientation
movement = vec3(X, Y, 0)
rotation = quat(curTheta, AXIS_Z)
movement = rotation.rotateVec(movement)
self.nao.position = list(cur_position + movement)
self.nao.orientation = (curTheta + Theta) % (math.pi * 2)
def walkTo(self, X, Y, Theta):
cur_position = vec3(self.nao.position)
curTheta = self.nao.orientation
movement = vec3(X, Y, 0)
rotation = quat(curTheta, AXIS_Z)
movement = rotation.rotateVec(movement)
self.nao.position = list(cur_position + movement)
self.nao.orientation = (curTheta + Theta) % (math.pi * 2)
def fitnessMovement(self):
"Cumulative movement between frames"
bg = self.bpgs[0]
for bp in bg.bodyparts:
p = bp.geom.getPosition()
if hasattr(bp, 'lastPos'):
m = (vec3(p) - vec3(bp.lastPos)).length()
d = ((self.meanPos(bg) - self.startpos).length())
self.score += m + d / 500
self.m += m
self.d += d / 500
bp.lastPos = p
def get_basis_first_person(self): DEG2RAD = math.pi / 180.0 yaw, pitch = self.yaw*DEG2RAD, self.pitch*DEG2RAD cosPitch = math.cos(pitch) sinPitch = math.sin(pitch) cosYaw = math.cos(yaw) sinYaw = math.sin(yaw) r_hat = cgt.vec3([sinYaw*cosPitch, -sinPitch,cosPitch*cosYaw]) f_hat = cgt.vec3([cosYaw,0,-sinYaw]) #r_hat = cgt.vec3([cosYaw,0,-sinYaw]) u_hat = cgt.vec3([sinYaw*sinPitch, cosPitch,cosYaw*sinPitch]) return r_hat, u_hat, f_hat
def playerMoveForce( old_velocity, yaw_in_radians, forward_vel, strafe_vel, mass, timestep, max_force ):
q = quaternionForAxisAngle( yaw_in_radians, vec3(0,1,0) )
relative_velocity = q.inverse().toMat3() * old_velocity
cur_fwd = relative_velocity.z
cur_strafe = relative_velocity.x
fwd_force = neededForceForVelocity( cur_fwd, forward_vel, mass, timestep)
stf_force = neededForceForVelocity( cur_strafe, strafe_vel, mass, timestep)
force_mag = math.sqrt( fwd_force*fwd_force + stf_force*stf_force )
if force_mag <= 0.0001:
return vec3(0,0,0)
#don't let force exceed maximum
if force_mag > max_force:
fwd_force *= float(max_force)/float(force_mag)
stf_force *= float(max_force)/float(force_mag)
return q.toMat3() * vec3(stf_force, 0, fwd_force)
def __init__(self):
self.isEnabled_=False
self.isFirstRender_=True
# The path used for input folder. If empty string is provided the current graph is shown, otherwise
# the first graph from the input folder is loaded and the user is able to switch to other graphs in that
# folder using the gamepad
self.inputFolderPath_=''
# The path for print output folder. If empty string is provided printing will be disabled
self.printFolderPath_=''
self.basevpn=vec3(-1,0,0) # view point normal vector of camera at the beginning
self.baseup=vec3(0,1,0) # up vector of camera at the beginning
self.prevpn=vec3(self.basevpn) # view point normal of previous frame
def num_world_xf(point, coords, worldToLocal=False): '''Transforms a point from local to world coordinates. This gives the same result as sym_world_xf, but is numerical (and thus much faster).''' assert(True not in [(math.isnan(x) or math.isinf(x)) for x in point+coords]) quat = num_euler_to_quat(coords[3:dof]) trans = cgtypes.vec3(coords[:3]) p = cgtypes.vec3(point) ret = None if worldToLocal: quat = quat.inverse() ret = quat.rotateVec(p - trans) #world to local else: ret = quat.rotateVec(p) + trans #local to world return [ret.x, ret.y, ret.z]
def update( self, kb, message_queue, timedelta):
world = kb.world
#walk to center of the map if we don't know where the treasure is...
if not kb.pather:
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(world.getEntByName("MapCenter").location))
def _configureJoint(self):
# set joint position and orientation
block = self._joint.body1
if block.type == ACTIVE:
sgn = 1
elif block.type == PASSIVE:
sgn = -1
#jointPosition = block.pos + sgn * vec3(0.25 * block.lx, 0, 0)
jointPosition = block.pos
jointRotation = block.rot.rotate(radians(90), vec3(0, 0, 1))
self._joint.pos = jointPosition
self._joint.rot = jointRotation
# set joint parameters
if self.cfg is not None:
motorfmax = self.cfg.get('motorfmax')
lostop = self.cfg.get('lostop')
histop = self.cfg.get('histop')
if motorfmax is not None:
self._joint.motorfmax = motorfmax
if lostop is not None:
self._joint.lostop = lostop
if histop is not None:
self._joint.histop = histop
logger.info("using motor fmax: %s" % motorfmax)
def update( self, kb, message_queue, timedelta):
#if self.entity.isHoldingSomething() and isinstance( kb.target, Ents.Actor ):
# if (kb.target.location - self.entity.location) > 800:
# kb.target = None
if kb.target is None and kb.isHoldingThingOfType("crate"):
all_enemies = kb.infoForAllKnownOfType(self.targetType)
if len(all_enemies) == 0: return
# THE FOLLOWING LINE WAS FAGGED UP BY IAN ON DECEMBER 4TH 2006
enemies = [ x for x in all_enemies if x.ent.knowledge.isHoldingThingOfType('treasurechest') ]
if len(enemies) == 0:
enemies = all_enemies
target = enemies[0]
for n in enemies:
if (n.ent.location - self.entity.location).length() < (target.ent.location-self.entity.location).length():
target = n
# don't retarget same enemy
if (target.location-self.entity.location).length() > 800:
return
#Make sure ninja isn't too far out
#if (target.location - self.entity.location).length() < 500:
print "Found enemy! ", target.typeName
kb.target = target.ent
#set the pather to lock onto it
kb.pather = PathFinder(self.entity)
kb.pather.setup( vec3(kb.target.location), kb.target )
def __init__(self,
max_simsecs=30.0,
fitnessName='meandistance',
noise_sd=0.01,
quick=0):
Sim.__init__(self, max_simsecs, noise_sd, quick)
log.debug('BPGSim.__init__')
self.geom_contact = {}
self.startpos = vec3(0, 0, 0)
self.relaxed = 0
self.prev_v = []
fitnessMap = {
'meandistance': self.fitnessMeanDistance,
'cumulativez': self.fitnessCumulativeZ,
'movement': self.fitnessMovement,
'after': self.fitnessAfter,
'meanxv': self.fitnessMeanXV,
'walk': self.fitnessWalk
}
if not fitnessName:
fitnessName = 'meandistance'
self.fitnessMethod = fitnessMap[fitnessName]
self.relax_time = RELAX_TIME
self.d = 0.0
self.m = 0.0
def update(self, kb, message_queue, timedelta):
if kb.target: return
world = kb.world
treasures_known = [
x for x in kb.infoForAllKnownOfType("treasurechest")
if x.ent.owner is None
]
map_center = world.getEntByName("MapCenter")
distance_to_center = (map_center.location -
self.entity.location).length()
#if not pathing anywhere...
if not kb.pather and distance_to_center >= 300:
#then walk to center of the map if we don't know where the treasure is...
if len(treasures_known) == 0:
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(map_center.location))
self.goToCenter = True
#otherwise, walk to the treasure
if len(treasures_known) > 0:
treasure = kb.returnClosestThing(self.entity, treasures_known)
kb.pather = PathFinder(self.entity)
kb.pather.setup(treasure.location)
self.goToCenter = True
if kb.pather and distance_to_center <= 20 and self.goToCenter:
kb.pather = None
self.goToCenter = False
def step(self):
"Single step of sim. Sets self.finished when sim is over"
log.debug('step')
self.points = []
self.space.collide(None, self.handleCollide)
self.worldStep()
self.contactGroup.empty()
self.total_time += DT
log.debug('stepped world by %f time is %f', DT, self.total_time)
# check for sim blowing up
for g in self.space:
if g.placeable():
v = vec3(g.getBody().getLinearVel())
if v.length() > 150:
self.fail() # blew up, early exit
return
nan = [
g for g in self.space if g.placeable() for p in g.getPosition()
if str(p) == 'nan'
]
if nan and self.max_simsecs != 0:
self.fail()
elif self.total_time > self.max_simsecs and self.max_simsecs != 0:
self.finished = 1
else:
self.updateFitness()
def update(self, kb, message_queue, timedelta):
#if kb.target and kb.target.owner != None:
# kb.target = None
#dont try to pickup anything if entity is already holding an object
if self.entity.isHoldingSomething():
return
#find something if entity doesn't have a target
if not kb.target:
throwables = [x for x in kb.infoForAllKnownOfType("crate")
#if not x.ent.thrown ]
if (not x.ent.moving() and not x.ent.inImpassableArea()
and x.ent.owner == None) ]
if len(throwables) == 0: return
target_info = throwables[0]
for n in throwables:
l1 = (n.ent.location - self.entity.location).length()
l2 = (target_info.ent.location-self.entity.location).length()
if l1 < l2:
target_info = n
kb.target = target_info.ent
kb.target.owner = self.entity
#repath to get to the throwable
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(target_info.location), target_info.ent )
print "Found thing to throw! (", target_info.typeName, ")"
def update(self, kb, message_queue, timedelta):
world = kb.world
#walk to center of the map if we don't know where the treasure is...
if not kb.pather:
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(world.getEntByName("MapCenter").location))
def _scaleDistanceUnits(self, distanceObjects):
distanceUnit = self.distanceUnit
for objectName, objectData in distanceObjects:
for itemName in objectData:
item = objectData[itemName]
if itemName in ('position', 'lengths'):
logger.debug("[_scaleDistanceUnits] position|lengths (before): %s" % str(objectData[itemName]))
objectData[itemName] = vec3(item) * distanceUnit
logger.debug("[_scaleDistanceUnits] position|lengths (after): %s" % str(objectData[itemName]))
elif itemName in ('radius', 'nearThreshold'):
logger.debug("[_scaleDistanceUnits] radius|nearThreshold (before): %s" % str(objectData[itemName]))
objectData[itemName] = item * distanceUnit
logger.debug("[_scaleDistanceUnits] radius|nearThreshold (after): %s" % str(objectData[itemName]))
elif itemName in ('motor',):
motorObject = objectData[itemName]
if motorObject.has_key('motorfmax'):
logger.debug("[_scaleDistanceUnits] motorfmax (before): %s" % str(motorObject['motorfmax']))
motorObject['motorfmax'] *= distanceUnit
logger.debug("[_scaleDistanceUnits] motorfmax (after): %s" % str(motorObject['motorfmax']))
elif itemName in ('sensor',):
sensorObject = objectData[itemName]
if sensorObject.has_key('effective_range'):
logger.debug("[_scaleDistanceUnits] effective_range (before): %s" % str(sensorObject['effective_range']))
sensorObject['effective_range'] *= distanceUnit
logger.debug("[_scaleDistanceUnits] effective_range (after): %s" % str(sensorObject['effective_range']))
def update(self, kb, message_queue, timedelta):
#if kb.target and kb.target.owner != None:
# kb.target = None
#dont try to pickup anything if entity is already holding an object
if self.entity.isHoldingSomething():
return
#find something if entity doesn't have a target
if not kb.target:
throwables = [
x for x in kb.infoForAllKnownOfType("crate")
#if not x.ent.thrown ]
if (not x.ent.moving() and not x.ent.inImpassableArea()
and x.ent.owner == None)
]
if len(throwables) == 0: return
target_info = throwables[0]
for n in throwables:
l1 = (n.ent.location - self.entity.location).length()
l2 = (target_info.ent.location - self.entity.location).length()
if l1 < l2:
target_info = n
kb.target = target_info.ent
kb.target.owner = self.entity
#repath to get to the throwable
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(target_info.location), target_info.ent)
print "Found thing to throw! (", target_info.typeName, ")"
def testStrFuncs(self):
"""Test the string functions.
"""
self.assertEqual("ab", sl.concat("a", "b"))
self.assertEqual("c=(0.2, 0.3, 0.4)", sl.format("c=%c", vec3(0.2,0.3,0.4)))
self.assertEqual(True, sl.match("pa", "spam"))
self.assertEqual(False, sl.match("ap", "spam"))
def update( self, kb, message_queue, timedelta):
if kb.target: return
world = kb.world
treasures_known = [x for x in kb.infoForAllKnownOfType("treasurechest") if x.ent.owner is None ]
map_center = world.getEntByName("MapCenter")
distance_to_center = (map_center.location - self.entity.location).length()
#if not pathing anywhere...
if not kb.pather and distance_to_center >= 300:
#then walk to center of the map if we don't know where the treasure is...
if len(treasures_known) == 0:
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(map_center.location))
self.goToCenter = True
#otherwise, walk to the treasure
if len(treasures_known) > 0:
treasure = kb.returnClosestThing(self.entity, treasures_known)
kb.pather = PathFinder(self.entity)
kb.pather.setup(treasure.location)
self.goToCenter = True
if kb.pather and distance_to_center <= 20 and self.goToCenter:
kb.pather = None
self.goToCenter = False
def update(self, kb, message_queue, timedelta):
#if self.entity.isHoldingSomething() and isinstance( kb.target, Ents.Actor ):
# if (kb.target.location - self.entity.location) > 800:
# kb.target = None
if kb.target is None and kb.isHoldingThingOfType("crate"):
all_enemies = kb.infoForAllKnownOfType(self.targetType)
if len(all_enemies) == 0: return
# THE FOLLOWING LINE WAS FAGGED UP BY IAN ON DECEMBER 4TH 2006
enemies = [
x for x in all_enemies
if x.ent.knowledge.isHoldingThingOfType('treasurechest')
]
if len(enemies) == 0:
enemies = all_enemies
target = enemies[0]
for n in enemies:
if (n.ent.location - self.entity.location).length() < (
target.ent.location - self.entity.location).length():
target = n
# don't retarget same enemy
if (target.location - self.entity.location).length() > 800:
return
#Make sure ninja isn't too far out
#if (target.location - self.entity.location).length() < 500:
print "Found enemy! ", target.typeName
kb.target = target.ent
#set the pather to lock onto it
kb.pather = PathFinder(self.entity)
kb.pather.setup(vec3(kb.target.location), kb.target)
def playerMoveForce(old_velocity, yaw_in_radians, forward_vel, strafe_vel,
mass, timestep, max_force):
q = quaternionForAxisAngle(yaw_in_radians, vec3(0, 1, 0))
relative_velocity = q.inverse().toMat3() * old_velocity
cur_fwd = relative_velocity.z
cur_strafe = relative_velocity.x
fwd_force = neededForceForVelocity(cur_fwd, forward_vel, mass, timestep)
stf_force = neededForceForVelocity(cur_strafe, strafe_vel, mass, timestep)
force_mag = math.sqrt(fwd_force * fwd_force + stf_force * stf_force)
if force_mag <= 0.0001:
return vec3(0, 0, 0)
#don't let force exceed maximum
if force_mag > max_force:
fwd_force *= float(max_force) / float(force_mag)
stf_force *= float(max_force) / float(force_mag)
return q.toMat3() * vec3(stf_force, 0, fwd_force)
def interpolateVectors( vecA, vecB, t ):
r = vec3()
diff = vecB - vecA
r.x = vecA.x + diff.x*t
r.y = vecA.y + diff.y*t
r.z = vecA.z + diff.z*t
return r
def setDesiredVelocity(self, vector):
self.moveVector = vec3(vector)
if self.moveVector.length() and self.joint:
self.joint.ForceDestroy()
self.joint = None
elif not self.moveVector.length() and not self.joint:
self.joint = self.world.solver.makeUpVectorJoint( self.body, (0,1,0) )
def interpolateVectors(vecA, vecB, t):
r = vec3()
diff = vecB - vecA
r.x = vecA.x + diff.x * t
r.y = vecA.y + diff.y * t
r.z = vecA.z + diff.z * t
return r
def unprojectRay ( x, y, invViewMatrix, projectionMatrix, screenWidth, screenHeight ):
sx = (((2.0 * x) / screenWidth ) - 1) / projectionMatrix[ 0,0 ]
sy = (((2.0 * y) / screenHeight) - 1) / projectionMatrix[ 1,1 ]
sz = 1.0
ray_direction = vec3( sx * invViewMatrix[0,0] + sy * invViewMatrix[1,0] + sz * invViewMatrix[2,0],
sx * invViewMatrix[0,1] + sy * invViewMatrix[1,1] + sz * invViewMatrix[2,1],
sx * invViewMatrix[0,2] + sy * invViewMatrix[1,2] + sz * invViewMatrix[2,2] )
return ray_direction
def setup(self, target_point, target_ent=None): self.nextPointIndex = 1 self.targetPoint = vec3(target_point) self.targetEnt = target_ent start_time = Platform.time_in_seconds() self.path = self.entity.pathToLocation( self.targetPoint ) end_time = Platform.time_in_seconds() self.pathGraphic = None
def faceTowards(self, direction_vector):
if direction_vector.length():
# figure out the current direction
current_direction = MathUtil.rotateVectorAroundAxis(vec3(0, 0, 1), self.yaw, vec3(0, 1, 0))
direction_vector = direction_vector.normalize()
# interpolate between current direction and desired direction
d = MathUtil.interpolateVectors(current_direction, direction_vector, 0.1)
self.yaw = MathUtil.yawAngle(d)
def setup(self, target_point, target_ent=None):
self.nextPointIndex = 1
self.targetPoint = vec3(target_point)
self.targetEnt = target_ent
start_time = Platform.time_in_seconds()
self.path = self.entity.pathToLocation(self.targetPoint)
end_time = Platform.time_in_seconds()
self.pathGraphic = None
def setDesiredVelocity(self, vector):
self.moveVector = vec3(vector)
if self.moveVector.length() and self.joint:
self.joint.ForceDestroy()
self.joint = None
elif not self.moveVector.length() and not self.joint:
self.joint = self.world.solver.makeUpVectorJoint(
self.body, (0, 1, 0))
def setDesiredVelocity(self, vector, yaw=0):
"""Sets the motion vector for the avatar to move in."""
self.body.Unfreeze()
if yaw:
self.moveVector = MathUtil.rotateVectorAroundAxis(
vector, yaw, vec3(0, 1, 0))
else:
self.moveVector = vector
def updateForces(self, timestep, maxForce, yaw=0):
""" Determines how much force is needed to move the player at the
desired speed. If yaw is specified, motion is relative to that angle."""
if not Settings.ApplyPlayerForces:
self.moveForce = vec3()
return
self.moveForce = Physics.DetermineMoveForceForVelocity(
self.moveVector, maxForce, self.body, timestep, yaw)
def num_world_xf(point, coords, worldToLocal=False):
'''Transforms a point from local to world coordinates. This gives the same
result as sym_world_xf, but is numerical (and thus much faster).'''
assert (True
not in [(math.isnan(x) or math.isinf(x)) for x in point + coords])
quat = num_euler_to_quat(coords[3:dof])
trans = cgtypes.vec3(coords[:3])
p = cgtypes.vec3(point)
ret = None
if worldToLocal:
quat = quat.inverse()
ret = quat.rotateVec(p - trans) #world to local
else:
ret = quat.rotateVec(p) + trans #local to world
return [ret.x, ret.y, ret.z]
def testStrFuncs(self):
"""Test the string functions.
"""
self.assertEqual("ab", sl.concat("a", "b"))
self.assertEqual("c=(0.2, 0.3, 0.4)",
sl.format("c=%c", vec3(0.2, 0.3, 0.4)))
self.assertEqual(True, sl.match("pa", "spam"))
self.assertEqual(False, sl.match("ap", "spam"))
def testgVectorInIMUCoords(self):
self.exp = ExperimentSceneMock.ExperimentSceneMock()
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), cgtypes.vec3([0, 0, -1]))
self.exp.rotativeTableAAngle = math.pi/2
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), cgtypes.vec3([0, -1, 0]))
self.exp.rotativeTableAAngle = 0
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), cgtypes.vec3([0, 0, -1]))
self.exp.rotativeTableBAngle = math.pi/2
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), cgtypes.vec3([1, 0, 0]))
self.exp.rotativeTableBAngle = 0
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), cgtypes.vec3([0, 0, -1]))
self.exp.rotativeTableAAngle = 1.5
self.exp.rotativeTableBAngle = 1.8
rotation = cgtypes.mat3.fromEulerXYZ(1.5, 1.8, 0).inverse()
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), rotation * cgtypes.vec3([0, 0, -1]))
self.exp.baseXYZAngles = (math.pi/2, 0.0, 0.0)
self.assertAlmostEquals(self.exp.gravityVector_IMU_coords(), cgtypes.vec3([0, -1, 0]))
def _getRotation(self, a, b):
x = a - b
relative_position = x > 0 and -1 or x < 0 and 1 or 0
current_angles = vec3(self.receiver.rot.toEulerXYZ())
angles = current_angles - self._initial_angles
rotY = cos(angles[1])
rotZ = 1 - sin(angles[2])
rotation = relative_position * rotY * rotZ
return int(round(rotation))
def setDesiredVelocity(self, vector, yaw=0):
"""Sets the motion vector for the avatar to move in."""
self.body.Unfreeze()
if yaw:
self.moveVector = MathUtil.rotateVectorAroundAxis(vector,
yaw,
vec3(0,1,0))
else:
self.moveVector = vector
def intersectRayWithPlane(plane, ray_origin, ray_direction):
plane_normal = vec3(plane[0], plane[1], plane[2])
d = plane[3]
denom = (plane_normal * ray_direction)
if denom == 0:
return None
num = -(plane_normal * ray_origin + d)
t = num / denom
return t
def meanPos(self, bpg):
tx = 0.0
ty = 0.0
tz = 0.0
for bp in bpg.bodyparts:
(x, y, z) = bp.geom.getPosition()
tx += x
ty += y
tz += z
return vec3(tx, ty, tz) / len(bpg.bodyparts)