def rayhit_closet(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
ray = OdeRayGeom(length=1)
length, dir = rm.unit_vector(pto - pfrom, toggle_length=True)
ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2])
ray.setLength(length)
contact_entry = OdeUtil.collide(ray, tgt_cdmesh, max_contacts=10)
contact_points = [
da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()
]
min_id = np.argmin(np.linalg.norm(pfrom - np.array(contact_points),
axis=1))
contact_normals = [
da.pdv3_to_npv3(contact_entry.getContactGeom(i).getNormal())
for i in range(contact_entry.getNumContacts())
]
return contact_points[min_id], contact_normals[min_id]
def __simulationTask(self, task):
self.DTA += globalClock.getDt()
numSteps = int(self.DTA / self.DTAStep)
if numSteps > 10:
self.notify.warning('phyics steps = %d' % numSteps)
startTime = globalClock.getRealTime()
while self.DTA >= self.DTAStep:
if self.deterministic:
OdeUtil.randSetSeed(0)
self.DTA -= self.DTAStep
self.preStep()
self.simulate()
self.postStep()
if self.canRender:
self.placeBodies()
return task.cont
def is_collided(obj_ode_trimesh0, obj_ode_trimesh1):
"""
check if two odetrimesh are collided
:param obj_ode_trimesh00: an instance of OdeTriMeshGeom
:param obj_ode_trimesh1: an instance of OdeTriMeshGeom
:return:
author: weiwei
date: 20210118, 20211215
"""
contact_entry = OdeUtil.collide(obj_ode_trimesh0, obj_ode_trimesh1, max_contacts=10)
contact_points = [da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()]
return (True, contact_points) if len(contact_points) > 0 else (False, contact_points)
def setupSimulation(self):
DistributedMinigamePhysicsWorld.DistributedMinigamePhysicsWorld.setupSimulation(
self)
self.world.setGravity(0, 0, -32.174)
self.world.setAutoDisableFlag(1)
self.world.setAutoDisableLinearThreshold(0.5 * MetersToFeet)
self.world.setAutoDisableAngularThreshold(OdeUtil.getInfinity())
self.world.setAutoDisableSteps(10)
self.world.setCfm(1e-05 * MetersToFeet)
self.world.initSurfaceTable(3)
self.world.setSurfaceEntry(0, 1, 0.2, 0, 0, 0, 0, 0, 0.1)
self.world.setSurfaceEntry(0, 0, 0.1, 0.9, 0.1, 0, 0, 0, 0)
self.world.setSurfaceEntry(0, 2, 0.9, 0.9, 0.1, 0, 0, 0, 0)
self.floor = OdePlaneGeom(self.space, Vec4(0.0, 0.0, 1.0, -20.0))
self.floor.setCollideBits(self.allTiresMask)
self.floor.setCategoryBits(self.floorMask)
self.westWall = OdePlaneGeom(
self.space, Vec4(1.0, 0.0, 0.0, IceGameGlobals.MinWall[0]))
self.westWall.setCollideBits(self.allTiresMask)
self.westWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.westWall, self.fenceSurfaceType)
self.space.setCollideId(self.westWall, self.wallCollideId)
self.eastWall = OdePlaneGeom(
self.space, Vec4(-1.0, 0.0, 0.0, -IceGameGlobals.MaxWall[0]))
self.eastWall.setCollideBits(self.allTiresMask)
self.eastWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.eastWall, self.fenceSurfaceType)
self.space.setCollideId(self.eastWall, self.wallCollideId)
self.southWall = OdePlaneGeom(
self.space, Vec4(0.0, 1.0, 0.0, IceGameGlobals.MinWall[1]))
self.southWall.setCollideBits(self.allTiresMask)
self.southWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.southWall, self.fenceSurfaceType)
self.space.setCollideId(self.southWall, self.wallCollideId)
self.northWall = OdePlaneGeom(
self.space, Vec4(0.0, -1.0, 0.0, -IceGameGlobals.MaxWall[1]))
self.northWall.setCollideBits(self.allTiresMask)
self.northWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.northWall, self.fenceSurfaceType)
self.space.setCollideId(self.northWall, self.wallCollideId)
self.floorTemp = OdePlaneGeom(self.space, Vec4(0.0, 0.0, 1.0, 0.0))
self.floorTemp.setCollideBits(self.allTiresMask)
self.floorTemp.setCategoryBits(self.floorMask)
self.space.setSurfaceType(self.floorTemp, self.iceSurfaceType)
self.space.setCollideId(self.floorTemp, self.floorCollideId)
self.space.setAutoCollideWorld(self.world)
self.space.setAutoCollideJointGroup(self.contactgroup)
self.totalPhysicsSteps = 0
def rayhit_all(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
ray = OdeRayGeom(length=1)
length, dir = rm.unit_vector(pto-pfrom, toggle_length=True)
ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2])
ray.setLength(length)
hit_entry = OdeUtil.collide(ray, tgt_cdmesh)
hit_points = [da.pdv3_to_npv3(point) for point in hit_entry.getContactPoints()]
hit_normals = [da.pdv3_to_npv3(hit_entry.getContactGeom(i).getNormal()) for i in range(hit_entry.getNumContacts())]
return hit_points, hit_normals
def is_collided(objcm0, objcm1):
"""
check if two objcm are collided after converting the specified cdmesh_type
:param objcm0: an instance of CollisionModel or CollisionModelCollection
:param objcm1: an instance of CollisionModel or CollisionModelCollection
:return:
author: weiwei
date: 20210118
"""
obj0 = gen_cdmesh_vvnf(*objcm0.extract_rotated_vvnf())
obj1 = gen_cdmesh_vvnf(*objcm1.extract_rotated_vvnf())
contact_entry = OdeUtil.collide(obj0, obj1, max_contacts=10)
contact_points = [
da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()
]
return (True,
contact_points) if len(contact_points) > 0 else (False,
contact_points)
def setupSimulation(self):
DistributedMinigamePhysicsWorld.DistributedMinigamePhysicsWorld.setupSimulation(self)
self.world.setGravity(0, 0, -32.174)
self.world.setAutoDisableFlag(1)
self.world.setAutoDisableLinearThreshold(0.5 * MetersToFeet)
self.world.setAutoDisableAngularThreshold(OdeUtil.getInfinity())
self.world.setAutoDisableSteps(10)
self.world.setCfm(1e-05 * MetersToFeet)
self.world.initSurfaceTable(3)
self.world.setSurfaceEntry(0, 1, 0.2, 0, 0, 0, 0, 0, 0.1)
self.world.setSurfaceEntry(0, 0, 0.1, 0.9, 0.1, 0, 0, 0, 0)
self.world.setSurfaceEntry(0, 2, 0.9, 0.9, 0.1, 0, 0, 0, 0)
self.floor = OdePlaneGeom(self.space, Vec4(0.0, 0.0, 1.0, -20.0))
self.floor.setCollideBits(self.allTiresMask)
self.floor.setCategoryBits(self.floorMask)
self.westWall = OdePlaneGeom(self.space, Vec4(1.0, 0.0, 0.0, IceGameGlobals.MinWall[0]))
self.westWall.setCollideBits(self.allTiresMask)
self.westWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.westWall, self.fenceSurfaceType)
self.space.setCollideId(self.westWall, self.wallCollideId)
self.eastWall = OdePlaneGeom(self.space, Vec4(-1.0, 0.0, 0.0, -IceGameGlobals.MaxWall[0]))
self.eastWall.setCollideBits(self.allTiresMask)
self.eastWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.eastWall, self.fenceSurfaceType)
self.space.setCollideId(self.eastWall, self.wallCollideId)
self.southWall = OdePlaneGeom(self.space, Vec4(0.0, 1.0, 0.0, IceGameGlobals.MinWall[1]))
self.southWall.setCollideBits(self.allTiresMask)
self.southWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.southWall, self.fenceSurfaceType)
self.space.setCollideId(self.southWall, self.wallCollideId)
self.northWall = OdePlaneGeom(self.space, Vec4(0.0, -1.0, 0.0, -IceGameGlobals.MaxWall[1]))
self.northWall.setCollideBits(self.allTiresMask)
self.northWall.setCategoryBits(self.wallMask)
self.space.setSurfaceType(self.northWall, self.fenceSurfaceType)
self.space.setCollideId(self.northWall, self.wallCollideId)
self.floorTemp = OdePlaneGeom(self.space, Vec4(0.0, 0.0, 1.0, 0.0))
self.floorTemp.setCollideBits(self.allTiresMask)
self.floorTemp.setCategoryBits(self.floorMask)
self.space.setSurfaceType(self.floorTemp, self.iceSurfaceType)
self.space.setCollideId(self.floorTemp, self.floorCollideId)
self.space.setAutoCollideWorld(self.world)
self.space.setAutoCollideJointGroup(self.contactgroup)
self.totalPhysicsSteps = 0