def transform(self, mesh, face_up=False):
x = self.getUp()
if face_up:
z = pgl.Vector3(0, 0, 1)
else:
z = self.getHeading()
bo = pgl.BaseOrientation(x, z ^ x)
matrix = pgl.Transform4(bo.getMatrix())
matrix.translate(self.getPosition())
# print 'Position ', turtle.getPosition()
mesh = mesh.transform(matrix)
return mesh
def adjust_lu(m):
t = m.getColumn(3)
t = Vector3(t.x, t.y, t.z)
m3 = pgl.Matrix3(m)
x, y, z = Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)
X, Y, Z = m * Vector4(1, 0, 0, 0), m * Vector4(0, 1, 0, 0), m * Vector4(0, 0, 1, 0)
Z = Vector3(Z.x, Z.y, Z.z)
new_x = z ^ Z
local_m = m
if pgl.normSquared(new_x) > 1e-3:
new_y = Z ^ new_x
new_x.normalize()
new_y.normalize()
local_m = pgl.BaseOrientation(new_x, new_y).getMatrix()
local_m = local_m.translation(t) * local_m
return local_m