def setQuaternion(self, Quaternion):
"""Set the orientation of the body in world coordinates (the display object is oriented to match)."""
odelib.BaseBody.setQuaternion(self, Quaternion)
# now set the orientation of the display frame
# Rotating a point is q * (0,v) * q-1
# q-1 is w, -x, -y, -z assuming that q is a unit quaternion
w1, x1, y1, z1 = Quaternion
v1 = ivisual.vector(x1, y1, z1)
w3 = w1
v3 = -v1
# First do the axis vector
w2 = 0.0
v2 = ivisual.vector((1.0, 0.0, 0.0))
# This code is equivalent to a quaternion multiply: qR = q1 * q2 * q3
w12 = (w1 * w2) - ivisual.dot(v1, v2)
v12 = (w1 * v2) + (w2 * v1) + ivisual.cross(v1, v2)
wR = (w12 * w3) - ivisual.dot(v12, v3)
vR = (w12 * v3) + (w3 * v12) + ivisual.cross(v12, v3)
self._myFrame.axis = vR
# Do it again for the up vector
w2 = 0.0
v2 = ivisual.vector((0.0, 1.0, 0.0))
# This code is equivalent to a quaternion multiply: qR = q1 * q2 * q3
w12 = (w1 * w2) - ivisual.dot(v1, v2)
v12 = (w1 * v2) + (w2 * v1) + ivisual.cross(v1, v2)
wR = (w12 * w3) - ivisual.dot(v12, v3)
vR = (w12 * v3) + (w3 * v12) + ivisual.cross(v12, v3)
self._myFrame.up = vR
def setQuaternion(self, Quaternion):
"""Set the orientation of the body in world coordinates (the display object is oriented to match)."""
odelib.BaseBody.setQuaternion(self, Quaternion)
# now set the orientation of the display frame
# Rotating a point is q * (0,v) * q-1
# q-1 is w, -x, -y, -z assuming that q is a unit quaternion
w1, x1, y1, z1 = Quaternion
v1 = ivisual.vector(x1, y1, z1)
w3 = w1
v3 = -v1
# First do the axis vector
w2 = 0.0
v2 = ivisual.vector((1.0, 0.0, 0.0))
# This code is equivalent to a quaternion multiply: qR = q1 * q2 * q3
w12 = (w1 * w2) - ivisual.dot(v1, v2)
v12 = (w1 * v2) + (w2 * v1) + ivisual.cross(v1, v2)
wR = (w12 * w3) - ivisual.dot(v12, v3)
vR = (w12 * v3) + (w3 * v12) + ivisual.cross(v12, v3)
self._myFrame.axis = vR
# Do it again for the up vector
w2 = 0.0
v2 = ivisual.vector((0.0, 1.0, 0.0))
# This code is equivalent to a quaternion multiply: qR = q1 * q2 * q3
w12 = (w1 * w2) - ivisual.dot(v1, v2)
v12 = (w1 * v2) + (w2 * v1) + ivisual.cross(v1, v2)
wR = (w12 * w3) - ivisual.dot(v12, v3)
vR = (w12 * v3) + (w3 * v12) + ivisual.cross(v12, v3)
self._myFrame.up = vR
def GetElementWorldLocation(self, ElementKey):
"""Returns the location of the specified element in world coordinates."""
assert ElementKey in self._elementDict
self._elementDict.get(ElementKey).SetVisible(isVisible)
localPos = self.GetElement(ElementKey).pos
xAxis = ivisual.norm(self._myFrame.axis)
zAxis = ivisual.norm(ivisual.cross(self._myFrame.axis, self._myFrame.up))
yAxis = ivisual.norm(ivisual.cross(z_axis, x_axis))
worldPos = self._myFrame.pos + (localPos.x * xAxis) + (localPos.y * yAxis) + (localPos.z * zAxis)
return worldPos
def GetElementWorldLocation(self, ElementKey):
"""Returns the location of the specified element in world coordinates."""
assert ElementKey in self._elementDict
self._elementDict.get(ElementKey).SetVisible(isVisible)
localPos = self.GetElement(ElementKey).pos
xAxis = ivisual.norm(self._myFrame.axis)
zAxis = ivisual.norm(
ivisual.cross(self._myFrame.axis, self._myFrame.up))
yAxis = ivisual.norm(ivisual.cross(z_axis, x_axis))
worldPos = self._myFrame.pos + (localPos.x * xAxis) + (
localPos.y * yAxis) + (localPos.z * zAxis)
return worldPos
def build(self, Verticies, Faces, Colors=None, Normals=None, IsSoft=False):
"""Build a displayable tri-mesh object. This is an overload of ode.TriMeshData.build()."""
# Fill out our TriMeshData object properties by calling the parent
ode.TriMeshData.build(self, Verticies, Faces)
self._verticies = []
self._colors = []
self._normals = []
# Expand the mesh to make it suitable for use with Visual
for face in Faces:
for index, vertex in enumerate(face):
self._verticies.append(Verticies[vertex])
if Colors is not None:
self._colors.append(Colors[vertex])
else:
# Assign a default
self._colors.append(ivisual.color.white)
if Normals is not None:
self._normals.append(Normals[vertex])
else:
# Compute the normals using cross products
normal = ivisual.norm(
ivisual.cross(Verticies[face[index]],
Verticies[face[(index + 1) % 3]]))
self._normals.append(normal)
if IsSoft:
# Need to go back and average normals -- implement later
pass
def build(self, Verticies, Faces, Colors=None, Normals=None, IsSoft=False):
"""Build a displayable tri-mesh object. This is an overload of ode.TriMeshData.build()."""
# Fill out our TriMeshData object properties by calling the parent
ode.TriMeshData.build(self, Verticies, Faces)
self._verticies = []
self._colors = []
self._normals = []
# Expand the mesh to make it suitable for use with Visual
for face in Faces:
for index, vertex in enumerate(face):
self._verticies.append(Verticies[vertex])
if Colors is not None:
self._colors.append(Colors[vertex])
else:
# Assign a default
self._colors.append(ivisual.color.white)
if Normals is not None:
self._normals.append(Normals[vertex])
else:
# Compute the normals using cross products
normal = ivisual.norm(ivisual.cross(Verticies[face[index]], Verticies[face[(index + 1) % 3]]))
self._normals.append(normal)
if IsSoft:
# Need to go back and average normals -- implement later
pass
