1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
|
# PyEPL: hardware/vr/avatar.pyx
#
# Copyright (C) 2003-2005 Michael J. Kahana
# Authors: Ian Schleifer, Per Sederberg, Aaron Geller, Josh Jacobs
# URL: http://memory.psych.upenn.edu/programming/pyepl
#
# Distributed under the terms of the GNU Lesser General Public License
# (LGPL). See the license.txt that came with this file.
"""
This modules contains the LowVAvatar class.
"""
import pyepl.hardware.vr
import eyes
import itertools
import math
import ode
import numpy
cdef object unitz
unitz = numpy.array((0.0, 0.0, 1.0))
cdef extern from "ode/ode.h":
ctypedef double dReal
ctypedef dReal dMatrix3[4*3]
void dRFromEulerAngles(dMatrix3 R, dReal phi, dReal theta, dReal psi)
def RFromEulerAngles(phi, theta, psi):
"""
Calculate the rotation matrix for the given Euler angles.
"""
cdef dMatrix3 R
dRFromEulerAngles(R, phi, theta, psi)
return [R[0], R[1], R[2], R[4], R[5], R[6], R[8], R[9], R[10]]
class LowVAvatar:
"""
A LowVAvatar represents a user's position, orientation, and body
shape in the virtual environment.
"""
def __init__(self, env, posorient = None):
"""
Create a LowVAvatar.
"""
self.env = env
self.eyevector = (0, 0, 0)
self.eyes = []
if posorient is not None:
self.posorient = posorient
else:
self.posorient = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def travel(self, **controls):
"""
"""
self.posorient = self.getNewPosOrient(**controls)
if controls.has_key("view_pitch"):
vp = controls["view_pitch"]
else:
vp = 0.0
self.posorient = (self.posorient[0] + self.eyevector[0], self.posorient[1] + self.eyevector[1], self.posorient[2] + self.eyevector[2],
self.posorient[3], self.posorient[4] + vp, self.posorient[5])
for eye in self.eyes:
eye.reposition(*self.posorient)
def getNewPosOrient(self): # To be overridden
"""
"""
pass
def positionOrientation(self):
"""
Return a 6-tuple of position and orientation data: x, y, z,
yaw, pitch, roll.
"""
return self.posorient
def teleport(self, pos = None, angle = None, tilt = None, roll = None):#FIX!!
"""
Set the absolute position, angle, tilt, and roll.
"""
pass #...
def attachEye(self, eye):
"""
Lock motion of eye with the position of this avatar.
"""
self.eyes.append(eye)
def dettachEye(self, eye):
"""
Unlock motion of eye.
"""
self.eyes.remove(eye)
class LowVAvatarSpeedBubble(LowVAvatar):
"""
A LowVAvatar represents a user's position, orientation, and body
shape in the virtual environment.
"""
def __init__(self, env, posorient = None, radius = 0.5, eyeheight = 3.8, density = 1.0):
"""
Create a LowVAvatar.
"""
LowVAvatar.__init__(self, env, posorient)
self.eyevector = (0, eyeheight - radius, 0)
self.body = ode.Body(env.world)
M = ode.Mass()
M.setSphere(density, radius)
self.body.setMass(M)
self.geom = ode.GeomSphere(env.space, radius)
self.geom.setBody(self.body)
self.body.setPosition((self.posorient[0], self.posorient[1] + radius, self.posorient[2]))
self.body.setRotation(RFromEulerAngles(self.posorient[4] * (numpy.pi / 180.0),
self.posorient[3] * (numpy.pi / 180.0),
self.posorient[5] * (numpy.pi / 180.0)
) # order...? Only sure about yaw...
)
self.geom.mu = 0
def getNewPosOrient(self, forward_speed = 0.0, yaw_speed = 0.0):
"""
"""
global unitz
cdef float x
cdef float y
cdef float z
cdef float yaw
cdef float pitch
cdef float roll
rot = numpy.reshape(numpy.array(self.body.getRotation()), (3, 3))
unitdir = -numpy.dot(rot, unitz)
self.body.setLinearVel(unitdir * forward_speed)
self.body.setAngularVel((0.0, -yaw_speed, 0.0))
self.env.dynamicStep(30)
rot = numpy.reshape(numpy.array(self.body.getRotation()), (3, 3))
unitdir = numpy.dot(rot, unitz)
# NB. for now all this only works for yaw
xunit, yunit, zunit = unitdir
# the following gives yaw values ranging from -90 to 270 deg.,
# with 90 directly ahead, and -90 directly behind
yaw = numpy.arctan(xunit/zunit) * (180.0 / numpy.pi)
if zunit < 0.0:
yaw = yaw + 180.0
roll=0
pitch=0
x, y, z = self.body.getPosition()
return (x, y, z, yaw, pitch, roll)
|
