def updateVizNode(self, pos_XYZ, quat_XYZW):
transformMatrix = viz.Transform()
transformMatrix.setQuat(quat_XYZW)
transformMatrix.postTrans(pos_XYZ)
transformMatrix.postAxisAngle(0, 1, 0, 90)
self.vizNode.setMatrix(transformMatrix)
def updateView(self):
data = self.tracker.getData()
#print data;
pos = data[:3]
rot = data[3:7]
m = viz.Transform()
m.setQuat(rot)
m.postTrans(pos)
m.postAxisAngle(0, 1, 0, 90)
m.preMult(self.preMat)
m.postMult(self.postMat)
if self.attachTo != None and not self.off:
self.attachTo.setMatrix(m)
#self.extra_draw.draw()
if self.particle != None:
pos = m.getTrans()
self.particle.moveTo(Vector([pos[0], self.offset[1], pos[2]]))
#print "pos: ", pos, "quat: ", rot
self.pos = pos
self.rot = rot
self.matrix = m
def animateTexture():
#Increment texture position
tex.pos += ANIMATE_SPEED * viz.elapsed()
#Create transform for texture
mat = viz.Transform()
mat.setTrans(tex.pos,0,0)
#Apply transform to texture
quad.texmat(mat)
def __init__(self, index, marker_ids, center_marker_ids, localOffest_xyz):
super(RigidTracker, self).__init__(index, marker_ids)
self.center_marker_ids = center_marker_ids
self._pose = Pose(pos=(0, 0, 0), quat=(1, 0, 0, 0), cond=-1)
self._transform = viz.Transform()
self._localOffset = localOffest_xyz
self.filepath = []
self.filename = []
def getOrient(self, dist):
d = dist - self.getStartWorldDist()
m = viz.Transform()
m.postAxisAngle(0, 1, 0, 90)
r = self.roll - self.roll * 3 * d / self.length
if r < 0:
r = 0
#print "r = ", r
m.postAxisAngle(1, 0, 0, r)
#m.set( 0,0,1,0, 0,1,0,0, -1,0,0,0, 0,0,0,1 )
#m.postAxisAngle( -1, 0, 0, -self.roll )
return m
def getOrient(self, distWorld):
if (self.distStartWorld <= distWorld) and (distWorld <
self.distEndWorld):
d = distWorld - self.getStartWorldDist()
theta = (1 - d / self.length) * 90
tF = [
math.sin(math.radians(theta)), -math.cos(math.radians(theta)),
0
]
tL = self.crossProductUnit(tF, self.up)
tU = self.crossProductUnit(tL, tF)
m = viz.Transform()
m.set(tL[0], tL[1], tL[2], 0, tU[0], tU[1], tU[2], 0, -tF[0],
-tF[1], -tF[2], 0, 0, 0, 0, 1)
return m
else:
print("ERROR in getOrient Piece2: distance out of range")
exit()
def getOrient(self, distWorld):
if (self.distStartWorld <= distWorld) and (distWorld <
self.distEndWorld):
d = distWorld - self.getStartWorldDist()
theta = 180 + d / self.length * 360
tF = [
math.cos(math.radians(theta)), self.zchange / self.length,
-math.sin(math.radians(theta))
]
tF = self.unitVector(tF)
tL = self.crossProductUnit(tF, self.up)
tU = self.crossProductUnit(tL, tF)
m = viz.Transform()
m.set(tL[0], tL[1], tL[2], 0, tU[0], tU[1], tU[2], 0, -tF[0],
-tF[1], -tF[2], 0, 0, 0, 0, 1)
m.postAxisAngle(tF[0], tF[1], tF[2], -self.roll)
return m
else:
print("ERROR in getOrient Piece2: distance out of range")
exit()
def getHeadOrientation(self):
# is server data up to date?
# is rotateRigid working correctly?
#
# Quaternion
rot = [
self.serverData.pose[4], self.serverData.pose[5],
-self.serverData.pose[6], -self.serverData.pose[3]
]
#[0.002087714894559653, 0.7072209410999575, -0.0021176456789796234, 0.7069863488731505]
transformMatrix = viz.Transform()
transformMatrix.setQuat(rot)
# Not sure why I need to rotate 180 degs here.
transformMatrix.postAxisAngle(0, 1, 0, 90)
#import vizmat
#euler_XYZ = vizmat.QuatToEuler(transformMatrix.getQuat())
quat_XYZW = transformMatrix.getQuat()
quat_XYZW[3] = -quat_XYZW[3]
print str(quat_XYZW)
self.rotateRigid(quat_XYZW)
def __init__(self,
world,
space,
shape,
pos,
size=[],
bounciness=1,
friction=0,
vertices=None,
indices=None):
self.node3D = viz.addGroup()
# If it is NOT linked it updates seld.node3D pose with pos/quat pose on each frame
# If it is NOT linked it updates pos/quat pose with node3D pose on each frame
# This allows a linked phsynode to be moved around by an external source via the node3D
self.isLinked = 0
self.geom = 0
self.body = 0
self.parentWorld = []
self.parentSpace = []
self.bounciness = bounciness
self.friction = friction
# A list of bodies that it will stick to upon collision
self.stickTo_gIdx = []
self.collisionPosLocal_XYZ = []
if shape == 'plane':
# print 'phsEnv.createGeom(): type=plane expects pos=ABCD,and NO size. SIze is auto infinite.'
self.geom = ode.GeomPlane(space, [pos[0], pos[1], pos[2]], pos[3])
self.parentSpace = space
# No more work needed
elif shape == 'sphere':
#print 'Making sphere physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setSphereTotal(mass, size)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomSphere(space, size)
self.geom.setBody(self.body)
self.parentSpace = space
################################################################################################
################################################################################################
#elif shape == 'cylinder':
elif ('cylinder' in shape):
#print 'Making cylinder physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
radius = size[1]
length = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
if (shape[-2:] == '_X'):
direction = 1
elif (shape[-2:] == '_Y'):
direction = 2
else:
direction = 3 # direction - The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
self.geomMass.setCylinderTotal(mass, direction, radius, length)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomCylinder(space, radius, length)
self.geom.setPosition(pos)
self.geom.setBody(self.body)
# This bit compensates for a problem with ODE
# Note how the body is created in line with any axis
# When I wrote this note, it was in-line with Y (direction=2)
# The geom, however, can only be made in-line with the Z axis
# This creates an offset to bring the two in-line
vizOffsetTrans = viz.Transform()
if (shape[-2:] == '_X'):
vizOffsetTrans.setAxisAngle(1, 0, 0, 90)
elif (shape[-2:] == '_Y'):
vizOffsetTrans.setAxisAngle(0, 0, 1, 90)
vizOffsetQuat = vizOffsetTrans.getQuat()
odeRotMat = self.vizQuatToRotationMat(vizOffsetQuat)
#print self.geom.getRotation()
self.geom.setOffsetRotation(odeRotMat)
self.parentSpace = space
elif shape == 'box':
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
length = size[1]
width = size[2]
height = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setBoxTotal(mass, length, width, height)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomBox(space, [length, width, height])
self.geom.setPosition(pos)
self.geom.setBody(self.body)
self.parentSpace = space
elif shape == 'trimesh':
if (vertices == None or indices == None):
print 'physNode.init(): For trimesh, must pass in vertices and indices'
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
triMeshData = ode.TrisMeshData()
triMeshData.build(vertices, indices)
self.geom = ode.GeomTriMesh(td, space)
self.geom.setBody(self.body)
## Set parameters for drawing the trimesh
body.shape = "trimesh"
body.geom = self.geom
self.parentSpace = space
else:
print 'physEnv.physNode.init(): ' + str(
type) + ' not implemented yet!'
return
pass
#print '**************UPDATING THE NODE *****************'
self.updateNodeAct = vizact.onupdate(viz.PRIORITY_PHYSICS,
self.updateNode3D)
def getOrient(self, dist):
m = viz.Transform()
m.set(0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 0, 1)
return m
def __init__(self, index, marker_ids, center_marker_ids):
super(RigidTracker, self).__init__(index, marker_ids)
self.center_marker_ids = center_marker_ids
self._pose = Pose(pos=(0, 0, 0), quat=(1, 0, 0, 0), cond=-1)
self._transform = viz.Transform()
