def start(self): for component in self.node.data: if isinstance(component, Rigidbody): # Check if parent has rigidbody, and use that node if it does for p_component in self.node.parent.data: if isinstance(p_component, Rigidbody): self.parent_path = p_component.body_path self.update_parent = False if self.parent_path is None: parent_node = BulletRigidBodyNode(self.node.parent.name + "_node") parent_node.set_mass(0) self.parent_path = EComponent.panda_root_node.attach_new_node(parent_node) self.parent_path.setPos(helper.np_vec3_to_panda(self.node.parent.transform.get_world_translation())) rot = np.degrees(self.node.parent.transform.get_world_rotation()) self.parent_path.setHpr(LVector3f(rot[1], rot[0], rot[2])) self.parent_path.setScale(helper.np_vec3_to_panda(self.node.parent.transform.get_world_scale())) # Create constraint child_transform = TransformState.make_pos(LVector3f(0, 0, 0)) node_pos = self.node.transform.get_translation() * self.node.transform.get_world_scale() parent_transform = TransformState.make_pos(helper.np_vec3_to_panda(node_pos)) constraint = BulletConeTwistConstraint(component.body_path.node(), self.parent_path.node(), child_transform, parent_transform) constraint.set_limit(float(self.property_vals["swing_1"]), float(self.property_vals["swing_2"]), float(self.property_vals["max_twist"])) EComponent.physics_world.attachConstraint(constraint)
def segSegIntersection( start1, end1, start2, end2 ): width1 = max( start1.size, end1.size ) width2 = max( start2.size, end2.size ) min1 = LVector3f( min( start1.pos.x, end1.pos.x ) - width1, min( start1.pos.y, end1.pos.y ) - width1, min( start1.pos.z, end1.pos.z ) - width1 ) max1 = LVector3f( max( start1.pos.x, end1.pos.x ) + width1, max( start1.pos.y, end1.pos.y ) + width1, max( start1.pos.z, end1.pos.z ) + width1 ) min2 = LVector3f( min( start2.pos.x, end2.pos.x ) - width2, min( start2.pos.y, end2.pos.y ) - width2, min( start2.pos.z, end2.pos.z ) - width2 ) max2 = LVector3f( max( start2.pos.x, end2.pos.x ) + width2, max( start2.pos.y, end2.pos.y ) + width2, max( start2.pos.z, end2.pos.z ) + width2 ) # If the bounding boxes overlap, also check if the lines cross: contains = bb1.contains(bb2) if contains == BoundingVolume.IF_possible: p1,p2,dist = segSegDistance( start1.pos, end1.pos, start2.pos, end2.pos ) if dist < width1 + width2: return True return False
def testRender(self): env = BasicEnvironment("0004d52d1aeeb8ae6de39d6bd993e992", suncgDatasetRoot=TEST_SUNCG_DATA_DIR, depth=True) env.agent.setPos(LVector3f(42, -39, 1)) env.agent.setHpr(LVector3f(60.0, 0.0, 0.0)) env.step() image = env.renderWorld.getRgbImages()['agent-0'] depth = env.renderWorld.getDepthImages(mode='distance')['agent-0'] fig = plt.figure(figsize=(16, 8)) plt.axis("off") ax = plt.subplot(121) ax.imshow(image) ax = plt.subplot(122) ax.imshow(depth / np.max(depth), cmap='binary') plt.show(block=False) time.sleep(1.0) plt.close(fig) env.destroy()
def start(self): for component in self.node.data: if isinstance(component, Rigidbody): # Check if parent has rigidbody, and use that node if it does for p_component in self.node.parent.data: if isinstance(p_component, Rigidbody): self.parent_path = p_component.body_path self.update_parent = False if self.parent_path is None: parent_node = BulletRigidBodyNode(self.node.parent.name + "_node") parent_node.set_mass(0) self.parent_path = EComponent.panda_root_node.attach_new_node( parent_node) self.parent_path.setPos( helper.np_vec3_to_panda( self.node.parent.transform.get_world_translation())) rot = np.degrees( self.node.parent.transform.get_world_rotation()) self.parent_path.setHpr(LVector3f(rot[1], rot[0], rot[2])) self.parent_path.setScale( helper.np_vec3_to_panda( self.node.parent.transform.get_world_scale())) # Create constraint node_pos = self.node.transform.get_translation( ) * self.node.transform.get_world_scale() constraint = BulletSphericalConstraint( component.body_path.node(), self.parent_path.node(), LVector3f(0, 0, 0), LVector3f(node_pos[0], node_pos[1], node_pos[2])) EComponent.physics_world.attachConstraint(constraint)
def testInit(self): houseId = "0004d52d1aeeb8ae6de39d6bd993e992" scene = SunCgSceneLoader.loadHouseFromJson(houseId, SUNCG_DATA_DIR) agent = scene.agents[0] # Configure the agent transform = TransformState.makePosHpr(pos=LVector3f(38.42, -39.10, 1.70), hpr=LVector3f(-77.88, -13.93, 0.0)) agent.setTransform(transform) renderer = DepthRenderer(scene, size=(512, 512), fov=75.0) agentId = agent.getTag('agent-id') image = renderer.getDepthImage(agentId) fig = plt.figure(figsize=(8, 8)) plt.ion() plt.show() plt.axis("off") plt.imshow(image) plt.draw() plt.pause(1.0) plt.close(fig)
def testAgent(self): physics = None viewer = None try: scene = Scene() physics = Panda3dBulletPhysics(scene, debug=True) agent = scene.agents[0].find('**/+BulletRigidBodyNode') agent.setPos(LVector3f(0, 0, 1.0)) agent.node().setLinearVelocity(LVector3f(1, 0, 0)) agent.node().setAngularVelocity(LVector3f(0, 0, 1)) agent.node().setActive(True) viewer = Viewer(scene, interactive=False) viewer.disableMouse() mat = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, -10, 0, 1]) mat = LMatrix4f(*mat.ravel()) viewer.cam.setMat(mat) for _ in range(50): viewer.step() time.sleep(1.0) finally: self.hulkSmash(None, physics, viewer)
def reset(self): self.gameEngine.space_craft.connect_to_shuttle(self.frame) self.velocity = LVector3f(0, 0, 0) self.acceleration = LVector3f(0, 0, 0) self.spinning_velocity = LVector3f(0, 0, 0) self.spinning_acceleration = LVector3f(0, 0, 0) self.frame.set_hpr(self.frame.get_hpr()) self.compute_unit_vectors()
def compute_unit_vectors(self): h = TO_RAD * self.frame.get_h() p = TO_RAD * self.frame.get_p() r = TO_RAD * self.frame.get_r() self._u = LVector3f(-num.sin(h) * num.cos(p), num.cos(h) * num.cos(p), num.sin(p)) self._n = LVector3f( num.cos(h) * num.cos(r), num.sin(h) * num.cos(r), -num.sin(r))
def set_max_linear_speed(self, maxLinearSpeed): self.mMaxSpeedXYZ = LVector3f( abs(maxLinearSpeed.get_x()), abs(maxLinearSpeed.get_y()), abs(maxLinearSpeed.get_z())) self.mMaxSpeedSquaredXYZ = LVector3f( maxLinearSpeed.get_x() * maxLinearSpeed.get_x(), maxLinearSpeed.get_y() * maxLinearSpeed.get_y(), maxLinearSpeed.get_z() * maxLinearSpeed.get_z())
def set_default_args(self, idx): pref = 'lights[%s].' % idx render.setShaderInput(pref + 'pos', LVector4f(0, 0, 0, 1)) render.setShaderInput(pref + 'amb', LVector3f(0, 0, 0)) render.setShaderInput(pref + 'diff', LVector3f(0, 0, 0)) render.setShaderInput(pref + 'spec', LVector3f(0, 0, 0)) render.setShaderInput(pref + 'dir', LVector3f(0, 0, 0)) render.setShaderInput(pref + 'exp', .0) render.setShaderInput(pref + 'cutoff', .0)
def load_preview_model(self): p_str = "{}/{}_{}_{}.bam".format(self.path, self.name, self.preview_type, self.preview_rec) model_file = Filename(p_str) self.pre_model_np = loader.loadModel(model_file).getChildren()[0] SM.set_planet_shaders(self, self.pre_model_np, self.preview_type) self.pre_model_np.setShaderInput("light_vec", LVector3f(-1, 0, 0)) self.pre_model_np.setShaderInput("atmos_vals", LVector4f(0, 0, 0, 0)) self.pre_model_np.setShaderInput("body_dir", LVector3f(0, 1, 0)) ### return self.pre_model_np
def _handle_user_events_(self, ue, dt, delta=LVector3f()): if not self.FOCUS: return delta.set(0, 0, 0) cmds = ue.get_cmds(self) # Y-axis movement. if "move_y" in cmds: self._y_val += ue.y_diff delta.setY(self._y_val) else: self._y_val = 0 # X-axis movement. if "move_left" in cmds: delta.setX(self._x_val) if "move_right" in cmds: delta.setX(-self._x_val) # Z-axiz movement. if "move_up" in cmds: delta.setZ(-self._z_val) if "move_down" in cmds: delta.setZ(self._z_val) # Heading rotation. if "rotate_heading" in cmds: self._h_val += ue.x_diff * self._rot_factor # Only change heading if mouse moves out of "y_zone". _y_zone = self._y_zone * self._rot_factor if self._h_val > _y_zone or self._h_val < -_y_zone: self.NP.setH(self.NP, -(self._h_val - _y_zone)) else: self._h_val = 0 # Pitch rotation. if "rotate_pitch" in cmds: self._p_val += ue.y_diff * self._rot_factor self.LENS.setViewHpr(0, -self._p_val * 20, 0) # Translate and apply delta. mat = self.NP.getTransform(render).getMat() trans_delta = LVector3d(*mat.xformVec(delta)) * dt self.focus_pos += trans_delta self.sys_pos = self.FOCUS.sys_pos - self.focus_pos # Correct tilt of camera to stay level with surface. delta.normalize() _pos = LVector3f(*self.focus_pos) _pos.normalize() deg = self._prev_pos.angleDeg(_pos) y_deg = deg * delta.y x_deg = deg * delta.x self.NP.setP(self.NP, y_deg) self.NP.setR(self.NP, -x_deg) self._prev_pos = _pos
def traverse(self, nodePath, dnaStorage): pParentXScale = nodePath.getParent().getScale().getX() parentZScale = nodePath.getScale().getZ() node = dnaStorage.findNode(self.code) if node is None: raise DNAError.DNAError( 'DNACornice code %d not found in DNAStorage' % self.code) nodePathA = nodePath.attachNewNode('cornice-internal', 0) node = node.find('**/*_d') np = node.copyTo(nodePathA, 0) np.setPosHprScale( LVector3f(0, 0, 0), LVector3f(0, 0, 0), LVector3f(1, pParentXScale / parentZScale, pParentXScale / parentZScale)) np.setEffect(DecalEffect.make()) np.flattenStrong() node = node.getParent().find('**/*_nd') np = node.copyTo(nodePathA, 1) np.setPosHprScale( LVector3f(0, 0, 0), LVector3f(0, 0, 0), LVector3f(1, pParentXScale / parentZScale, pParentXScale / parentZScale)) np.flattenStrong() nodePathA.setPosHprScale(LVector3f(0, 0, node.getScale().getZ()), LVector3f(0, 0, 0), LVector3f(1, 1, 1)) nodePathA.setColor(self.color) nodePathA.flattenStrong() return
def stop(self, play_sound=True): if self._mvt_tasks is not None: # eventually stops the ongoing tasks self._mvt_tasks.finish() self._mvt_tasks = None self.gameEngine.taskMgr.remove("goto_start") self.gameEngine.taskMgr.remove("goto_end") self.velocity = LVector3f(0, 0, 0) self.spinning_velocity = LVector3f(0, 0, 0) self.compute_unit_vectors() if play_sound: self._boost_sound()
def __init__(self, recipe): self.__dict__.update(recipe) self.SATS = [] self.sys_vec = LVector3d(0, 0, 0) self.sys_pos = LVector3d(0, 0, 0) self.sys_hpr = LVector3f(0, 0, 0) self.sys_rot = LVector3f(0, 0, 0) self.POS = LVector3f(0, 0, 0) self.far_radius = _env.ATMOS_RADIUS - self.radius self.near_radius = 0 self._mode = "far" ## self._lod = "low" self._loaded = False
def spawn(self, t=None, spin_time=2, init_pos=None, end_pos=None): self.rotate_task = self.model.hprInterval(duration=spin_time, hpr=(0, 360, 0)) self.rotate_task.loop() self.move_task = self.model.posInterval( 40, end_pos if end_pos is not None else LVector3f(-1500, -140, -2), startPos=init_pos if init_pos is not None else LVector3f( 1500, 150, 2)) self.move_task.start() self.model.show() self.game_engine.taskMgr.doMethodLater(40, self.unspawn, name="asteroid_end")
def makeFromDGI(self, dgi): DNAGroup.DNAGroup.makeFromDGI(self, dgi) x = dgi.getInt32() / 100.0 y = dgi.getInt32() / 100.0 z = dgi.getInt32() / 100.0 self.pos = LVector3f(x, y, z) h = dgi.getInt32() / 100.0 p = dgi.getInt32() / 100.0 r = dgi.getInt32() / 100.0 self.hpr = LVector3f(h, p, r) sx = dgi.getInt16() / 100.0 sy = dgi.getInt16() / 100.0 sz = dgi.getInt16() / 100.0 self.scale = LVector3f(sx, sy, sz)
def ballePredit(self, task): if self.modele is None: data = loadtxt("train_data.csv", delimiter=",", skiprows=1) train_x = data[0:1000, 0] / 15 train_y = data[0:1000, 1] / 500 self.modele = tf.keras.Sequential([ tf.keras.layers.Dense(units=1, input_shape=(1, ), activation=tf.nn.relu), tf.keras.layers.Dense(units=64, activation=tf.nn.relu), tf.keras.layers.Dense(units=1) ]) optimizer = tf.train.RMSPropOptimizer(0.001) self.modele.compile(loss='mse', optimizer=optimizer, metrics=['mae']) self.modele.fit(train_x, train_y, epochs=500, validation_split=0.2, verbose=0) # Chargement de la balle balle = Balle(self) x = randint(-2, 9) y = randint(3, 17) z = random() / 2 + 2 pos_balle = LVector3f(x, y, z) self.balles[str(balle.corps)] = balle position_panier = self.panier.geom_cylinder.getPosition() position = position_panier - pos_balle distance = (position.x**2 + position.y**2 + position.z**2)**0.5 pos_z = distance * 2 direction = LVector3f(position.x, position.y, pos_z) direction.normalize() force = self.modele.predict([distance / 15.0]).flatten()[0] * 500 balle.set_tir(direction * force, pos_balle) return task.again
def __call__( self, x, y, z ): p = LVector3f((x,y,z)) ap = p - self.startPoint distFromSeg = self.radius - ( ap - self.segDir * ap.dot( self.segDir ) ).length() v = p - self.floorPoint distFromFloor = v.dot( self.floorNormal ) distFromCenter = (LVector3f((x,y,z)) - self.centerPoint).length() distFromEdge = self.boundSphereRadius - distFromCenter - 1 #return distFromCenter - distFromFloor return min( distFromFloor, distFromSeg, distFromEdge )
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 __init__(self, axis): MeshGizmo.__init__(self) self.color = (0.2, 0.2, 0.8, 1) self.start_mouse_world_pos = np.array([0, 0, 0, 1]) self.start_translate_callback = None self.translate_callback = None self.translate_finished_callback = None self.component = None self.start_pos = np.array([0, 0, 0]) self.axis = axis self.data = None # Load sphere mesh file sphere_json = None sphere_path = Path( path.realpath(__file__) ).parent.parent.parent.parent / "res/meshes/sphere_handle.json" with open(sphere_path, "r") as file: sphere_json = json.load(file) # Generate mesh (renders on top of everything else) MeshGizmo.gen_geom(self, sphere_json) self.geom_path.setTag("shader type", "gizmo") self.geom_path.set_shader_input("gizmo_pos", LVector3f(0, 0, 0)) self.get_geom().setColor(self.color) self.get_geom().setBin("fixed", 0) self.get_geom().setDepthTest(False) self.get_geom().setLightOff() # Define plane normal self.plane_normal = np.array([1, 0, 0])
def __init__(self, attr_name, init_val, pos, val_range, callback, args=None): # unused val_range GameObject.__init__(self) self.__callback = callback self.__args = args or [] self.__lab = OnscreenText(text=attr_name, pos=pos, align=TextNode.ARight, fg=(1, 1, 1, 1), parent=base.a2dTopLeft, scale=.046) slider_pos = LVector3f(pos[0], 1, pos[1]) + (.05, 0, .01) self.__slider = Entry(pos=slider_pos, initial_text=str(init_val), cmd=self.__set_attr, parent=base.a2dTopLeft, scale=.05, frame_col=(0, 0, 0, .2), text_fg=(1, 1, 1, 1)) # txt_pos = LVector3f(pos[0], pos[1], 1) + (.6, 0, 0) self.widgets = [self.__slider, self.__lab] self.toggle()
def worker(): env = BasicEnvironment("0004d52d1aeeb8ae6de39d6bd993e992", suncgDatasetRoot=TEST_SUNCG_DATA_DIR, depth=False, debug=True) env.agent.setPos(LVector3f(45, -42, 1)) env.agent.setHpr(LVector3f(45.0, 0.0, 0.0)) # Simulation loop for _ in range(nbSteps): env.step() _ = env.getObservation() env.destroy()
def __init__(self, houseId, suncgDatasetRoot=None, size=(256, 256), debug=False, depth=False, realtime=False, dt=0.1, cameraTransform=None): self.__dict__.update(houseId=houseId, suncgDatasetRoot=suncgDatasetRoot, size=size, debug=debug, depth=depth, realtime=realtime, dt=dt, cameraTransform=cameraTransform) self.scene = SunCgSceneLoader.loadHouseFromJson(houseId, suncgDatasetRoot) agentRadius = 0.1 agentHeight = 1.6 if self.cameraTransform is None: self.cameraTransform = TransformState.makePos(LVector3f(0.0, 0.0, agentHeight/2.0 - agentRadius)) self.renderWorld = Panda3dRenderer(self.scene, size, shadowing=False, depth=depth, cameraTransform=self.cameraTransform) self.physicWorld = Panda3dBulletPhysics(self.scene, suncgDatasetRoot, debug=debug, objectMode='box', agentRadius=agentRadius, agentHeight=agentHeight, agentMass=60.0, agentMode='capsule') self.clock = ClockObject.getGlobalClock() self.worlds = { "physics": self.physicWorld, "render": self.renderWorld, } self.agent = self.scene.agents[0] self.agentRbNp = self.agent.find('**/+BulletRigidBodyNode') self.labeledNavMap = None self.occupancyMapCoord = None
def __init__(self, direction): MeshGizmo.__init__(self) self.color = (1.0, 1.0, 1.0, 1.0) self.start_mouse_world_pos = np.array([0, 0, 0, 1]) self.translate_callback = None self.translate_finished_callback = None self.component = None self.start_pos = np.array([0, 0, 0]) self.direction = direction # Load arrow mesh file arrow_json = None arrow_path = Path(path.realpath(__file__)).parent.parent.parent.parent / "res/meshes/translate_arrow.json" with open(arrow_path, "r") as file: arrow_json = json.load(file) # Generate mesh (renders on top of everything else) MeshGizmo.gen_geom(self, arrow_json) self.geom_path.setTag("shader type", "gizmo") self.geom_path.set_shader_input("gizmo_pos", LVector3f(0, 0, 0)) self.get_geom().setColor(self.color) self.get_geom().setBin("fixed", 0) self.get_geom().setDepthTest(False) self.get_geom().setLightOff() # Define plane normal self.plane_normal = None if self.direction == TranslateArrowGizmo.DIR_X: self.plane_normal = np.array([0, 0, 1]) elif self.direction == TranslateArrowGizmo.DIR_Y: self.plane_normal = np.array([0, 0, 1]) else: self.plane_normal = np.array([1, 0, 0])
def lookAt(pos, at): """ Takes 2 LVector3f vectors, Returns the lookAt matrix Mat3d """ # forward yaxis = at - pos print("forward ", yaxis) yaxis.normalize() # right xaxis = LVector3f.up().cross(yaxis) xaxis.normalize() print("right ", xaxis) # up zaxis = xaxis.cross(yaxis) zaxis.normalize() print("up ", zaxis) rmat = LMatrix4f() rmat.setRow(0, xaxis) rmat.setRow(1, zaxis) rmat.setRow(2, yaxis) tmat = LMatrix4f.translateMat(pos) print(rmat*tmat) return rmat * tmat
def dynamic_look_at(self, target=None, time=5, update_is_moving=True, end_func=NodePath): self.stop() v = LVector3f(target if target is not None else (0, 0, 0)) if update_is_moving: self.gameEngine.update_soft_state("is_moving", True) self._mvt_tasks = self.frame.hprInterval(time, get_hpr(v - self.frame.get_pos()), self.frame.get_hpr(), blendType='easeInOut') self._mvt_tasks.start() def end(_): self.stop(play_sound=False) if update_is_moving: self.gameEngine.update_soft_state("is_moving", False) if hasattr(end_func, '__call__'): end_func.__call__() self._boost_sound(max(0.1, time - 1.5)) self.gameEngine.taskMgr.doMethodLater(time, end, name="stabilization_end")
def swingTwistDecomposition( rotation, twistAxis ): ra = LVector3f( rotation.getI(), rotation.getJ(), rotation.getK() ) p = ra.project( twistAxis ) # return projection v1 onto v2 twist = Quat( rotation.getR(), p.getX(), p.getY(), p.getZ() ) twist.normalize() swing = rotation * twist.conjugate() return swing, twist
def step(self, observation): # TODO: do something useful with the observation x, y, z = observation['position'] logger.info('Agent %s at position (x=%f, y=%f, z=%f)' % (self.agentId, x, y, z)) # Constant speed forward (Y-axis) linearVelocity = LVector3f(0.0, 1.0, 0.0) self.setLinearVelocity(linearVelocity) # Randomly change angular velocity (rotation around Z-axis) if self.rotationStepCounter > self.rotationsStepDuration: # End of rotation self.rotationStepCounter = -1 self.setAngularVelocity(np.zeros(3)) elif self.rotationStepCounter >= 0: # During rotation self.rotationStepCounter += 1 else: # No rotation, initiate at random if np.random.random() > 0.5: angularVelocity = np.zeros(3) angularVelocity[2] = np.random.uniform(low=-np.pi, high=np.pi) self.rotationStepCounter = 0 self.setAngularVelocity(angularVelocity)
def mid_point(pa, pb): # Find the mid pt between two pts. pax, pay, paz = pa pbx, pby, pbz = pb px, py, pz = (pax + pbx) / 2, (pay + pby) / 2, (paz + pbz) / 2 p = LVector3f(px, py, pz) p.normalize() return p