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()
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()
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()
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()
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
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
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)
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)
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()
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()