def rayHit(pfrom, pto, geom):
"""
NOTE: this function is quite slow
find the nearest collision point between vec(pto-pfrom) and the mesh of nodepath
:param pfrom: starting point of the ray, Point3
:param pto: ending point of the ray, Point3
:param geom: meshmodel, a panda3d datatype
:return: None or Point3
author: weiwei
date: 20161201
"""
bulletworld = BulletWorld()
facetmesh = BulletTriangleMesh()
facetmesh.addGeom(geom)
facetmeshnode = BulletRigidBodyNode('facet')
bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True)
bullettmshape.setMargin(0)
facetmeshnode.addShape(bullettmshape)
bulletworld.attachRigidBody(facetmeshnode)
result = bulletworld.rayTestClosest(pfrom, pto)
if result.hasHit():
return result.getHitPos()
else:
return None
def bulletRayHit(pfrom, pto, geom):
"""
NOTE: this function is quite slow
find the nearest collision point between vec(pto-pfrom) and the mesh of nodepath
:param pfrom: starting point of the ray, Point3
:param pto: ending point of the ray, Point3
:param geom: meshmodel, a panda3d datatype
:return: None or Point3
author: weiwei
date: 20161201
"""
bulletworld = BulletWorld()
facetmesh = BulletTriangleMesh()
facetmesh.addGeom(geom)
facetmeshnode = BulletRigidBodyNode('facet')
bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True)
bullettmshape.setMargin(1e-6)
facetmeshnode.addShape(bullettmshape)
bulletworld.attach(facetmeshnode)
result = bulletworld.rayTestClosest(pfrom, pto)
if result.hasHit():
return result.getHitPos()
else:
return None
def genBulletCDMeshList(objcmlist, basenodepath=None, name='autogen'):
"""
generate the collision mesh of a nodepath using nodepathlist
this function suppose the nodepathlist is a list of models with many geomnodes
"Multi" means each nodepath in the nodepath list may have multiple nps (parent-child relations)
use genCollisionMeshMultiNp instead if the meshes have parent-child relations
:param nodepathlist: panda3d nodepathlist
:param basenodepath: the nodepath to compute relative transform, identity if none
:param name: the name of the rigidbody
:return: bulletrigidbody
author: weiwei
date: 20190514
"""
geombullnode = BulletRigidBodyNode(name)
for objcm in objcmlist:
gndcollection = objcm.objnp.findAllMatches("+GeomNode")
for gnd in gndcollection:
geom = copy.deepcopy(gnd.node().getGeom(0))
geomtf = gnd.getTransform(base.render)
if basenodepath is not None:
geomtf = gnd.getTransform(basenodepath)
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(0)
geombullnode.addShape(bullettmshape, geomtf)
return geombullnode
def isRayHitMeshAll(self, pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
geom = base.pg.packpandageom_fn(objcm.trimesh.vertices,
objcm.trimesh.face_normals,
objcm.trimesh.faces)
targetobjmesh = BulletTriangleMesh()
targetobjmesh.addGeom(geom)
bullettmshape = BulletTriangleMeshShape(targetobjmesh, dynamic=True)
bullettmshape.setMargin(1e-6)
targetobjmeshnode = BulletRigidBodyNode('facet')
targetobjmeshnode.addShape(bullettmshape)
self.world.attach(targetobjmeshnode)
result = self.world.rayTestAll(base.pg.npToV3(pfrom),
base.pg.npToV3(pto))
self.world.removeRigidBody(targetobjmeshnode)
if result.hasHits():
allhits = []
for hit in result.getHits():
allhits.append([hit.getHitPos(), -hit.getHitNormal()])
return allhits
else:
return []
def genBulletCDMeshMultiNp(nodepath, basenodepath=None, name='autogen'):
"""
generate the BulletCD mesh of a nodepath using nodepath
this function suppose the nodepath has multiple models with many geomnodes
use genBulletCDMeshMultiNp instead of genBulletCDMeshNp for generality
:param nodepath: the panda3d nodepath of the object
:param basenodepath: the nodepath to compute relative transform, identity if none
:param name: the name of the rigidbody
:return: bulletrigidbody
author: weiwei
date: 20161212, tsukuba
"""
gndcollection = nodepath.findAllMatches("**/+GeomNode")
geombullnode = BulletRigidBodyNode(name)
for gnd in gndcollection:
geom = gnd.node().getGeom(0)
geomtf = gnd.getTransform(base.render)
if basenodepath is not None:
geomtf = gnd.getTransform(basenodepath)
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(0)
geombullnode.addShape(bullettmshape, geomtf)
return geombullnode
def genCollisionMeshMultiNp(nodepath, basenodepath=None, name='autogen'):
"""
generate the collision mesh of a nodepath using nodepath
this function suppose the nodepath has multiple models with many geomnodes
use genCollisionMeshMultiNp instead of genCollisionMeshNp for generality
:param nodepath: the panda3d nodepath of the object
:param basenodepath: the nodepath to compute relative transform, identity if none
:param name: the name of the rigidbody
:return: bulletrigidbody
author: weiwei
date: 20161212, tsukuba
"""
gndcollection = nodepath.findAllMatches("**/+GeomNode")
geombullnode = BulletRigidBodyNode(name)
for gnd in gndcollection:
geom = gnd.node().getGeom(0)
geomtf = gnd.getTransform(base.render)
if basenodepath is not None:
geomtf = gnd.getTransform(basenodepath)
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(0)
geombullnode.addShape(bullettmshape, geomtf)
return geombullnode
def genCollisionMeshNp(nodepath, basenodepath=None, name='autogen'):
"""
generate the collision mesh of a nodepath using nodepath
this function suppose the nodepath is a single model with one geomnode
:param nodepath: the panda3d nodepath of the object
:param basenodepath: the nodepath to compute relative transform, identity if none
:param name: the name of the rigidbody
:return: bulletrigidbody
author: weiwei
date: 20161212, tsukuba
"""
geomnodepath = nodepath.find("**/+GeomNode")
geombullnode = BulletRigidBodyNode(name)
geom = geomnodepath.node().getGeom(0)
geomtf = nodepath.getTransform(base.render)
if basenodepath is not None:
geomtf = nodepath.getTransform(basenodepath)
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(0)
geombullnode.addShape(bullettmshape, geomtf)
return geombullnode
def gen_cdmesh_vvnf(vertices, vertex_normals, faces, name='auto'):
geom = da.pandageom_from_vvnf(vertices * SCALE_FOR_PRECISION, vertex_normals, faces)
bullet_triangles_mesh = BulletTriangleMesh()
bullet_triangles_mesh.addGeom(geom)
bullet_triangles_shape = BulletTriangleMeshShape(bullet_triangles_mesh, dynamic=True)
bullet_triangles_shape.setMargin(0)
geombullnode = BulletRigidBodyNode(name=name)
geombullnode.addShape(bullet_triangles_shape)
return geombullnode
def genBulletCDMesh(objcm, basenodepath=None, name='autogen'):
"""
generate the collision mesh of a nodepath using nodepath
this function suppose the nodepath has multiple models with many geomnodes
use genCollisionMeshMultiNp instead of genCollisionMeshNp for generality
:param nodepath: the panda3d nodepath of the object
:param basenodepath: the nodepath to compute relative transform, identity if none
:param name: the name of the rigidbody
:return: bulletrigidbody
author: weiwei
date: 20161212, tsukuba
"""
gndcollection = objcm.objnp.findAllMatches("+GeomNode")
geombullnode = BulletRigidBodyNode(name)
for gnd in gndcollection:
geom = copy.deepcopy(gnd.node().getGeom(0))
# vdata = geom.modifyVertexData()
# vertrewritter = GeomVertexRewriter(vdata, 'vertex')
# while not vertrewritter.isAtEnd():
# v = vertrewritter.getData3f()
# vertrewritter.setData3f(v[0]-objcm.com[0], v[1]-objcm.com[1], v[2]-objcm.com[2])
geomtf = gnd.getTransform(base.render)
if basenodepath is not None:
geomtf = gnd.getTransform(basenodepath)
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(-2)
geombullnode.addShape(bullettmshape, geomtf)
# rotmatpd4 = objcm.getMat(base.render)
# geombullnode.addShape(bullettmshape,
# TransformState.makeMat(rotmatpd4).
# setPos(rotmatpd4.xformPoint(Vec3(objcm.com[0], objcm.com[1], objcm.com[2]))))
from panda3d.core import Mat3, Mat4
# geombullnode.setTransform(TransformState.makeMat(Mat4(Mat3.identMat(), rotmatpd4.xformPoint(Vec3(objcm.com[0], objcm.com[1], objcm.com[2])))))
# print(objcm.com)
# print(geomtf.getMat())
# geombullnode.setTransform(TransformState.makeMat(geomtf.getMat()).setPos(geomtf.getMat().xformPoint(Vec3(objcm.com[0], objcm.com[1], objcm.com[2]))))
return geombullnode
def _createBulletShape_(self,np, apply_scale = True, apply_transform = True):
"""
@brief Creates a BulletTriangleMeshShape from the geometries in the model node. The passed
node path must point to a GeomNode type. The Geom objects are then added to a
BulletTriangleMesh object which is then used to initialize the Shape. Optionally,
the node's transform and scaled can be baked into the Geometries (recommended
according to the Panda3D guide https://www.panda3d.org/manual/index.php/Bullet_FAQ).
If you apply the scale and transform make sure not to apply them again to the shape or
the bullet node that contains the shape as it'll compound the effect.
@warning: If the underlying GeomNode contains multiple geometries each with a different
transform and scale then it's best to set apply_scale and apply_transform to True.
@param np: The nodepath containing the GeomNode
@param apply_scale: Boolean that allows choosing whether or not to apply the nodepath's scale to the geometries
@param apply_transform: Boolean that indicates whether or not to apply the nodepath's trasform to the geometries.
@return: A BulletTriangleMeshShape object containing the node's geometries
"""
geom_node = np.node()
if type(geom_node) is not GeomNode:
logging.error("Node %s is not a GeomNode node type"%(np.getName()))
return None
bullet_mesh = BulletTriangleMesh()
# Assembling transform
tr = TransformState.makeIdentity()
geom_scale = np.getScale()
if apply_transform:
tr = tr.setPos(np.getPos())
tr = tr.setQuat(np.getQuat())
sc = LPoint3(1)
tr = tr.setScale(sc)
if apply_scale: # baking scale into geometry data
tr = tr.setScale(geom_scale)
for geom in geom_node.getGeoms():
bullet_mesh.addGeom(geom,True,tr)
bullet_shape = BulletTriangleMeshShape(bullet_mesh,False,True,True)
bullet_shape.setMargin(GameObject.DEFAULT_COLLISION_MARGIN)
return bullet_shape
def genBullectCDMeshFromGeom(geom, name='autogen'):
"""
generate the collision mesh of a nodepath using geom
:param geom: the panda3d geom of the object
:param basenodepath: the nodepath to compute relative transform
:return: bulletrigidbody
author: weiwei
date: 20161212, tsukuba
"""
geomtf = TransformState.makeIdentity()
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
geombullnode = BulletRigidBodyNode(name)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(0)
geombullnode.addShape(bullettmshape, geomtf)
return geombullnode
def genCollisionMeshGeom(geom, name='autogen'):
"""
generate the collision mesh of a nodepath using geom
:param geom: the panda3d geom of the object
:param basenodepath: the nodepath to compute relative transform
:return: bulletrigidbody
author: weiwei
date: 20161212, tsukuba
"""
geomtf = TransformState.makeIdentity()
geombullmesh = BulletTriangleMesh()
geombullmesh.addGeom(geom)
geombullnode = BulletRigidBodyNode(name)
bullettmshape = BulletTriangleMeshShape(geombullmesh, dynamic=True)
bullettmshape.setMargin(0)
geombullnode.addShape(bullettmshape, geomtf)
return geombullnode
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))