Esempio n. 1
0
 def __init__(self, world, space, pos, color, capsules, radius, mass=2, fixed=False, orientation=v(1, 0, 0, 0)):
     "capsules is a list of (start, end) points"
     
     self.capsules = capsules
     
     self.body = ode.Body(world)
     self.body.setPosition(pos)
     self.body.setQuaternion(orientation)
     m = ode.Mass()
     # computing MOI assuming sphere with .5 m radius
     m.setSphere(mass/(4/3*math.pi*.5**3), .5) # setSphereTotal is broken
     self.body.setMass(m)
     
     self.geoms = []
     self.geoms2 = []
     for start, end in capsules:
         self.geoms.append(ode.GeomTransform(space))
         x = ode.GeomCapsule(None, radius, (end-start).mag())
         self.geoms2.append(x)
         self.geoms[-1].setGeom(x)
         self.geoms[-1].setBody(self.body)
         x.setPosition((start+end)/2 + v(random.gauss(0, .01), random.gauss(0, .01), random.gauss(0, .01)))
         a = (end - start).unit()
         b = v(0, 0, 1)
         x.setQuaternion(sim_math_helpers.axisangle_to_quat((a%b).unit(), -math.acos(a*b)))
     
     self.color = color
     self.radius = radius
     
     if fixed:
         self.joint = ode.FixedJoint(world)
         self.joint.attach(self.body, None)
         self.joint.setFixed()
Esempio n. 2
0
    def __init__(self,
                 world,
                 space,
                 pos,
                 color,
                 capsules,
                 radius,
                 mass=2,
                 fixed=False,
                 orientation=v(1, 0, 0, 0)):
        "capsules is a list of (start, end) points"

        self.capsules = capsules

        self.body = ode.Body(world)
        self.body.setPosition(pos)
        self.body.setQuaternion(orientation)
        m = ode.Mass()
        # computing MOI assuming sphere with .5 m radius
        m.setSphere(mass / (4 / 3 * math.pi * .5**3),
                    .5)  # setSphereTotal is broken
        self.body.setMass(m)

        self.geoms = []
        self.geoms2 = []
        for start, end in capsules:
            self.geoms.append(ode.GeomTransform(space))
            x = ode.GeomCapsule(None, radius, (end - start).mag())
            self.geoms2.append(x)
            self.geoms[-1].setGeom(x)
            self.geoms[-1].setBody(self.body)
            x.setPosition((start + end) / 2 + v(random.gauss(
                0, .01), random.gauss(0, .01), random.gauss(0, .01)))
            a = (end - start).unit()
            b = v(0, 0, 1)
            x.setQuaternion(
                sim_math_helpers.axisangle_to_quat((a % b).unit(),
                                                   -math.acos(a * b)))

        self.color = color
        self.radius = radius

        if fixed:
            self.joint = ode.FixedJoint(world)
            self.joint.attach(self.body, None)
            self.joint.setFixed()
Esempio n. 3
0
 def draw(self):
     glBegin(GL_LINES)
     for x in xrange(-10, 10+1):
         for y in xrange(-10, 10+1):
             pos = v(x, y, 0)
             vel = self.func(pos)
             glColor3d(1, 0, 0)
             glVertex3f(*pos-vel/2)
             glColor3d(1, 1, 0)
             glVertex3f(*pos+vel/2)
     glEnd()
Esempio n. 4
0
 def draw(self):
     glBegin(GL_LINES)
     for x in xrange(-10, 10 + 1):
         for y in xrange(-10, 10 + 1):
             pos = v(x, y, 0)
             vel = self.func(pos)
             glColor3d(1, 0, 0)
             glVertex3f(*pos - vel / 2)
             glColor3d(1, 1, 0)
             glVertex3f(*pos + vel / 2)
     glEnd()
Esempio n. 5
0
 def init(self, world, pos=v(0, -10, 2)):
     self.world = world
     self.pos = pos
     
     self.display_flags = pygame.DOUBLEBUF|pygame.OPENGL|pygame.RESIZABLE
     self.display = pygame.display.set_mode((640, 480), self.display_flags)
     self.clock = pygame.time.Clock()
     
     self.pitch = self.yaw = 0
     self.grabbed = False
     
     self.fovy = 100
Esempio n. 6
0
    def init(self, world, pos=v(0, -10, 2)):
        self.world = world
        self.pos = pos

        self.display_flags = pygame.DOUBLEBUF | pygame.OPENGL | pygame.RESIZABLE
        self.display = pygame.display.set_mode((640, 480), self.display_flags)
        self.clock = pygame.time.Clock()

        self.pitch = self.yaw = 0
        self.grabbed = False

        self.fovy = 100
Esempio n. 7
0
 def draw(self):
     q = gluNewQuadric()
     glColor3f(*self.color)
     with GLMatrix:
         rotate_to_body(self.body)
         for start, end in self.capsules:
             with GLMatrix:
                 glTranslate(*start)
                 gluSphere(q, self.radius, 30, 15)
             with GLMatrix:
                 glTranslate(*end)
                 gluSphere(q, self.radius, 30, 15)
             with GLMatrix:
                 glTranslate(*start)
                 a = (end - start).unit()
                 b = v(0, 0, 1)
                 glRotate(-math.degrees(math.acos(a*b)), *(a%b).unit())
                 gluCylinder(q, self.radius, self.radius, (end - start).mag(), 10, 1)
Esempio n. 8
0
 def draw(self):
     q = gluNewQuadric()
     glColor3f(*self.color)
     with GLMatrix:
         rotate_to_body(self.body)
         for start, end in self.capsules:
             with GLMatrix:
                 glTranslate(*start)
                 gluSphere(q, self.radius, 30, 15)
             with GLMatrix:
                 glTranslate(*end)
                 gluSphere(q, self.radius, 30, 15)
             with GLMatrix:
                 glTranslate(*start)
                 a = (end - start).unit()
                 b = v(0, 0, 1)
                 glRotate(-math.degrees(math.acos(a * b)), *(a % b).unit())
                 gluCylinder(q, self.radius, self.radius,
                             (end - start).mag(), 10, 1)
Esempio n. 9
0
 def step(self):
     dt = self.clock.tick()/1000
     
     for event in pygame.event.get():
         if event.type == pygame.MOUSEMOTION:
             if self.grabbed:
                 self.yaw += event.rel[0]/100
                 
                 self.pitch += -event.rel[1]/100
                 # caps it to a quarter turn up or down
                 self.pitch = min(max(self.pitch, -math.pi/2), math.pi/2)
         
         elif event.type == pygame.KEYDOWN:
             if event.key == pygame.K_TAB:
                 self.grabbed = not self.grabbed
                 pygame.event.set_grab(self.grabbed)
                 pygame.mouse.set_visible(not self.grabbed)
             
             elif event.key == pygame.K_q:
                 sys.exit()
         
         elif event.type == pygame.QUIT:
             sys.exit()
         
         elif event.type == pygame.VIDEORESIZE:
             self.display = pygame.display.set_mode(event.size, self.display_flags)
             glViewport(0, 0, self.display.get_width(), self.display.get_height())
     
     rot_matrix = euler_matrix(self.yaw, self.pitch, 0)
     
     forward  = rotate_vec(v(1,0,0), rot_matrix)
     left     = rotate_vec(v(0,1,0), rot_matrix)
     local_up = rotate_vec(v(0,0,1), rot_matrix)
     
     keys = pygame.key.get_pressed()
     
     speed = 50 if keys[pygame.K_LSHIFT] else 5
     speed *= (.1 if keys[pygame.K_LCTRL] else 1)
     
     if keys[pygame.K_w]: self.pos += forward*dt*speed
     if keys[pygame.K_s]: self.pos += -forward*dt*speed
     if keys[pygame.K_a]: self.pos += left*dt*speed
     if keys[pygame.K_d]: self.pos += -left*dt*speed
     if keys[pygame.K_SPACE]: self.pos += v(0, 0, 1)*dt*speed
     if keys[pygame.K_c]: self.pos += v(0, 0, -1)*dt*speed
     
     glMatrixMode(GL_PROJECTION)
     glLoadIdentity()
     perspective(self.fovy, self.display.get_width()/self.display.get_height(), 0.1)
     
     glMatrixMode(GL_MODELVIEW)
     glLoadIdentity()
     # rotates into the FLU coordinate system
     glMultMatrixf([
         [ 0., 0.,-1., 0.],
         [-1., 0., 0., 0.],
         [ 0., 1., 0., 0.],
         [ 0., 0., 0., 1.]
     ])
     # after that, +x is forward, +y is left, and +z is up
     
     glMultMatrixf(rot_matrix.T)
     
     glTranslate(*-self.pos)
     pos = self.pos
     
     self.world.draw()
     
     glDisable(GL_LIGHTING)
     glDisable(GL_DEPTH_TEST)
     with GLMatrix:
         glTranslate(*self.pos + forward + left - local_up)
         glBegin(GL_LINES)
         for i in xrange(3):
             glColor3f(*(j==i for j in xrange(3)))
             glVertex3f(0, 0, 0)
             glVertex3f(*(.1*(j==i) for j in xrange(3)))
         glEnd()
     glEnable(GL_DEPTH_TEST)
     glEnable(GL_LIGHTING)
     
     pygame.display.flip()
Esempio n. 10
0
    def step(self):
        dt = self.clock.tick() / 1000

        for event in pygame.event.get():
            if event.type == pygame.MOUSEMOTION:
                if self.grabbed:
                    self.yaw += event.rel[0] / 100

                    self.pitch += -event.rel[1] / 100
                    # caps it to a quarter turn up or down
                    self.pitch = min(max(self.pitch, -math.pi / 2),
                                     math.pi / 2)

            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_TAB:
                    self.grabbed = not self.grabbed
                    pygame.event.set_grab(self.grabbed)
                    pygame.mouse.set_visible(not self.grabbed)

                elif event.key == pygame.K_q:
                    sys.exit()

            elif event.type == pygame.QUIT:
                sys.exit()

            elif event.type == pygame.VIDEORESIZE:
                self.display = pygame.display.set_mode(event.size,
                                                       self.display_flags)
                glViewport(0, 0, self.display.get_width(),
                           self.display.get_height())

        rot_matrix = euler_matrix(self.yaw, self.pitch, 0)

        forward = rotate_vec(v(1, 0, 0), rot_matrix)
        left = rotate_vec(v(0, 1, 0), rot_matrix)
        local_up = rotate_vec(v(0, 0, 1), rot_matrix)

        keys = pygame.key.get_pressed()

        speed = 50 if keys[pygame.K_LSHIFT] else 5
        speed *= (.1 if keys[pygame.K_LCTRL] else 1)

        if keys[pygame.K_w]: self.pos += forward * dt * speed
        if keys[pygame.K_s]: self.pos += -forward * dt * speed
        if keys[pygame.K_a]: self.pos += left * dt * speed
        if keys[pygame.K_d]: self.pos += -left * dt * speed
        if keys[pygame.K_SPACE]: self.pos += v(0, 0, 1) * dt * speed
        if keys[pygame.K_c]: self.pos += v(0, 0, -1) * dt * speed

        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        perspective(self.fovy,
                    self.display.get_width() / self.display.get_height(), 0.1)

        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()
        # rotates into the FLU coordinate system
        glMultMatrixf([[0., 0., -1., 0.], [-1., 0., 0., 0.], [0., 1., 0., 0.],
                       [0., 0., 0., 1.]])
        # after that, +x is forward, +y is left, and +z is up

        glMultMatrixf(rot_matrix.T)

        glTranslate(*-self.pos)
        pos = self.pos

        self.world.draw()

        glDisable(GL_LIGHTING)
        glDisable(GL_DEPTH_TEST)
        with GLMatrix:
            glTranslate(*self.pos + forward + left - local_up)
            glBegin(GL_LINES)
            for i in xrange(3):
                glColor3f(*(j == i for j in xrange(3)))
                glVertex3f(0, 0, 0)
                glVertex3f(*(.1 * (j == i) for j in xrange(3)))
            glEnd()
        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)

        pygame.display.flip()