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
