def getCollisionShapeFromModel(model, mode='box', defaultCentered=False):
#NOTE: make sure the position is relative to the center of the object
minBounds, maxBounds = model.getTightBounds()
offset = minBounds + (maxBounds - minBounds) / 2.0
transform = TransformState.makeIdentity()
if mode == 'mesh':
# Use exact triangle mesh approximation
mesh = BulletTriangleMesh()
geomNodes = model.findAllMatches('**/+GeomNode')
for nodePath in geomNodes:
geomNode = nodePath.node()
for n in range(geomNode.getNumGeoms()):
geom = geomNode.getGeom(n)
mesh.addGeom(geom)
shape = BulletTriangleMeshShape(mesh, dynamic=False)
transform = model.getTransform()
elif mode == "sphere":
minBounds, maxBounds = model.getTightBounds()
dims = maxBounds - minBounds
radius = np.sqrt(np.square(dims[0]) + np.square(dims[1])) / 2.0
height = dims[2]
shape = BulletCapsuleShape(radius, 2 * radius)
if not defaultCentered:
transform = TransformState.makePos(offset)
elif mode == "hull":
shape = BulletConvexHullShape()
geomNodes = model.findAllMatches('**/+GeomNode')
for nodePath in geomNodes:
geomNode = nodePath.node()
for n in range(geomNode.getNumGeoms()):
geom = geomNode.getGeom(n)
shape.addGeom(geom)
elif mode == 'box':
# Bounding box approximation
minBounds, maxBounds = model.getTightBounds()
dims = maxBounds - minBounds
shape = BulletBoxShape(Vec3(dims.x / 2, dims.y / 2, dims.z / 2))
if not defaultCentered:
transform = TransformState.makePos(offset)
elif mode == 'capsule':
minBounds, maxBounds = model.getTightBounds()
dims = maxBounds - minBounds
radius = np.sqrt(np.square(dims[0]) + np.square(dims[1])) / 2.0
height = dims[2]
shape = BulletCapsuleShape(radius, height - 2 * radius)
if not defaultCentered:
transform = TransformState.makePos(offset)
else:
raise Exception(
'Unknown mode type for physic object collision shape: %s' % (mode))
return shape, transform
def __init__(self,
initor,
cdtype="triangles",
mass=.3,
restitution=0,
allow_deactivation=False,
allow_ccd=True,
friction=.2,
dynamic=True,
name="rbd"):
"""
TODO: triangles do not seem to work (very slow) in the github version (20210418)
Use convex if possible
:param initor: could be itself (copy), or an instance of collision model
:param type: triangle or convex
:param mass:
:param restitution: bounce parameter
:param friction:
:param dynamic: only applicable to triangle type, if an object does not move with force, it is not dynamic
:param name:
author: weiwei
date: 20190626, 20201119
"""
super().__init__(name)
if isinstance(initor, gm.GeometricModel):
if initor._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
self.com = initor.objtrm.center_mass * base.physics_scale
self.setMass(mass)
self.setRestitution(restitution)
self.setFriction(friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allow_deactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(.01 * base.physics_scale)
self.setAngularSleepThreshold(.01 * base.physics_scale)
else:
self.setDeactivationEnabled(False)
if allow_ccd: # continuous collision detection
self.setCcdMotionThreshold(1e-7)
self.setCcdSweptSphereRadius(0.0005 * base.physics_scale)
geom_np = initor.objpdnp.getChild(0).find("+GeomNode")
geom = copy.deepcopy(geom_np.node().getGeom(0))
vdata = geom.modifyVertexData()
vertices = copy.deepcopy(
np.frombuffer(vdata.modifyArrayHandle(0).getData(),
dtype=np.float32))
vertices.shape = (-1, 6)
vertices[:, :3] = vertices[:, :3] * base.physics_scale - self.com
vdata.modifyArrayHandle(0).setData(
vertices.astype(np.float32).tobytes())
geomtf = geom_np.getTransform()
geomtf = geomtf.setPos(geomtf.getPos() * base.physics_scale)
if cdtype == "triangles":
geombmesh = BulletTriangleMesh()
geombmesh.addGeom(geom)
bulletshape = BulletTriangleMeshShape(geombmesh,
dynamic=dynamic)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype == "convex":
bulletshape = BulletConvexHullShape(
) # TODO: compute a convex hull?
bulletshape.addGeom(geom, geomtf)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype == 'box':
minx = min(vertices[:, 0])
miny = min(vertices[:, 1])
minz = min(vertices[:, 2])
maxx = max(vertices[:, 0])
maxy = max(vertices[:, 1])
maxz = max(vertices[:, 2])
pcd_box = CollisionBox(Point3(minx, miny, minz),
Point3(maxx, maxy, maxz))
bulletshape = BulletBoxShape.makeFromSolid(pcd_box)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
else:
raise NotImplementedError
pd_homomat = geomtf.getMat()
pd_com_pos = pd_homomat.xformPoint(
Vec3(self.com[0], self.com[1], self.com[2]))
np_homomat = dh.pdmat4_to_npmat4(pd_homomat)
np_com_pos = dh.pdv3_to_npv3(pd_com_pos)
np_homomat[:3, 3] = np_com_pos # update center to com
self.setTransform(
TransformState.makeMat(dh.npmat4_to_pdmat4(np_homomat)))
elif isinstance(initor, BDBody):
self.com = initor.com.copy()
self.setMass(initor.getMass())
self.setRestitution(initor.restitution)
self.setFriction(initor.friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allow_deactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(.01 * base.physics_scale)
self.setAngularSleepThreshold(.01 * base.physics_scale)
else:
self.setDeactivationEnabled(False)
if allow_ccd:
self.setCcdMotionThreshold(1e-7)
self.setCcdSweptSphereRadius(0.0005 * base.physics_scale)
np_homomat = copy.deepcopy(initor.get_homomat())
np_homomat[:3, 3] = np_homomat[:3, 3] * base.physics_scale
self.setTransform(
TransformState.makeMat(dh.npmat4_to_pdmat4(np_homomat)))
self.addShape(initor.getShape(0), initor.getShapeTransform(0))
def __init__(self, objinit, cdtype="triangle", mass=.3, restitution=0, allowdeactivation=False, allowccd=True,
friction=.2, dynamic=True, name="rbd"):
"""
:param objinit: could be itself (copy), or an instance of collision model
:param type: triangle or convex
:param mass:
:param restitution: bounce parameter
:param friction:
:param dynamic: only applicable to triangle type, if an object does not move with force, it is not dynamic
:param name:
author: weiwei
date: 20190626, 20201119
"""
super().__init__(name)
if isinstance(objinit, gm.GeometricModel):
if objinit._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
self.com = objinit.objtrm.center_mass
self.setMass(mass)
self.setRestitution(restitution)
self.setFriction(friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allowdeactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(0.001)
self.setAngularSleepThreshold(0.001)
else:
self.setDeactivationEnabled(False)
if allowccd: # continuous collision detection
self.setCcdMotionThreshold(1e-6)
self.setCcdSweptSphereRadius(0.0005)
gnd = objinit.objpdnp.getChild(0).find("+GeomNode")
geom = copy.deepcopy(gnd.node().getGeom(0))
vdata = geom.modifyVertexData()
vertrewritter = GeomVertexRewriter(vdata, 'vertex')
while not vertrewritter.isAtEnd(): # shift local coordinate to geom to correctly update dynamic changes
v = vertrewritter.getData3f()
vertrewritter.setData3f(v[0] - self.com[0], v[1] - self.com[1], v[2] - self.com[2])
geomtf = gnd.getTransform()
if cdtype is "triangle":
geombmesh = BulletTriangleMesh()
geombmesh.addGeom(geom)
bulletshape = BulletTriangleMeshShape(geombmesh, dynamic=dynamic)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype is "convex":
bulletshape = BulletConvexHullShape() # TODO: compute a convex hull?
bulletshape.addGeom(geom, geomtf)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
else:
raise NotImplementedError
pdmat4 = geomtf.getMat()
pdv3 = pdmat4.xformPoint(Vec3(self.com[0], self.com[1], self.com[2]))
homomat = dh.pdmat4_to_npmat4(pdmat4)
pos = dh.pdv3_to_npv3(pdv3)
homomat[:3, 3] = pos # update center to com
self.setTransform(TransformState.makeMat(dh.npmat4_to_pdmat4(homomat)))
elif isinstance(objinit, BDBody):
self.com = objinit.com.copy()
self.setMass(objinit.getMass())
self.setRestitution(objinit.restitution)
self.setFriction(objinit.friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allowdeactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(0.001)
self.setAngularSleepThreshold(0.001)
else:
self.setDeactivationEnabled(False)
if allowccd:
self.setCcdMotionThreshold(1e-6)
self.setCcdSweptSphereRadius(0.0005)
self.setTransform(TransformState.makeMat(dh.npmat4_to_pdmat4(objinit.gethomomat())))
self.addShape(objinit.getShape(0), objinit.getShapeTransform(0))