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
