Exemple #1
0
    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
Exemple #2
0
  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()
Exemple #3
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 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])
Exemple #5
0
 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 )
Exemple #6
0
  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()
Exemple #7
0
    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
Exemple #8
0
	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 ) )
Exemple #9
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
Exemple #11
0
	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
Exemple #12
0
    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))
Exemple #13
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
Exemple #14
0
 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
Exemple #15
0
 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()
Exemple #16
0
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
Exemple #17
0
 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)
Exemple #18
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))
Exemple #19
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))
Exemple #20
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
Exemple #21
0
    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)
Exemple #23
0
    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)
Exemple #24
0
 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
Exemple #25
0
	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
Exemple #26
0
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)
Exemple #27
0
    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
Exemple #28
0
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]
Exemple #29
0
	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))
Exemple #30
0
    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)
Exemple #31
0
	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 )
Exemple #32
0
 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
Exemple #33
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
Exemple #34
0
 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()
Exemple #35
0
	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, ")"
Exemple #36
0
    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))
Exemple #37
0
 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']))
Exemple #38
0
    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, ")"
Exemple #39
0
 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"))
Exemple #40
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
Exemple #41
0
    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)
Exemple #42
0
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)
Exemple #43
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
Exemple #44
0
 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) )
Exemple #45
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
Exemple #46
0
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
Exemple #47
0
	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
Exemple #48
0
 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)
Exemple #49
0
 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
Exemple #50
0
 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))
Exemple #51
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
Exemple #52
0
 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)
Exemple #53
0
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]
Exemple #54
0
 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"))
Exemple #55
0
 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]))
Exemple #56
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))
Exemple #57
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
Exemple #58
0
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
Exemple #59
0
 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)