def loadMesh(shape_filename):
"""
loads a vtk .vtp file and returns a Bullet concave shape
WARNING triangles cells assumed!
"""
reader = vtk.vtkXMLPolyDataReader()
reader.SetFileName(shape_filename)
reader.Update()
polydata = reader.GetOutput()
points = polydata.GetPoints().GetData()
num_points = points.GetNumberOfTuples()
num_triangles = polydata.GetNumberOfCells()
keep = None
shape = None
if polydata.GetCellType(0) == 5:
apoints = np.empty((num_points, 3))
for i in range(0, points.GetNumberOfTuples()):
p = points.GetTuple(i)
apoints[i, 0] = p[0]
apoints[i, 1] = p[1]
apoints[i, 2] = p[2]
aindices = np.empty((num_triangles, 3), dtype=np.int32)
for i in range(0, num_triangles):
c = polydata.GetCell(i)
aindices[i, 0] = c.GetPointIds().GetId(0)
aindices[i, 1] = c.GetPointIds().GetId(1)
aindices[i, 2] = c.GetPointIds().GetId(2)
tri = btTriangleIndexVertexArray(apoints, aindices)
shape = btGImpactMeshShape(tri)
shape.updateBound()
keep = tri, apoints, aindices
else: # assume convex shape
coors = dict()
for i in range(0, points.GetNumberOfTuples()):
coors[points.GetTuple(i)] = 1
shape = btConvexHullShape()
for p in coors:
shape.addPoint(btVector3(*p))
return keep, shape
T = 20 # end time
h = 0.005 # time step
g = 9.81 # gravity
theta = 0.5 # theta scheme
#
# dynamical system
#
position_init = 10
velocity_init = 0
if (True):
box = btConvexHullShape()
box.addPoint(btVector3(-1.0, 1.0, -1.0))
box.addPoint(btVector3(-1.0, -1.0, -1.0))
box.addPoint(btVector3(-1.0, -1.0, 1.0))
box.addPoint(btVector3(-1.0, 1.0, 1.0))
box.addPoint(btVector3(1.0, 1.0, 1.0))
box.addPoint(btVector3(1.0, 1.0, -1.0))
box.addPoint(btVector3(1.0, -1.0, -1.0))
box.addPoint(btVector3(1.0, -1.0, 1.0))
else:
box = btBoxShape(btVector3(1.0, 1.0, 1.0))
# a bullet shape with a mass (1.0)
box1 = BulletWeightedShape(box, 1.0)
# A Bullet Dynamical System : a shape + a mass + position and velocity
body = BulletDS(box1,
def importObject(self, name, translation, orientation,
velocity, contactors, mass, inertia, body_class, shape_class):
if body_class is None:
body_class = BulletDS
if self._broadphase is not None and 'input' in self._data:
if mass == 0.:
# a static object
rbase = btQuaternion(orientation[1],
orientation[2],
orientation[3],
orientation[0])
for c in contactors:
(w, x, y, z) = c.orientation
c_orientation = btQuaternion(x, y, z, w)
rc_orientation = mul(rbase, c_orientation)
c_origin = btVector3(c.translation[0],
c.translation[1],
c.translation[2])
rc_origin = quatRotate(rbase, c_origin)
rc_sorigin = btVector3(rc_origin.x() + translation[0],
rc_origin.y() + translation[1],
rc_origin.z() + translation[2])
static_cobj = btCollisionObject()
static_cobj.setCollisionFlags(
btCollisionObject.CF_STATIC_OBJECT)
self._static_origins.append(rc_sorigin)
self._static_orientations.append(rc_orientation)
transform = btTransform(rc_orientation)
transform.setOrigin(rc_sorigin)
self._static_transforms.append(transform)
static_cobj.setWorldTransform(transform)
static_cobj.setCollisionShape(
self._shape.get(c.name))
self._static_cobjs.append(static_cobj)
self._broadphase.addStaticObject(static_cobj,
int(c.group))
else:
# a moving object
bws = BulletWeightedShape(
self._shape.get(contactors[0].name), mass)
if inertia is not None:
bws.setInertia(inertia[0], inertia[1], inertia[2])
body = body_class(bws,
translation + orientation,
velocity,
contactors[0].translation,
contactors[0].orientation,
contactors[0].group)
for contactor in contactors[1:]:
shape_id = self._shapeid[contactor.name]
body.addCollisionShape(self._shape.get(contactor.name),
contactor.translation,
contactor.orientation,
contactor.group)
# set external forces
self._set_external_forces(body)
# add the dynamical system to the non smooth
# dynamical system
nsds = self._broadphase.model().nonSmoothDynamicalSystem()
nsds.insertDynamicalSystem(body)
nsds.setOSI(body, self._osi)
nsds.setName(body, str(name))
def get(self, shape_name, shape_class=None, face_class=None, edge_class=None):
if not shape_name in self._shapes:
# load shape if it is an existing file
if not isinstance(self.url(shape_name), str) and \
not 'primitive' in self.attributes(shape_name):
# assume a vtp file (xml) stored in a string buffer
if self.attributes(shape_name)['type'] == 'vtk':
if self.shape(shape_name).dtype == h5py.new_vlen(str):
with tmpfile() as tmpf:
data = self.shape(shape_name)[:][0]
tmpf[0].write(data)
tmpf[0].flush()
self._tri[index], self._shape[index] = loadMesh(
tmpf[1])
else:
assert False
elif self.attributes(shape_name)['type'] in['step']:
from OCC.STEPControl import STEPControl_Reader
from OCC.BRep import BRep_Builder
from OCC.TopoDS import TopoDS_Compound
from OCC.IFSelect import IFSelect_RetDone, IFSelect_ItemsByEntity
builder = BRep_Builder()
comp = TopoDS_Compound()
builder.MakeCompound(comp)
step_name = self.attributes(shape_name)['step']
with tmpfile(contents=self.shape(step_name)[:][0]) as tmpf:
step_reader = STEPControl_Reader()
status = step_reader.ReadFile(tmpf[1])
if status == IFSelect_RetDone: # check status
failsonly = False
step_reader.PrintCheckLoad(failsonly, IFSelect_ItemsByEntity)
step_reader.PrintCheckTransfer(failsonly, IFSelect_ItemsByEntity)
ok = step_reader.TransferRoot(1)
nbs = step_reader.NbShapes()
l=[]
for i in range(1, nbs+1):
shape = step_reader.Shape(i)
builder.Add(comp, shape)
self._shapes[shape_name] = comp
self._io._keep.append(self._shapes[shape_name])
elif self.attributes(shape_name)['type'] in['brep']:
if not 'contact' in self.attributes(shape_name):
# the reference brep
if shape_class is None:
brep_class = OccContactShape
else:
brep_class = shape_class
if 'occ_indx' in self.attributes(shape_name):
from OCC.BRepTools import BRepTools_ShapeSet
shape_set = BRepTools_ShapeSet()
shape_set.ReadFromString(self.shape(shape_name)[:][0])
the_shape = shape_set.Shape(shape_set.NbShapes())
location = shape_set.Locations().Location(self.attributes(shape_name)['occ_indx'])
the_shape.Location(location)
brep = brep_class()
brep.setData(the_shape)
else:
# raw brep
brep = brep_class()
brep.importBRepFromString(self.shape(shape_name)[:][0])
self._shapes[shape_name] = brep
self._io._keep.append(self._shapes[shape_name])
else:
# a contact on a brep
assert 'contact' in self.attributes(shape_name)
assert 'index' in self.attributes(shape_name)
assert 'brep' in self.attributes(shape_name)
contact_index = self.attributes(shape_name)['index']
ref_brep = self.get(self.attributes(shape_name)['brep'], shape_class)
if self.attributes(shape_name)['contact'] == 'Face':
if face_class is None:
face_maker = OccContactFace
else:
face_maker = face_class
self._shapes[shape_name] = \
face_maker(ref_brep,
contact_index)
elif self.attributes(shape_name)['contact'] == 'Edge':
if edge_class is None:
edge_maker = OccContactEdge
else:
edge_maker = edge_class
self._shapes[shape_name] = \
edge_maker(ref_brep,
contact_index)
self._io._keep.append(self._shapes[shape_name])
else:
# a convex point set
convex = btConvexHullShape()
for points in self.shape(shape_name):
convex.addPoint(btVector3(float(points[0]),
float(points[1]),
float(points[2])))
self._shapes[shape_name] = convex
elif isinstance(self.url(shape_name), str) and \
os.path.exists(self.url(shape_name)):
self._tri[shape_name], self._shapes[shape_name] = loadMesh(
self.url(shape_name))
else:
# it must be a primitive with attributes
if isinstance(self.url(shape_name), str):
name = self.url(shape_name)
attrs = [float(x) for x in self.shape(shape_name)[0]]
else:
name = self.attributes(shape_name)['primitive']
attrs = [float(x) for x in self.shape(shape_name)[0]]
primitive = self._primitive[name]
if name in ['Box']:
self._shapes[shape_name] = primitive(btVector3(attrs[0] / 2,
attrs[1] / 2,
attrs[2] / 2))
elif name in ['Cylinder']:
self._shapes[shape_name] = primitive(btVector3(attrs[0],
attrs[1] / 2,
attrs[0]))
# elif name in ['Compound']:
# obj1 = attrs[0]
# orig1 = attrs[1:4]
# orie1 = attrs[4:8]
# obj2 = attrs[8]
# orig2 = attrs[9:12]
# orie2 = attrs[12:16]
# bcols = btCompoundShape()
# bcols.addChildShape(...
else:
self._shapes[shape_name] = primitive(*attrs)
return self._shapes[shape_name]