Exemplo n.º 1
0
    def _initAgents(self):

        # Load agents
        for agent in self.scene.scene.findAllMatches('**/agents/agent*'):

            transform = TransformState.makeIdentity()
            if self.agentMode == 'capsule':
                shape = BulletCapsuleShape(
                    self.agentRadius, self.agentHeight - 2 * self.agentRadius)
            elif self.agentMode == 'sphere':
                shape = BulletCapsuleShape(self.agentRadius,
                                           2 * self.agentRadius)

            # XXX: use BulletCharacterControllerNode class, which already handles local transform?
            node = BulletRigidBodyNode('physics')
            node.setMass(self.agentMass)
            node.setStatic(False)
            node.setFriction(self.defaultMaterialFriction)
            node.setRestitution(self.defaultMaterialRestitution)
            node.addShape(shape)
            self.bulletWorld.attach(node)

            # Constrain the agent to have fixed position on the Z-axis
            node.setLinearFactor(Vec3(1.0, 1.0, 0.0))

            # Constrain the agent not to be affected by rotations
            node.setAngularFactor(Vec3(0.0, 0.0, 0.0))

            node.setIntoCollideMask(BitMask32.allOn())
            node.setDeactivationEnabled(True)

            # Enable continuous collision detection (CCD)
            node.setCcdMotionThreshold(1e-7)
            node.setCcdSweptSphereRadius(0.50)

            if node.isStatic():
                agent.setTag('physics-mode', 'static')
            else:
                agent.setTag('physics-mode', 'dynamic')

            # Attach the physic-related node to the scene graph
            physicsNp = NodePath(node)
            physicsNp.setTransform(transform)

            # Reparent all child nodes below the new physic node
            for child in agent.getChildren():
                child.reparentTo(physicsNp)
            physicsNp.reparentTo(agent)

            # NOTE: we need this to update the transform state of the internal bullet node
            physicsNp.node().setTransformDirty()

            # Validation
            assert np.allclose(
                mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
                mat4ToNumpyArray(agent.getNetTransform().getMat()),
                atol=1e-6)
Exemplo n.º 2
0
    def _initLayoutModels(self):

        # Load layout objects as meshes
        for model in self.scene.scene.findAllMatches('**/layouts/object*/model*'):
            
            # NOTE: ignore models that have no geometry defined
            if model.getTightBounds() is None:
                logger.warning('Object %s has no geometry defined and will be ignored for physics!' % (str(model)))
                continue

            shape, transform = getCollisionShapeFromModel(
                model, mode='mesh', defaultCentered=False)

            node = BulletRigidBodyNode('physics')
            node.setMass(0.0)
            node.setFriction(self.defaultMaterialFriction)
            node.setRestitution(self.defaultMaterialRestitution)
            node.setStatic(True)
            node.addShape(shape)
            node.setDeactivationEnabled(True)
            node.setIntoCollideMask(BitMask32.allOn())
            self.bulletWorld.attach(node)

            # Attach the physic-related node to the scene graph
            physicsNp = model.getParent().attachNewNode(node)
            physicsNp.setTransform(transform)

            if node.isStatic():
                model.getParent().setTag('physics-mode', 'static')
            else:
                model.getParent().setTag('physics-mode', 'dynamic')

            # Reparent render and acoustics nodes (if any) below the new physic node
            # XXX: should be less error prone to just reparent all children
            # (except the hidden model)
            renderNp = model.getParent().find('**/render')
            if not renderNp.isEmpty():
                renderNp.reparentTo(physicsNp)
            acousticsNp = model.getParent().find('**/acoustics')
            if not acousticsNp.isEmpty():
                acousticsNp.reparentTo(physicsNp)
            semanticsNp = model.getParent().find('**/semantics')
            if not semanticsNp.isEmpty():
                semanticsNp.reparentTo(physicsNp)

            # NOTE: we need this to update the transform state of the internal
            # bullet node
            physicsNp.node().setTransformDirty()

            # Validation
            assert np.allclose(mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
                               mat4ToNumpyArray(model.getNetTransform().getMat()), atol=1e-6)
Exemplo n.º 3
0
    def _initObjects(self):

        # Load objects
        for model in self.scene.scene.findAllMatches(
                '**/objects/object*/model*'):
            modelId = model.getParent().getTag('model-id')

            # XXX: we could create BulletGhostNode instance for non-collidable objects, but we would need to filter out the collisions later on
            if not modelId in self.openedDoorModelIds:

                shape, transform = getCollisionShapeFromModel(
                    model, self.objectMode, defaultCentered=True)

                node = BulletRigidBodyNode('physics')
                node.addShape(shape)
                node.setFriction(self.defaultMaterialFriction)
                node.setRestitution(self.defaultMaterialRestitution)
                node.setIntoCollideMask(BitMask32.allOn())
                node.setDeactivationEnabled(True)

                if self.suncgDatasetRoot is not None:

                    # Check if it is a movable object
                    category = self.modelCatMapping.getCoarseGrainedCategoryForModelId(
                        modelId)
                    if category in self.movableObjectCategories:
                        # Estimate mass of object based on volumetric data and default material density
                        objVoxFilename = os.path.join(self.suncgDatasetRoot,
                                                      'object_vox',
                                                      'object_vox_data',
                                                      modelId,
                                                      modelId + '.binvox')
                        voxelData = ObjectVoxelData.fromFile(objVoxFilename)
                        mass = Panda3dBulletPhysics.defaultDensity * voxelData.getFilledVolume(
                        )
                        node.setMass(mass)
                    else:
                        node.setMass(0.0)
                        node.setStatic(True)
                else:
                    node.setMass(0.0)
                    node.setStatic(True)

                if node.isStatic():
                    model.getParent().setTag('physics-mode', 'static')
                else:
                    model.getParent().setTag('physics-mode', 'dynamic')

                # Attach the physic-related node to the scene graph
                physicsNp = model.getParent().attachNewNode(node)
                physicsNp.setTransform(transform)

                # Reparent render and acoustics nodes (if any) below the new physic node
                #XXX: should be less error prone to just reparent all children (except the hidden model)
                renderNp = model.getParent().find('**/render')
                if not renderNp.isEmpty():
                    renderNp.reparentTo(physicsNp)
                acousticsNp = model.getParent().find('**/acoustics')
                if not acousticsNp.isEmpty():
                    acousticsNp.reparentTo(physicsNp)

                # NOTE: we need this to update the transform state of the internal bullet node
                node.setTransformDirty()

                # NOTE: we need to add the node to the bullet engine only after setting all attributes
                self.bulletWorld.attach(node)

                # Validation
                assert np.allclose(
                    mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
                    mat4ToNumpyArray(
                        model.getParent().getNetTransform().getMat()),
                    atol=1e-6)

            else:
                logger.debug('Object %s ignored from physics' % (modelId))