def _get_local_quat(self):
rot = self.get_fixed_attribute("r", default=vec3(0,0,0))
qx = quat().fromAngleAxis(rot[0], (1, 0, 0))
qy = quat().fromAngleAxis(rot[1], (0, 1, 0))
qz = quat().fromAngleAxis(rot[2], (0, 0, 1))
q = qz*qy*qx
return q
def __mul__(self, other):
"""Multiplication.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q*2.0
(1.9378, 0.4320, 0.2160, 0.1080)
>>> print 2.0*q
(1.9378, 0.4320, 0.2160, 0.1080)
>>> print q*q
(0.8775, 0.4186, 0.2093, 0.1046)
"""
T = type(other)
# quat*scalar
if T==types.FloatType or T==types.IntType or T==types.LongType:
return quat(self.w*other, self.x*other, self.y*other, self.z*other)
# quat*quat
if isinstance(other, quat):
w1,x1,y1,z1 = self.w,self.x,self.y,self.z
w2,x2,y2,z2 = other.w,other.x,other.y,other.z
return quat(w1*w2-x1*x2-y1*y2-z1*z2,
w1*x2+x1*w2+y1*z2-z1*y2,
w1*y2+y1*w2-x1*z2+z1*x2,
w1*z2+z1*w2+x1*y2-y1*x2)
# unsupported
else:
# Try to delegate the operation to the other operand
if getattr(other,"__rmul__",None)!=None:
return other.__rmul__(self)
else:
raise TypeError("unsupported operand type for *")
def _get_local_quat(self):
rot = self.get_fixed_attribute("r", default=vec3(0, 0, 0))
qx = quat().fromAngleAxis(rot[0], (1, 0, 0))
qy = quat().fromAngleAxis(rot[1], (0, 1, 0))
qz = quat().fromAngleAxis(rot[2], (0, 0, 1))
q = qy * qz * qx
return q
def __mul__(self, other):
"""Multiplication.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q*2.0
(1.9378, 0.4320, 0.2160, 0.1080)
>>> print 2.0*q
(1.9378, 0.4320, 0.2160, 0.1080)
>>> print q*q
(0.8775, 0.4186, 0.2093, 0.1046)
"""
T = type(other)
# quat*scalar
if T == types.FloatType or T == types.IntType or T == types.LongType:
return quat(self.w * other, self.x * other, self.y * other,
self.z * other)
# quat*quat
if isinstance(other, quat):
w1, x1, y1, z1 = self.w, self.x, self.y, self.z
w2, x2, y2, z2 = other.w, other.x, other.y, other.z
return quat(w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
w1 * y2 + y1 * w2 - x1 * z2 + z1 * x2,
w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2)
# unsupported
else:
# Try to delegate the operation to the other operand
if getattr(other, "__rmul__", None) != None:
return other.__rmul__(self)
else:
raise TypeError("unsupported operand type for *")
def __init__(self, beta=0, axis=Up, pos=Zero, beta_f=0, axis_f=Up, fwd=Fwd, left=Left, up=Up):
"""Sets angle, axis vector, and position vector;
also sets up a coordinate axis.
Since rotations require a normalized axis, we
take the norm of the axis we want to rotate by.
Note that the angle is given in bradians.
Note that the position is set *without* altering
the coordinate system. To match the
coordinate system with the current rotations,
call match_coordinates()."""
self.beta = beta
self.axis = axis.norm()
self.pos = pos
self.beta_f = beta_f
self.axis_f = axis_f
# We'll set up the object's coordinate system now:
self.Up = up
self.Fwd = fwd
self.Left = left
self.coordination = quat(1) # this is the identity rotation.
# These are the dual quaternions used for the wireframe's position.
# First, we rotate around the center; then translate; finally rotate again.
self.init_rotation = quat(); self.init_rotation.rotation(self.beta, self.axis)
self.translation = quat(); self.translation.translation(self.pos)
self.final_rotation = quat(); self.final_rotation.rotation(self.beta_f, self.axis_f)
self.orientation = self.final_rotation*self.translation*self.init_rotation
self.world = self.orientation.matrix()
def marg_quats(*args, **kwargs):
'''
computes MARG sensor rotation quaternions
'''
marg_quat = kwargs.get('marg', quat(1))
quats = {}
for name in ['gyro', 'acc', 'mag']:
sensor_quat = kwargs.get(name, quat(1))
assert isinstance(sensor_quat, quat)
quats[name] = marg_quat * sensor_quat
return quats
def getcoordsys(self):
inv = self.coordination.inverse()
up = quat()
up.pureVector(self.Up)
up = self.coordination*up*inv
left = quat()
left.pureVector(self.Left)
left = self.coordination*left*inv
fwd = quat()
fwd.pureVector(self.Fwd)
fwd = self.coordination*fwd*inv
return (fwd.extractPureVector(), left.extractPureVector(), up.extractPureVector())
def __init__(self, *args):
"""Constructor.
0 arguments: zeroes
1 float argument: w component, x,y,z = (0,0,0)
1 quat argument: Make a copy
1 mat3 argument: Initialize by rotation matrix
1 mat4 argument: Initialize by rotation matrix
2 arguments: angle & axis (doesn't have to be of unit length)
4 arguments: components w,x,y,z
"""
# 0 arguments
if len(args) == 0:
self.w, self.x, self.y, self.z = (0.0, 0.0, 0.0, 0.0)
# 1 arguments
elif len(args) == 1:
T = type(args[0])
# Scalar
if T == types.FloatType or T == types.IntType or T == types.LongType:
self.w = args[0]
self.x, self.y, self.z = (0.0, 0.0, 0.0)
# quat
elif isinstance(args[0], quat):
q = args[0]
self.w = q.w
self.x = q.x
self.y = q.y
self.z = q.z
# mat3 or mat4
elif isinstance(args[0], _mat3) or isinstance(args[0], _mat4):
self.fromMat(args[0])
# List or Tuple
elif T == types.ListType or T == types.TupleType:
self.w, self.x, self.y, self.z = args[0]
# String
elif T == types.StringType:
s = args[0].replace(",", " ").replace(" ",
" ").strip().split(" ")
f = map(lambda x: float(x), s)
dummy = quat(f)
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
else:
raise TypeError, "quat() arg can't be converted to quat"
# 2 arguments (angle & axis)
elif len(args) == 2:
angle, axis = args
self.fromAngleAxis(angle, axis)
# 4 arguments
elif len(args) == 4:
self.w, self.x, self.y, self.z = args
else:
raise TypeError, "quat() arg can't be converted to quat"
def slerp(t, q0, q1, shortest=True):
"""Spherical linear interpolation between two quaternions.
The return value is an interpolation between q0 and q1. For t=0.0
the return value equals q0, for t=1.0 it equals q1.
q0 and q1 must be unit quaternions.
If shortest is True the interpolation is always done along the
shortest path.
"""
global _epsilon
ca = q0.dot(q1)
if shortest and ca < 0:
ca = -ca
neg_q1 = True
else:
neg_q1 = False
if ca >= 1.0:
o = 0.0
elif ca <= -1.0:
o = math.pi
else:
o = math.acos(ca)
so = math.sin(o)
if (abs(so) <= _epsilon):
return quat(q0)
a = math.sin(o * (1.0 - t)) / so
b = math.sin(o * t) / so
if neg_q1:
return q0 * a - q1 * b
else:
return q0 * a + q1 * b
def fromMat(self, m):
try:
return self._fromMat(m)
except:
pass
bestqlist = []
bestcnt = 0
for exponent in range(2, 7):
qlist = []
dist = 10**-exponent
for axis in [(-1,0,0),(+1,0,0),(0,-1,0),(0,1,0),(0,0,-1),(0,0,1)]:
rot = m * _mat4.rotation(dist, axis)
try:
qlist += [self._fromMat(rot)]
except:
pass
if len(qlist) >= bestcnt:
bestcnt = len(qlist)
bestqlist += qlist
else:
break
qlist = bestqlist
r = quat(0,0,0,0)
for q in qlist:
r += q
r = r.normalize()
#print("Got a matrix-2-quaternion lerp of", r, "using", len(qlist), "checks and dist", dist)
self.w, self.x, self.y, self.z = r[:]
return self
def log(self):
"""Return the natural logarithm of self."""
global _epsilon
b = math.sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
res = quat()
if abs(b)<=_epsilon:
res.x = 0.0
res.y = 0.0
res.z = 0.0
if self.w<=_epsilon:
raise ValueError("math domain error")
res.w = math.log(self.w)
else:
t = math.atan2(b, self.w)
f = t/b
res.x = f*self.x
res.y = f*self.y
res.z = f*self.z
ct = math.cos(t)
if abs(ct)<=_epsilon:
raise ValueError("math domain error")
r = self.w/ct
if r<=_epsilon:
raise ValueError("math domain error")
res.w = math.log(r)
return res
def fromMat(self, m):
try:
return self._fromMat(m)
except:
pass
bestqlist = []
bestcnt = 0
for exponent in range(2, 7):
qlist = []
dist = 10**-exponent
for axis in [(-1, 0, 0), (+1, 0, 0), (0, -1, 0), (0, 1, 0),
(0, 0, -1), (0, 0, 1)]:
rot = m * _mat4.rotation(dist, axis)
try:
qlist += [self._fromMat(rot)]
except:
pass
if len(qlist) >= bestcnt:
bestcnt = len(qlist)
bestqlist += qlist
else:
break
qlist = bestqlist
r = quat(0, 0, 0, 0)
for q in qlist:
r += q
r = r.normalize()
#print("Got a matrix-2-quaternion lerp of", r, "using", len(qlist), "checks and dist", dist)
self.w, self.x, self.y, self.z = r[:]
return self
def __pos__(self):
"""
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print +q
(0.9689, 0.2160, 0.1080, 0.0540)
"""
return quat(+self.w, +self.x, +self.y, +self.z)
def __pos__(self):
"""
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print +q
(0.9689, 0.2160, 0.1080, 0.0540)
"""
return quat(+self.w, +self.x, +self.y, +self.z)
def slerp(t, q0, q1, shortest=True):
"""Spherical linear interpolation between two quaternions.
The return value is an interpolation between q0 and q1. For t=0.0
the return value equals q0, for t=1.0 it equals q1.
q0 and q1 must be unit quaternions.
If shortest is True the interpolation is always done along the
shortest path.
"""
global _epsilon
ca = q0.dot(q1)
if shortest and ca<0:
ca = -ca
neg_q1 = True
else:
neg_q1 = False
if ca>=1.0:
o = 0.0
elif ca<=-1.0:
o = math.pi
else:
o = math.acos(ca)
so = math.sin(o)
if (abs(so)<=_epsilon):
return quat(q0)
a = math.sin(o*(1.0-t)) / so
b = math.sin(o*t) / so
if neg_q1:
return q0*a - q1*b
else:
return q0*a + q1*b
def log(self):
"""Return the natural logarithm of self."""
global _epsilon
b = math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
res = quat()
if abs(b) <= _epsilon:
res.x = 0.0
res.y = 0.0
res.z = 0.0
if self.w <= _epsilon:
raise ValueError("math domain error")
res.w = math.log(self.w)
else:
t = math.atan2(b, self.w)
f = t / b
res.x = f * self.x
res.y = f * self.y
res.z = f * self.z
ct = math.cos(t)
if abs(ct) <= _epsilon:
raise ValueError("math domain error")
r = self.w / ct
if r <= _epsilon:
raise ValueError("math domain error")
res.w = math.log(r)
return res
def conjugate(self):
"""Return conjugate.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q.conjugate()
(0.9689, -0.2160, -0.1080, -0.0540)
"""
return quat(self.w, -self.x, -self.y, -self.z)
def conjugate(self):
"""Return conjugate.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q.conjugate()
(0.9689, -0.2160, -0.1080, -0.0540)
"""
return quat(self.w, -self.x, -self.y, -self.z)
def __neg__(self):
"""Negation.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print -q
(-0.9689, -0.2160, -0.1080, -0.0540)
"""
return quat(-self.w, -self.x, -self.y, -self.z)
def __neg__(self):
"""Negation.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print -q
(-0.9689, -0.2160, -0.1080, -0.0540)
"""
return quat(-self.w, -self.x, -self.y, -self.z)
def translate_by_Left(self, velocity=1):
"""Tranlsates by the object's Left axis, multiplied by velocity."""
# To avoid round-off errors, we'll keep the coordinate
# axes constant, and just keep track of their orientation
# (called self.coordination).
# First, we need to calculate the Left vector.
leftQuat = quat()
leftQuat.pureVector(self.Left)
left = self.coordination*leftQuat*self.coordination.inverse()
# Second, we create the translation
translate = quat()
translate.translation(left.extractPureVector()*velocity)
# Finally, we apply the translation!
self.translation = self.translation*translate
self.pos = self.translation.get_translation()
def rotate_by_Up(self, beta=2):
"""Rotates by the object's up axis; also rotates
the remaining two axes by the same amount."""
# To avoid round-off errors, we'll keep the coordinate
# axes constant, and just keep track of their orientation
# (called self.coordination).
upQuat = quat()
upQuat.pureVector(self.Up)
up = self.coordination*upQuat*self.coordination.inverse()
rotation = quat()
rotation.rotation(beta, up.extractPureVector())
self.init_rotation = self.init_rotation*rotation
self.beta, self.axis = self.init_rotation.get_angle_axis()
# Now, we'll "rotate" the coordinate system, by applying
# the same rotation to the coordinate system quaternion.
self.coordination = rotation * self.coordination
def get_final_local_transform(self):
pm = self.xformparent.get_world_transform()
wt = self.get_world_transform()
if not self.phys_root:
wt = node.gettransformto(None, "inverse_initial_r")
m = pm.inverse() * wt
t, r, _ = m.decompose()
q = quat(r).normalize()
ps = pm.decompose()[2]
pos = mat4.scaling(ps) * vec4(*t)
return pos, q
def get_final_local_transform(self):
pm = self.xformparent.get_world_transform()
wt = self.get_world_transform()
if not self.phys_root:
wt = node.gettransformto(None, "inverse_initial_r")
m = pm.inverse() * wt
t, r, _ = m.decompose()
q = quat(r).normalize()
ps = pm.decompose()[2]
pos = mat4.scaling(ps) * vec4(*t)
return pos, q
def __add__(self, other):
"""Addition.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q+q
(1.9378, 0.4320, 0.2160, 0.1080)
"""
if isinstance(other, quat):
return quat(self.w+other.w, self.x+other.x,
self.y+other.y, self.z+other.z)
else:
raise TypeError("unsupported operand type for +")
def __sub__(self, other):
"""Subtraction.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q-q
(0.0000, 0.0000, 0.0000, 0.0000)
"""
if isinstance(other, quat):
return quat(self.w-other.w, self.x-other.x,
self.y-other.y, self.z-other.z)
else:
raise TypeError("unsupported operand type for +")
def __sub__(self, other):
"""Subtraction.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q-q
(0.0000, 0.0000, 0.0000, 0.0000)
"""
if isinstance(other, quat):
return quat(self.w - other.w, self.x - other.x, self.y - other.y,
self.z - other.z)
else:
raise TypeError("unsupported operand type for +")
def get_final_mesh_transform(self, physrootpos, bodies, verbose): phys = self.get_mesh_parentphys(bodies) if not phys: return None,None,None,None,None phys.writecount = 1 tm = self.get_world_transform() tp = phys.gettransformto(None, "actual", getparent=lambda n: n.getParent()) tmt, tmr, mscale = tm.decompose() tpt, tpr, _ = tp.decompose() q = quat(tpr.inverse()).normalize() p = tmt-tpt p = q.toMat4() * vec4(p[:]) if verbose: print(self.getName(), "has displacement", p, "compared to", phys.getName(), tmt, tpt) q = quat(tpr.inverse() * tmr).normalize() physidx = bodies.index(phys) if physidx == 0 and physrootpos: p -= physrootpos p = p[0:3] mscale = mscale.length() / (1+1+1)**.5 return phys,physidx,q,p,mscale
def __add__(self, other):
"""Addition.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q+q
(1.9378, 0.4320, 0.2160, 0.1080)
"""
if isinstance(other, quat):
return quat(self.w + other.w, self.x + other.x, self.y + other.y,
self.z + other.z)
else:
raise TypeError("unsupported operand type for +")
def normalize(self):
"""Return normalized quaternion.
>>> q=quat(0.9, 0.5, 0.2, 0.3)
>>> q=q.normalize()
>>> print q
(0.8250, 0.4583, 0.1833, 0.2750)
>>> print abs(q)
1.0
"""
nlen = 1.0/abs(self)
return quat(self.w*nlen, self.x*nlen, self.y*nlen, self.z*nlen)
def normalize(self):
"""Return normalized quaternion.
>>> q=quat(0.9, 0.5, 0.2, 0.3)
>>> q=q.normalize()
>>> print q
(0.8250, 0.4583, 0.1833, 0.2750)
>>> print abs(q)
1.0
"""
nlen = 1.0 / abs(self)
return quat(self.w * nlen, self.x * nlen, self.y * nlen, self.z * nlen)
def get_final_local_transform(self):
pm = self.xformparent.get_world_transform()
wt = self.get_world_transform()
if not self.phys_root:
wt = node.gettransformto(None, "inverse_initial_r")
m = pm.inverse() * wt
jointtype = self.get_fixed_attribute("joint", True)
if self.is_phys_root and jointtype not in (None, "exclude"):
m *= self._pointup_adjust_mat()
t, r, _ = m.decompose()
q = quat(r).normalize()
ps = pm.decompose()[2]
pos = mat4.scaling(ps) * vec4(*t)
return pos, q
def __div__(self, other):
"""Division.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q/2.0
(0.4844, 0.1080, 0.0540, 0.0270)
"""
T = type(other)
# quat/scalar
if T==types.FloatType or T==types.IntType or T==types.LongType:
return quat(self.w/other, self.x/other, self.y/other, self.z/other)
# unsupported
else:
raise TypeError, "unsupported operand type for /"
def __div__(self, other):
"""Division.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q/2.0
(0.4844, 0.1080, 0.0540, 0.0270)
"""
T = type(other)
# quat/scalar
if T==types.FloatType or T==types.IntType or T==types.LongType:
return quat(self.w/other, self.x/other, self.y/other, self.z/other)
# unsupported
else:
raise TypeError("unsupported operand type for /")
def __truediv__(self, other):
"""Division.
>>> q=quat(0.9689, 0.2160, 0.1080, 0.0540)
>>> print q/2.0
(0.4844, 0.1080, 0.0540, 0.0270)
"""
T = type(other)
# quat/scalar
if T in [float, int]:
return quat(self.w / other, self.x / other, self.y / other,
self.z / other)
# unsupported
else:
raise TypeError("unsupported operand type for /")
def centerverts(group, bodies, verbose):
for shape in group:
vtx = shape.get_fixed_attribute("rgvtx", optional=True)
if not vtx:
continue
mesh = shape.getParent();
if not mesh.get_fixed_attribute("center_vertices", optional=True):
continue
# Center around physics position, by removing translational offset for each vertex.
phys,physidx,q,p,s = mesh.get_final_mesh_transform(None, bodies, verbose)
p = quat(q).rotateVec(p)
if verbose:
print("Offsetting %s's %i vertices around %s, moving them %s." % (mesh.getName(), len(vtx)/3, phys.getName(), str(p)))
for idx in range(0, len(vtx), 3):
v = vec3(vtx[idx:idx+3]) + p
vtx[idx:idx+3] = v[:3]
def exp(self):
"""Return the exponential of self."""
global _epsilon
b = math.sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
res = quat()
if abs(b)<=_epsilon:
res.x = 0.0
res.y = 0.0
res.z = 0.0
res.w = math.exp(self.w)
else:
f = math.sin(b)/b
res.x = f*self.x
res.y = f*self.y
res.z = f*self.z
res.w = math.exp(self.w)*math.cos(b)
return res
def exp(self):
"""Return the exponential of self."""
global _epsilon
b = math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
res = quat()
if abs(b) <= _epsilon:
res.x = 0.0
res.y = 0.0
res.z = 0.0
res.w = math.exp(self.w)
else:
f = math.sin(b) / b
res.x = f * self.x
res.y = f * self.y
res.z = f * self.z
res.w = math.exp(self.w) * math.cos(b)
return res
def getLeftCoord(self):
left = quat()
left.pureVector(self.Left)
left = self.coordination*left*self.coordination.inverse()
return left.extractPureVector()
def add_final_rotation(self, beta, axis):
addition = quat()
addition.rotation(beta, axis)
self.final_rotation = addition*self.final_rotation
self.beta_f, self.axis_f = self.final_rotation.get_angle_axis()
def __pow__(self, other):
"""Return self**q."""
# if modulo!=None:
# raise TypeError, "unsupported operation"
q = quat(other)
return (q * self.log()).exp()
def __init__(self, *args):
"""Constructor.
0 arguments: zeroes
1 float argument: w component, x,y,z = (0,0,0)
1 quat argument: Make a copy
1 mat3 argument: Initialize by rotation matrix
1 mat4 argument: Initialize by rotation matrix
2 arguments: angle & axis (doesn't have to be of unit length)
4 arguments: components w,x,y,z
"""
# 0 arguments
if len(args) == 0:
self.w, self.x, self.y, self.z = (0.0, 0.0, 0.0, 0.0)
# 1 arguments
elif len(args) == 1:
T = type(args[0])
# Scalar
if T in [float, int]:
self.w = float(args[0])
self.x, self.y, self.z = (0.0, 0.0, 0.0)
# quat
elif isinstance(args[0], quat):
q = args[0]
self.w = q.w
self.x = q.x
self.y = q.y
self.z = q.z
# mat3 or mat4
elif isinstance(args[0], _mat3) or isinstance(args[0], _mat4):
self.fromMat(args[0])
# List or Tuple
elif T in [list, tuple]:
dummy = quat(*args[0])
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
# String
elif T is str:
s = args[0].replace(",", " ").replace(" ",
" ").strip().split(" ")
if s == [""]:
s = []
f = list([float(x) for x in s])
dummy = quat(f)
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
else:
raise TypeError("quat() arg can't be converted to quat")
# 2 arguments (angle & axis)
elif len(args) == 2:
angle, axis = args
self.fromAngleAxis(angle, axis)
# 4 arguments
elif len(args) == 4:
w, x, y, z = args
self.w = float(w)
self.x = float(x)
self.y = float(y)
self.z = float(z)
else:
raise TypeError("quat() arg can't be converted to quat")
def getFwdCoord(self):
fwd = quat()
fwd.pureVector(self.Fwd)
fwd = self.coordination*fwd*self.coordination.inverse()
return fwd.extractPureVector()
def add_init_rotation(self, beta, axis):
addition = quat()
addition.rotation(beta, axis)
self.init_rotation = addition*self.init_rotation
self.beta, self.axis = self.init_rotation.get_angle_axis()
def __init__(self, *args):
"""Constructor.
0 arguments: zeroes
1 float argument: w component, x,y,z = (0,0,0)
1 quat argument: Make a copy
1 mat3 argument: Initialize by rotation matrix
1 mat4 argument: Initialize by rotation matrix
2 arguments: angle & axis (doesn't have to be of unit length)
4 arguments: components w,x,y,z
"""
# 0 arguments
if len(args)==0:
self.w, self.x, self.y, self.z = (0.0, 0.0, 0.0, 0.0)
# 1 arguments
elif len(args)==1:
T = type(args[0])
# Scalar
if T==types.FloatType or T==types.IntType or T==types.LongType:
self.w = float(args[0])
self.x, self.y, self.z = (0.0, 0.0, 0.0)
# quat
elif isinstance(args[0], quat):
q=args[0]
self.w = q.w
self.x = q.x
self.y = q.y
self.z = q.z
# mat3 or mat4
elif isinstance(args[0], _mat3) or isinstance(args[0], _mat4):
self.fromMat(args[0])
# List or Tuple
elif T==types.ListType or T==types.TupleType:
dummy = quat(*args[0])
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
# String
elif T==types.StringType:
s=args[0].replace(","," ").replace(" "," ").strip().split(" ")
if s==[""]:
s=[]
f=map(lambda x: float(x), s)
dummy = quat(f)
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
else:
raise TypeError("quat() arg can't be converted to quat")
# 2 arguments (angle & axis)
elif len(args)==2:
angle, axis = args
self.fromAngleAxis(angle,axis)
# 4 arguments
elif len(args)==4:
w,x,y,z = args
self.w = float(w)
self.x = float(x)
self.y = float(y)
self.z = float(z)
else:
raise TypeError("quat() arg can't be converted to quat")
def add_translation(self, pos):
addition = quat()
addition.translation(pos)
self.translation = self.translation * addition
self.pos = self.translation.get_translation()
def __init__(s, x, M, v):
""" Initialize the plane, at point x, and with mass M, velocity
vector v """
s.P = x # The center position
s.left = vector(-1.0, 0.0, 0.0) # the left wing
s.right = vector(1.0, 0.0, 0.0) # the right wing
s.tail = vector(0.0, 0.0, -1.0) # the tail
s.nose = vector(0.0, 0.0, 1.0) # the nose
s.up = vector(0.0, 1.0, 0.0) # up vector
# The vectors below are the ROTATED vectors
# (call rotateVectors() to update them)
s.l = vector(-1.0, 0.0, 0.25) # the left wing
s.r = vector(1.0, 0.0, 0.25) # the right wing
s.t = vector(0.0, 0.0, -1.0) # the tail
s.n = vector(0.0, 0.0, 1.0) # the nose
s.lift = vector(0.0, 1.0, 0.0) # The lift vector
s.acc = vector(0.0, 0.0, 0.0)
s.omega = matrix([0, 0, 0]) # represents rotational velocity
s.M = M # total mass of the plane
s.PForces = [] # Forces acting on plane overall -
# these will move the plane around linearly
# Each part of the plane has its own list of forces.
# These will constribute to the plane's rotation.
# Gravity acts on everything, so it's allllways there
s.lForces = [] # left wing forces
s.rForces = [] # right wing forces
s.nForces = [] # nose forces
s.tForces = [] # forces on the tail
s.pointForces = {} # Point force dictionary -
# allows you to get forces lists by name
s.pointForces['left'] = s.lForces
s.pointForces['right'] = s.rForces
s.pointForces['nose'] = s.nForces
s.pointForces['tail'] = s.tForces
s.pointForces['l'] = s.lForces
s.pointForces['r'] = s.rForces
s.pointForces['n'] = s.nForces
s.pointForces['t'] = s.tForces
s.I = matrix([[0.177721, 0.0, 0.0], [0.0, 0.304776, 0.0],
[0.0, 0.0, 0.177721]]) * 100
# This is the inertial tensor.
# It represents the plane's distribution of mass.
# Currently, it assumes the plane is a uniform disk shape; obviously
# this could be improved!
s.Iinv = linalg.inv(s.I)
# The state of the airplane:
# Rotation matrix
s.q = quat(0.0, vector(1.0, 0.0, 0.0)) # Rotation quaternion
s.R = matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0],
[0.0, 0.0,
1.0]]) # The airplane starts out straight+level
s.RDot = matrix([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]]) # Rate of change of rot. matrix
s.V = v # starting velocity vector
s.AV = vector(0.0, 0.0, 0.0) # starting angular velocity
s.LM = v.scale(s.M) # the linear momentum
s.AM = vector(0.0, 0.0, 0.0) # the angular momentum
rigidBody.instances.append(s)
def getUpCoord(self):
up = quat()
up.pureVector(self.Up)
up = self.coordination*up*self.coordination.inverse()
return up.extractPureVector()
def __pow__(self, other):
"""Return self**q."""
# if modulo!=None:
# raise TypeError, "unsupported operation"
q = quat(other)
return (q*self.log()).exp()
