def test_pso_create_shape():
ts1 = TransformState.makePos((1, 3, 5))
ts2 = TransformState.makePos((2, 4, 6))
par2 = (Vec3(4, 1, 7), )
obj0 = RBSO("0")
obj1 = RBSO("1")
obj2 = RBSO("2")
obj0.set_shape("Box")
obj1.set_shape(("Box", (), ts1))
obj2.set_shape(("Box", par2, ts2))
obj0.create_shape()
obj1.create_shape()
obj2.create_shape()
sh0 = obj0.node().getShape(0)
sh1 = obj1.node().getShape(0)
sh2 = obj2.node().getShape(0)
assert isinstance(sh0, BulletBoxShape)
assert isinstance(sh1, BulletBoxShape)
assert isinstance(sh2, BulletBoxShape)
assert obj0.node().getShapeMat(0) == Mat4.identMat()
assert obj1.node().getShapeMat(0) == ts1.getMat()
assert obj2.node().getShapeMat(0) == ts2.getMat()
assert sh0.getHalfExtentsWithMargin() == Vec3(0.5, 0.5, 0.5)
assert sh1.getHalfExtentsWithMargin() == Vec3(0.5, 0.5, 0.5)
assert sh2.getHalfExtentsWithMargin() == par2
def test_boxshape_scale():
S0 = BoxShape()
args = Vec3(1, 2, 3)
S1 = BoxShape([args])
scale = Vec3(5, 6, 7)
S0.scale(scale)
S1.scale(scale)
assert S0[0][0] == Vec3(*imap(mul, BoxShape.args0.values()[0], scale))
assert S1[0][0] == Vec3(*imap(mul, args, scale))
def test_boxshape_shift():
S0 = BoxShape()
args = Vec3(1, 2, 3)
S1 = BoxShape([args])
pos = Vec3(10, 20, 30)
quat = Quat(1, 1, 0, 0)
quat.normalize()
ones = Vec3(1, 1, 1)
T = TransformState.makePosQuatScale(pos, quat, ones)
S0.shift(pos=pos, quat=quat)
S1.shift(pos=pos, quat=quat)
S1.shift(pos=pos, quat=quat)
assert S0[1] == T
assert S1[1] == T.compose(T)
def test_boxshape_transform():
args = Vec3(1, 2, 3)
S0 = BoxShape([args])
scale = Vec3(5, 6, 7)
pos = Vec3(10, 20, 30)
quat = Quat(1, 1, 0, 0)
quat.normalize()
ones = Vec3(1, 1, 1)
rbso = RBSO("rbso")
rbso.set_scale(scale)
rbso.set_pos(pos)
rbso.set_quat(quat)
S0.transform(rbso)
T = TransformState.makePosQuatScale(pos, quat, ones)
assert S0[0][0] == Vec3(*imap(mul, args, scale))
assert S0[1] == T
def __init__(self):
# Parent init.
super(Picker, self).__init__()
self.disableMouse()
# Picker stuff.
self.contact_margin = Vec3(0.01, 0.01, 0.01)
self.parser = None
self.marked = None
self.attached_pairs = set()
self.contacts = None
self.contact_points = None
self.contact_bottoms = None
self.compound_components = []
self.compound_objects = []
self.joints = JointManager()
self.wire_attrib = RenderModeAttrib.make(RenderModeAttrib.MWireframe,
4.)
self.attachment_colors = (Vec4(0.1, 0.1, 1.,
1.), Vec4(0.1, 0.8, 0.1,
1.), Vec4(1., 0.1, 1., 1.),
Vec4(1., 0.5, 0.1, 1.), Vec4(1., 1., 1., 1.))
self.max_attach = 999
self.permanent_events += ["mouse1"]
# Make cursor dot.
self.cursor = self._build_cursor("cross")
s = 0.08
self.cursor.setScale(s, s, s)
self.cursor.setColor(1, 1, 1, 1)
self.cursor.reparentTo(self.aspect2d)
self.taskMgr.add(self.draw_cursor2d, "draw_cursor2d")
self.permanent_tasks.append("draw_cursor2d")
def test_boxshape():
# __init__
S0 = BoxShape()
args = Vec3(1, 2, 3)
S1 = BoxShape([args])
assert S0[0][0] == BoxShape.args0.values()[0]
assert S1[0][0] == args
class BoxShape(BaseShape):
""" BulletBoxShape."""
bshape = p3b.BulletBoxShape
args0 = OrderedDict((("HalfExtentsWithMargin", Vec3(0.5, 0.5, 0.5)), ))
def scale(self, scale):
args = (Vec3(*[s * a for s, a in izip(scale, self[0][0])]), )
self[0] = args
class PlaneShape(BaseShape):
""" BulletPlaneShape."""
bshape = p3b.BulletPlaneShape
args0 = OrderedDict((("PlaneNormal", Vec3(0, 0, 1)), ("PlaneConstant", 0)))
def scale(self, scale):
s = self[0][0].dot(Vec3(*scale)) / self[0][0].length()
args = (self[0][0], self[0][1] * s)
self[0] = args
def _make_connector(self, parent, points, extent, name):
""" Makes connector object."""
connector = self.base_connector.copy()
scale = Vec3(*(np.ptp(points, axis=0)))
scale_extended = scale + extent
pos = Point3(*(np.min(points, axis=0) + scale / 2.))
connector.apply_prop(dict(name=name, scale=scale_extended, pos=pos),
other=self.scene)
connector.wrtReparentTo(parent)
return connector
def show_attachment(self, ij, f_on):
""" Turns on/off attachment graphic."""
if f_on:
parent = self.contact_bottoms[ij]
points = self.contact_points[ij]
extent = Vec3(0.15, 0.15, 0.15)
name = "connector_%d-%d" % ij
self.connectors[ij] = self._make_connector(parent, points, extent,
name)
self.connectors[ij].init_tree(tags=("model", ))
else:
self.connectors.pop(ij).removeNode()
def bump(self, task):
""" Task: perform bump."""
mag0 = Vec3(0, 0, 1. / self.bbase.sim_par["size"]) * 10.
pos = Point3(-1, 0, 0)
nodes = self.background.descendants()
bodies = [n.node() for n in nodes if n.type_ is BulletRigidBodyNode]
for body in bodies:
mag = mag0 * body.getMass()
print mag
body.applyForce(mag, pos)
#BP()
return task.done
def __init__(self,
world,
scene,
margin=(0.01, 0.01, 0.01),
type_=PSO,
names=None):
self.scene = scene
self.top = self.scene.getTop()
# Store initial state.
self.cache = self.scene.store_tree()
self.margin = Vec3(*margin)
bodies = self.scene.descendants(type_=type_, names=names)
super(ContactDetector, self).__init__(world, bodies)
self.detect()
def show_marked(self, node, f_on):
""" Turns on/off marked graphic."""
if f_on:
extent = Vec3(0.15, 0.15, 0.15)
name = "mark"
self.mark = self._make_mark(node, extent, name)
self.mark.init_tree(tags=("model", ))
# Exclude object from casting shadows
self.mark.hide(self.shadow_mask)
self.mark.setTransparency(TransparencyAttrib.MAlpha)
self.mark.setDepthWrite(False)
self.mark.setBin("fixed", 0, priority=5)
else:
self.mark.removeNode()
def test_apply_prop_read_prop_SSO():
sso = SSO("foo")
other = SSO("other")
other.setPos(100, 200, 300)
other.setHpr(23, 20, 100)
other.setScale(6, 2, 9)
prop0 = {
"name": "testname",
"pos": Point3(1, 2, 3),
"quat": Quat(2**0.5, 2**0.5, 0, 0),
"scale": Vec3(10, 9, 8),
}
sso.setName(prop0["name"])
sso.setPos(prop0["pos"])
sso.setQuat(prop0["quat"])
sso.setScale(prop0["scale"])
assert prop0 == sso.read_prop()
oprop = sso.read_prop(other=other)
sso.wrtReparentTo(other)
assert sso.read_prop() == oprop
def scale(self, scale):
args = (Vec3(*[s * a for s, a in izip(scale, self[0][0])]), )
self[0] = args
def scale(self, scale):
s = self[0][0].dot(Vec3(*scale)) / self[0][0].length()
args = (self[0][0], self[0][1] * s)
self[0] = args
def test_pso_get_shape():
shape = ("Box", (Vec3(4, 1, 7), ), TransformState.makePos((2, 4, 6)))
obj = RBSO("rso")
obj.set_shape(shape)
assert obj.get_shape() == shape
def shift(self, pos=(0, 0, 0), quat=(1, 0, 0, 0)):
""" Translate and rotate shape's transform."""
ones = Vec3(1, 1, 1)
T = TransformState.makePosQuatScale(pos, quat, ones)
xform = T.compose(self[1])
self[1] = xform