Beispiel #1
0
  def __init__( self ):
    ParticleEffect.ParticleEffect.__init__( self )
    self.reset()
    self.setPos(0.000, 0.000, 0.000)
    self.setHpr(0.000, 0.000, 0.000)
    self.setScale(1.000, 1.000, 1.000)
    self.particle = Particles.Particles('particles-1')
    # Particles parameters
    self.particle.setFactory("PointParticleFactory")
    self.particle.setRenderer("LineParticleRenderer")
#    self.particle.setRenderer("PointParticleRenderer")
    self.particle.setEmitter("DiscEmitter")
    self.particle.setPoolSize( 4096 )
    self.particle.setBirthRate( 0.01 )
    self.particle.setLitterSize(10)
    self.particle.setLitterSpread(0)
    self.particle.setSystemLifespan(0.0000)
    self.particle.setLocalVelocityFlag(1)
    self.particle.setSystemGrowsOlderFlag(0)
    # Factory parameters
    self.particle.factory.setLifespanBase(1.5000)
    self.particle.factory.setLifespanSpread(0.0000)
    self.particle.factory.setMassBase(1.0000)
    self.particle.factory.setMassSpread(0.0000)
    self.particle.factory.setTerminalVelocityBase(400.0000)
    self.particle.factory.setTerminalVelocitySpread(0.0000)
    # Point factory parameters
    # Renderer parameters
    self.particle.renderer.setAlphaMode(BaseParticleRenderer.PRALPHANONE)
    self.particle.renderer.setUserAlpha(1.00)
#    # Point parameters
#    self.particle.renderer.setPointSize(1.00)
#    self.particle.renderer.setStartColor(Vec4(1.00, 1.00, 1.00, 1.00))
#    self.particle.renderer.setEndColor(Vec4(1.00, 1.00, 1.00, 1.00))
#    self.particle.renderer.setBlendType(PointParticleRenderer.PPONECOLOR)
#    self.particle.renderer.setBlendMethod(BaseParticleRenderer.PPNOBLEND)
    # Line parameters
#    self.particle.renderer.setHeadColor(Vec4(1.00, 1.00, 1.00, 1.00))
#    self.particle.renderer.setTailColor(Vec4(1.00, 1.00, 1.00, 1.00))
    self.particle.renderer.setHeadColor(Vec4(1.00, 1.00, 1.00, 1.0))
    self.particle.renderer.setTailColor(Vec4(1.00, 1.00, 1.00, 1.0))
    self.particle.renderer.setLineScaleFactor(1.00)
    # Emitter parameters
    self.particle.emitter.setEmissionType(BaseParticleEmitter.ETEXPLICIT)
    self.particle.emitter.setAmplitude(1.0000)
    self.particle.emitter.setAmplitudeSpread(0.0000)
    self.particle.emitter.setOffsetForce(Vec3(0.0000, 0.0000, 0.0000))
    self.particle.emitter.setExplicitLaunchVector(Vec3(0.0000, 0.0000, 0.0000))
    self.particle.emitter.setRadiateOrigin(Point3(0.0000, 0.0000, 0.0000))
    # Disc parameters
    self.particle.emitter.setRadius(1.0000)
    self.addParticles(self.particle)
    self.forceGroup = ForceGroup.ForceGroup('gravity')
    # Force parameters
    force0 = LinearVectorForce(Vec3(0.0000, 0.0000, -1.0000), 1.0000, 0)
    force0.setVectorMasks(1, 1, 1)
    force0.setActive(1)
    self.forceGroup.addForce(force0)
    self.addForceGroup(self.forceGroup)
    def __setupGravity(self):
        base.particlesEnabled = True
        base.enableParticles()

        gravityFN = ForceNode('world-forces')
        gravityFNP = render.attachNewNode(gravityFN)
        gravityForce = LinearVectorForce(0, 0,
                                         -6)  #gravity acceleration ft/s^2
        gravityFN.addForce(gravityForce)

        base.physicsMgr.addLinearForce(gravityForce)
Beispiel #3
0
 def perturb(self, force, time=1000, backToColor=None):
       print "perturbing %s, %s"%(force, time)
       node1 = self._node1
       node2 = self._node2
       actor1 = self._actor1
       actor2 = self._actor2
       self._timeOut = globalClock.getLongTime() + time/1000
       force2 = node2.getRelativeVector( node1, force)
       force2 = Vec3( -1*force2.x, -1*force2.y, -1*force2.z)
       
       forceN1 = ForceNode('Impulse1')
       lvf1 = LinearVectorForce( force )
       lvf1.setMassDependent(1)
       forceN1.addForce(lvf1)
       node1.attachNewNode(forceN1)
       actor1.getPhysical(0).addLinearForce(lvf1)
       forceN2 = ForceNode("Impulse2")
       lvf2 = LinearVectorForce( force2)
       lvf2.setMassDependent(1)
       forceN2.addForce(lvf2)
       node2.attachNewNode(forceN2)
       actor2.getPhysical(0).addLinearForce(lvf2)
       self._impulse1 = lvf1
       self._impulse2 = lvf2
       if backToColor:
             self._backToColor = backToColor
Beispiel #4
0
    def __init__(self, parent=None):
        '''
        Create a new Vehicle node. Physics should be initialized before any
        instances of Vehicle are created.

        arguments:
          parent -- A PandaNode for the vehicle to attach to. Default is None,
                    in which case the Vehicle should be added to the scene
                    graph via NodePath.attachNewNode().

        '''
        ActorNode.__init__(self, 'VehiclePhysics')
        base.physicsMgr.attachPhysicalNode(self)

        self.getPhysicsObject().setMass(MASS)
        if parent:
            self.myPath = parent.attachNewNode(self)
        else:
            self.myPath = NodePath(self)

        # Load vehicle model and place in the transparent bin.
        vehicleModel = loader.loadModel(MODEL_PATH)
        hull = vehicleModel.find('**/Hull')
        hull.setBin('transparent', 30)
        pwnsEnclosure = vehicleModel.find('**/Pwns_Enclosure')
        pwnsEnclosure.setBin('transparent', 30)
        self.myPath.setPos(0, 0, -0.0)
        selectable = self.myPath.attachNewNode(SelectableNode('vehicle sel'))
        vehicleModel.reparentTo(selectable)

        # ==== Initialize Physics ==== #
        thrusterForceNode = ForceNode('ThrusterForce')
        self.myPath.attachNewNode(thrusterForceNode)

        self.linearForce = LinearVectorForce(0, 0, 0)
        self.linearForce.setMassDependent(1)
        self.angularForce = AngularVectorForce(0, 0, 0)

        thrusterForceNode.addForce(self.linearForce)
        thrusterForceNode.addForce(self.angularForce)

        self.getPhysical(0).addLinearForce(self.linearForce)
        self.getPhysical(0).addAngularForce(self.angularForce)

        self.previousXY = (self.myPath.getX(), self.myPath.getY())

        self.tm = ThrusterManager()

        # Add self.updatePhysics to the task manager and run this task as
        # frequently as possible.
        self.updatePhysicsTask = taskMgr.add(self.updatePhysics,
                                             'UpdatePhysics')

        # ==== Initialize Cameras ==== #
        lens = PerspectiveLens()
        lens.setNearFar(0.05, 100.0)
        #Use either FocalLength or Fov. Fov ~40 is about what actual forward cameras are
        #lens.setFocalLength(0.8)
        lens.setFov(70, 70)
        camera = Camera("Forward_left", lens).getPath()
        camera.reparentTo(vehicleModel.find('**/Forward_Camera'))
        camera.setX(camera.getX() - 0.1)  # Forward cameras 20cm apart
        camera.setY(
            camera.getY() +
            0.05)  # Move in front of torpedo tubes to avoid abstruction
        camera.setHpr(0, 0, 0)

        camera = Camera("Forward_right", lens).getPath()
        camera.reparentTo(vehicleModel.find('**/Forward_Camera'))
        camera.setX(camera.getX() + 0.1)  # Forward cameras 20cm apart
        camera.setY(
            camera.getY() +
            0.05)  # Move in front of torpedo tubes to avoid abstruction
        camera.setHpr(0, 0, 0)

        lens = PerspectiveLens()
        lens.setNearFar(0.05, 100.0)
        lens.setFocalLength(0.8)
        camera = Camera("Downward", lens).getPath()
        camera.reparentTo(vehicleModel.find('**/Downward_Camera'))
        camera.setHpr(0, -90, 0)

        #Layout link (to access hydrophone information)
        self.layout = None

        #Hydrophone variables
        self.start_time = time()
        self.last_hydro_update = time()
Beispiel #5
0
class Vehicle(ActorNode):
    def __init__(self, parent=None):
        '''
        Create a new Vehicle node. Physics should be initialized before any
        instances of Vehicle are created.

        arguments:
          parent -- A PandaNode for the vehicle to attach to. Default is None,
                    in which case the Vehicle should be added to the scene
                    graph via NodePath.attachNewNode().

        '''
        ActorNode.__init__(self, 'VehiclePhysics')
        base.physicsMgr.attachPhysicalNode(self)

        self.getPhysicsObject().setMass(MASS)
        if parent:
            self.myPath = parent.attachNewNode(self)
        else:
            self.myPath = NodePath(self)

        # Load vehicle model and place in the transparent bin.
        vehicleModel = loader.loadModel(MODEL_PATH)
        hull = vehicleModel.find('**/Hull')
        hull.setBin('transparent', 30)
        pwnsEnclosure = vehicleModel.find('**/Pwns_Enclosure')
        pwnsEnclosure.setBin('transparent', 30)
        self.myPath.setPos(0, 0, -0.0)
        selectable = self.myPath.attachNewNode(SelectableNode('vehicle sel'))
        vehicleModel.reparentTo(selectable)

        # ==== Initialize Physics ==== #
        thrusterForceNode = ForceNode('ThrusterForce')
        self.myPath.attachNewNode(thrusterForceNode)

        self.linearForce = LinearVectorForce(0, 0, 0)
        self.linearForce.setMassDependent(1)
        self.angularForce = AngularVectorForce(0, 0, 0)

        thrusterForceNode.addForce(self.linearForce)
        thrusterForceNode.addForce(self.angularForce)

        self.getPhysical(0).addLinearForce(self.linearForce)
        self.getPhysical(0).addAngularForce(self.angularForce)

        self.previousXY = (self.myPath.getX(), self.myPath.getY())

        self.tm = ThrusterManager()

        # Add self.updatePhysics to the task manager and run this task as
        # frequently as possible.
        self.updatePhysicsTask = taskMgr.add(self.updatePhysics,
                                             'UpdatePhysics')

        # ==== Initialize Cameras ==== #
        lens = PerspectiveLens()
        lens.setNearFar(0.05, 100.0)
        #Use either FocalLength or Fov. Fov ~40 is about what actual forward cameras are
        #lens.setFocalLength(0.8)
        lens.setFov(70, 70)
        camera = Camera("Forward_left", lens).getPath()
        camera.reparentTo(vehicleModel.find('**/Forward_Camera'))
        camera.setX(camera.getX() - 0.1)  # Forward cameras 20cm apart
        camera.setY(
            camera.getY() +
            0.05)  # Move in front of torpedo tubes to avoid abstruction
        camera.setHpr(0, 0, 0)

        camera = Camera("Forward_right", lens).getPath()
        camera.reparentTo(vehicleModel.find('**/Forward_Camera'))
        camera.setX(camera.getX() + 0.1)  # Forward cameras 20cm apart
        camera.setY(
            camera.getY() +
            0.05)  # Move in front of torpedo tubes to avoid abstruction
        camera.setHpr(0, 0, 0)

        lens = PerspectiveLens()
        lens.setNearFar(0.05, 100.0)
        lens.setFocalLength(0.8)
        camera = Camera("Downward", lens).getPath()
        camera.reparentTo(vehicleModel.find('**/Downward_Camera'))
        camera.setHpr(0, -90, 0)

        #Layout link (to access hydrophone information)
        self.layout = None

        #Hydrophone variables
        self.start_time = time()
        self.last_hydro_update = time()

    def setLayout(self, layout):
        #Add a link to the layout to allow for referencing other objects
        #This is necessary for the hydrophone addition
        self.layout = layout

    def getDepth(self):
        ''' Returns the depth of the vehicle, in meters. '''
        return -0.15 - self.myPath.getZ()

    def getHeading(self):
        ''' Returns the heading of the vehicle, in clockwise degrees. '''
        # Panda uses counter-clockwise degrees, with the range (-180, 180].
        heading = self.myPath.getH()
        if heading < 0:
            return -heading
        elif heading > 0:
            return 360 - heading
        else:
            return 0

    def updatePhysics(self, task):
        '''
        Use the motor PWM values calculated by the controller to apply forces
        to the simulated vehicle.

        This runs at every frame, so it needs to complete quickly.

        '''
        outputs = shm.kalman.get()
        self.tm.update(outputs)
        passive_wrench = vehicle.passive_forces(outputs, self.tm)
        passive_forces, passive_torques = passive_wrench[:3], \
                                          passive_wrench[3:]

        # Get motor thrusts
        thrusts = np.array(self.tm.get_thrusts())

        # Add passive forces and torques to that produced by thrusters,
        # converting them to sub space first.
        force = self.tm.total_thrust(thrusts) + \
                self.tm.orientation.conjugate() * passive_forces
        torque = self.tm.total_torque(thrusts) + \
                self.tm.orientation.conjugate() * passive_torques

        # Finally apply forces and torques to the model
        # we also need to account for panda3d's strange coordinate system
        # (x and y are flipped and z points up (instead of down))
        self.linearForce.setVector(force_subspace[1], \
                                   force_subspace[0], \
                                  -force_subspace[2])

        # We're supposed to use axis angle here, but I'm being sneaky
        # and using the torque vector directly, i.e. non normalized axis angle
        # with the hopes that this LRotationf constructor will figure it out
        self.angularForce.setQuat(\
            LRotationf(LVector3f(torque_subspace[1], \
                                 torque_subspace[0], \
                                -torque_subspace[2]), 1))

        # Update shared variables for controller
        outputs.heading = self.getHeading()
        outputs.pitch = self.myPath.getP()
        outputs.roll = self.myPath.getR()

        # This velocity is in world space
        # We need to put it into THRUST CONVENTION SPACE
        # which we assume kalman outputs in...
        velocity = self.getPhysicsObject().getVelocity()

        # Bring the velocity into THRUST CONVENTION SPACE
        # Don't forget to account for panda's coordinate system
        velocity = self.tm.heading_quat.conjugate() * \
                  np.array((velocity.getY(), velocity.getX(), -velocity.getZ()))

        outputs.velx = velocity[0]
        outputs.vely = velocity[1]
        outputs.depth_rate = velocity[2]

        outputs.depth = self.getDepth()
        outputs.north = self.myPath.getY()
        outputs.east = self.myPath.getX()

        dX = self.myPath.getX() - self.previousXY[0]
        dY = self.myPath.getY() - self.previousXY[1]

        # Forward and sway are in THRUST CONVENTION SPACE
        # don't forget to account for panda's coordinate system
        dF, dS, dD = self.tm.heading_quat.conjugate() * np.array((dY, dX, 0.0))

        outputs.forward += dF
        outputs.sway += dS

        # Output some quaternions, accounting for Panda's coordinate system
        outputs.q0, outputs.q2, outputs.q1, outputs.q3 = self.myPath.getQuat()
        outputs.q3 *= -1.0

        shm.kalman.set(outputs)

        svHeadingInt.set(self.getHeading())
        svDepth.set(self.getDepth())
        #XXX: Approximate altitude assuming that the pool is 12 feet deep
        svAltitude.set(3.6 - self.getDepth())
        svDvlDmgNorth.set(self.myPath.getY())
        svDvlDmgEast.set(self.myPath.getX())

        self.previousXY = (self.myPath.getX(), self.myPath.getY())  #update

        self.output_hydro_data()

        return Task.cont

    def output_hydro_data(self):
        #Update simulated hydrophone values
        pingers = []  #Get all pingers from the layout
        for element in self.layout.elements:
            if element.getTypeName() == "Pinger":
                pingers.append(element)

        HYDRO_TICK_PERIOD = 1

        if time() - self.last_hydro_update > HYDRO_TICK_PERIOD:
            dt = time() - self.last_hydro_update
            self.last_hydro_update = time()

            if shm.hydrophones_settings.dsp_mode.get() == 1:  #Search mode
                #Incr search count
                shm.hydrophones_results.search_count.set(
                    shm.hydrophones_results.search_count.get() + 1)

                #Generate proper "hydrophone bins" marks
                sb = 0
                for p in pingers:
                    f = p.pinger_frequency
                    dc = (f - (shm.hydrophones_settings.searchCenter.get() -
                               shm.hydrophones_settings.searchDelta.get())
                          ) / shm.hydrophones_settings.searchStep.get() + 0.5
                    sb |= 1 << int(dc)
                shm.hydrophones_results.search_bins.set(sb)

            else:  #Track Mode
                #Incr ping count
                shm.hydrophones_results.ping_count.set(
                    shm.hydrophones_results.ping_count.get() + 1)

                #Determine which pinger we are actively tracking (within 0.7khz of target)
                targetp = None
                for p in pingers:
                    if abs(shm.hydrophones_settings.trackFrequency.get() -
                           p.pinger_frequency) < 700:
                        targetp = p

                if targetp is not None:
                    shm.hydrophones_results.intensity.set(
                        int(shm.hydrophones_settings.trackMagThresh.get() +
                            1e4 * random()))
                    shm.hydrophones_results.ping_time.set(int(dt * 1000))

                    pp = targetp.path.getPos()

                    vv = vector.Vector(self.myPath.getY(), self.myPath.getX())
                    pv = vector.Vector(pp.getY(), pp.getX())

                    #heading
                    dv = pv - vv
                    ang = vector.GetAuvAngle(dv)
                    hdiff = helpers.heading_diff(self.getHeading(), ang)

                    shm.hydrophones_results.heading.set(hdiff)

                    #elevation
                    dh = self.myPath.getZ() - pp.getZ()
                    dist = vector.Length(dv)
                    elev = math.degrees(math.atan2(dist, dh))
                    elev = min(elev, 90)

                    shm.hydrophones_results.elevation.set(elev)

                    #phase calculations
                    dy = self.myPath.getY() - pp.getY()
                    dx = self.myPath.getX() - pp.getX()
                    yang = math.degrees(math.atan2(dist, dy))
                    xang = math.degrees(math.atan2(dist, dx))

                    shm.hydrophones_results.phaseX.set((90.0 - xang) / 90.0)
                    shm.hydrophones_results.phaseY.set((90.0 - yang) / 90.0)
                    shm.hydrophones_results.phaseZ.set((90.0 - elev) / 90.0)

                else:
                    shm.hydrophones_results.heading.set(0)
                    shm.hydrophones_results.elevation.set(0)
                    shm.hydrophones_results.intensity.set(0)
                    shm.hydrophones_results.ping_time.set(0)

            shm.hydrophones_results.uptime.set(int(time() - self.start_time))

    def __del__(self):
        ''' Remove update tasks from the panda task manager. '''
        taskMgr.remove(self.updatePhysicsTask)
        ActorNode.__del__(self)
Beispiel #6
0
    def _make_pfx (self, enode, rnode, pos, radius, scale1, scale2,
                   birthrate, lifespan, zspinini, zspinfin, zspinvel,
                   poolsize, littersize, amplitude, ampspread, risefact,
                   texpath, color, alphamode, softstop):

        pfx = ParticleEffect()
        pfx.setPos(pos)

        p0 = make_particles()
        p0.setPoolSize(poolsize)
        p0.setBirthRate(birthrate)
        p0.setLitterSize(littersize)
        p0.setLitterSpread(0)
        #p0.setSystemLifespan(1.0)
        #p0.setLocalVelocityFlag(1)
        #p0.setSystemGrowsOlderFlag(0)

        # p0.setFactory("PointParticleFactory")
        # p0.factory.setLifespanBase(lifespan)
        # p0.factory.setLifespanSpread(0.0)
        # #p0.factory.setMassBase(1.00)
        # #p0.factory.setMassSpread(0.00)
        # #p0.factory.setTerminalVelocityBase(400.0000)
        # #p0.factory.setTerminalVelocitySpread(0.0000)

        p0.setFactory("ZSpinParticleFactory")
        p0.factory.setLifespanBase(lifespan)
        p0.factory.setLifespanSpread(0.0)
        #p0.factory.setMassBase(1.00)
        #p0.factory.setMassSpread(0.00)
        #p0.factory.setTerminalVelocityBase(400.0000)
        #p0.factory.setTerminalVelocitySpread(0.0000)
        p0.factory.setAngularVelocity(zspinvel)
        p0.factory.setFinalAngle(zspinfin)
        p0.factory.setInitialAngle(zspinini)

        p0.setRenderer("SpriteParticleRenderer")
        p0.renderer.setAlphaMode(alphamode)
        texpaths = texpath
        if not isinstance(texpath, (tuple, list)):
            texpaths = [texpaths]
        for texpath in texpaths:
            texture = base.load_texture("data", texpath)
            p0.renderer.addTexture(texture)
        # p0.renderer.setUserAlpha(alpha)
        p0.renderer.setColor(color)
        p0.renderer.setXScaleFlag(1)
        p0.renderer.setYScaleFlag(1)
        p0.renderer.setAnimAngleFlag(1)
        p0.renderer.setInitialXScale(scale1)
        p0.renderer.setFinalXScale(scale2)
        p0.renderer.setInitialYScale(scale1)
        p0.renderer.setFinalYScale(scale2)

        p0.setEmitter("SphereVolumeEmitter")
        p0.emitter.setRadius(radius)
        p0.emitter.setEmissionType(BaseParticleEmitter.ETRADIATE)
        p0.emitter.setAmplitude(amplitude)
        p0.emitter.setAmplitudeSpread(ampspread)
        #p0.emitter.setOffsetForce(Vec3(0.0000, 0.0000, 0.0000))
        #p0.emitter.setExplicitLaunchVector(Vec3(1.0000, 0.0000, 0.0000))
        #p0.emitter.setRadiateOrigin(Point3(0.0000, 0.0000, 0.0000))

        f0 = ForceGroup("vertex")
        force0 = LinearVectorForce(risefact * amplitude)
        force0.setActive(1)
        f0.addForce(force0)

        p0.setRenderParent(rnode)

        pfx.addForceGroup(f0)
        pfx.addParticles(p0)

        pfx.start(enode)
        if softstop:
            pfx.softStop()

        return pfx