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()