def matchPose(self, pose_rig): for node, parent in pose_rig.exposed_joints: if node.getName() not in joints_config: continue joint_config = joints_config[node.getName()] if 'joints' not in joint_config: continue joints = joint_config['joints'] if len(joints) == 0: continue box_np = self.root.find(node.getName()) if len(joints) > 1: pos = node.getPos(pose_rig.actor) hpr = node.getHpr(pose_rig.actor) box_np.setPosHpr(self.root, pos, hpr) else: joint = joints.keys()[0] child_node, child_parent = next((child_node, child_parent) for child_node, child_parent in pose_rig.exposed_joints if child_node.getName() == joint) box_np.setPos(child_parent, child_node.getPos(child_parent) / 2.0) quat = Quat() lookAt(quat, child_node.getPos(child_parent), Vec3.up()) box_np.setQuat(child_parent, quat) box_np.node().clearForces()
def create_colliders(root, pose_rig, joints_config): for node, parent in pose_rig.exposed_joints: if node.getName() not in joints_config: continue joint_config = joints_config[node.getName()] if "joints" not in joint_config: continue joints = joint_config["joints"] if len(joints) == 0: continue mass = joint_config["mass"] if "mass" in joint_config else 1 box_rb = BulletRigidBodyNode(node.getName()) box_rb.setMass(1.0) # box_rb.setLinearDamping(0.2) # box_rb.setAngularDamping(0.9) # box_rb.setFriction(1.0) # box_rb.setAnisotropicFriction(1.0) # box_rb.setRestitution(0.0) for joint in joints: child_node, child_parent = next( (child_node, child_parent) for child_node, child_parent in pose_rig.exposed_joints if child_node.getName() == joint ) pos = child_node.getPos(child_parent) width = pos.length() / 2.0 scale = Vec3(3, width, 3) shape = BulletBoxShape(scale) quat = Quat() lookAt(quat, child_node.getPos(child_parent), Vec3.up()) if len(joints) > 1: transform = TransformState.makePosHpr(child_node.getPos(child_parent) / 2.0, quat.getHpr()) else: transform = TransformState.makeHpr(quat.getHpr()) box_rb.addShape(shape, transform) box_np = root.attachNewNode(box_rb) if len(joints) > 1: pos = node.getPos(pose_rig.actor) hpr = node.getHpr(pose_rig.actor) box_np.setPosHpr(root, pos, hpr) else: box_np.setPos(child_parent, child_node.getPos(child_parent) / 2.0) box_np.lookAt(child_parent, child_node.getPos(child_parent)) yield box_np
def go_to(self, target, duration, position, direction, up): if up is None: up = LVector3d.up() frame = CelestiaBodyFixedReferenceFrame(target) up = frame.get_orientation().xform(up) if isclose(abs(up.dot(direction)), 1.0): print("Warning: lookat vector identical to up vector") orientation = LQuaterniond() lookAt(orientation, direction, up) self.move_and_rotate_camera_to(position, orientation, duration=duration)
def guide_missile(self, task): try: quat = Quat() lookAt(quat, self.target.np.getPos() - self.missile.anp.getPos(), Vec3.up()) self.missile.anp.setQuat(quat) fwd = quat.getForward() fwd.normalize() mvel = self.missile.anpo.getVelocity().length() self.missile.anpo.setVelocity(fwd*mvel) except: return task.done return task.cont
def go_to(self, target, duration, position, direction, up): if up is None: up = LVector3d.up() frame = SynchroneReferenceFrame(target) up = frame.get_orientation().xform(up) if isclose(abs(up.dot(direction)), 1.0): print("Warning: lookat vector identical to up vector") else: # Make the up vector orthogonal to the direction using Gram-Schmidt up = up - direction * up.dot(direction) orientation = LQuaterniond() lookAt(orientation, direction, up) self.move_and_rotate_to(position, orientation, duration=duration)
def go_to_surface(self, duration = None, height=1.001): if not self.ui.selected: return target = self.ui.selected if duration is None: duration = settings.slow_move print("Go to surface", target.get_name()) self.ui.sync_selected() center = target.get_rel_position_to(self.ship._global_position) direction = self.ship.get_pos() - center new_orientation = LQuaterniond() lookAt(new_orientation, direction) height = target.get_height_under(self.ship.get_pos()) + 10 * units.m new_position = center + new_orientation.xform(LVector3d(0, height, 0)) self.move_and_rotate_to(new_position, new_orientation, duration=duration)
def alignCarTowardsForceField(self, dt, p0, p1): modelPos = self.model.getPos() path = self.pointSegmentShortestPath(modelPos, p1, p0, bounds=True, boundsNone=True) if path is None: return None Len = path.length() if Len < 0.2: Len = 0.2 delta = 100.0 vec = (p1 - p0).normalized() vec *= delta originalQuat = self.model.getQuat() quat = Quat(originalQuat) other_quat = Quat(originalQuat) lookAt(quat, vec, Vec3().up()) lookAt(other_quat, -vec, Vec3().up()) if not quat.almostSameDirection(originalQuat, 0.5): [p0, p1] = [p1, p0] vec = (p0 - p1).normalized() * delta quat = other_quat if not quat.almostSameDirection(originalQuat, 0.5): return None # Make car go more towards the selected path at intersection angleSimilarityFactor = abs( self.direction.normalized().dot(vec.normalized()) + path.length()) # Go closer to road pos_strength = dt * angleSimilarityFactor pos_strength /= (Len * Len) if Len > 1.0 else 1.0 pos_strength *= ROAD_POS_STRENGTH pos_strength = sorted([0, pos_strength, 1])[1] position = modelPos - path * pos_strength strength = ALIGN_STRENGTH * dt / ((Len * Len) if Len > 1 else 1) strength = sorted([0, strength, 1])[1] self.car_shadow.setPos(self.model.getPos()) self.car_shadow.setHpr(self.model.getHpr()) self.model.setQuat(quat) targetHpr = self.model.getHpr(self.car_shadow) self.model.setQuat(originalQuat) currentHpr = self.model.getHpr(self.car_shadow) self.model.setHpr(self.car_shadow, targetHpr * strength + currentHpr * (1.0 - strength)) quat = self.model.getQuat() # Put velocity in direction of road blend_velocity = BLEND_VELOCITY_FAC_TOWARDS_ROAD * dt velocity = self.velocity.project(vec) * ( blend_velocity) + self.velocity * (1.0 - blend_velocity) self.model.setPos(position) self.model.setQuat(quat) self.velocity = velocity