def decompose(self):
"""Decomposes the matrix into a rotation and scaling part.
Returns a tuple (rotation, scaling). The scaling part is given
as a vec3, the rotation is still a mat3.
"""
try:
dummy = self.ortho()
except ZeroDivisionError:
return (mat3(1.0), _vec3(0))
x = dummy.getColumn(0)
y = dummy.getColumn(1)
z = dummy.getColumn(2)
xl = x.length()
yl = y.length()
zl = z.length()
scale = _vec3(xl, yl, zl)
x /= xl
y /= yl
z /= zl
dummy.setColumn(0, x)
dummy.setColumn(1, y)
dummy.setColumn(2, z)
if dummy.determinant() < 0.0:
dummy.setColumn(0, -x)
scale.x = -scale.x
return (dummy, scale)
def decompose(self):
"""Decomposes the matrix into a translation, rotation and scaling part.
Returns a tuple (translation, rotation, scaling). The
translation and scaling parts are given as vec3's, the rotation
is still given as a mat4.
"""
dummy = self.ortho()
dummy.setRow(3, _vec4(0.0, 0.0, 0.0, 1.0))
x = dummy.getColumn(0)
y = dummy.getColumn(1)
z = dummy.getColumn(2)
xl = x.length()
yl = y.length()
zl = z.length()
scale = _vec3(xl, yl, zl)
x /= xl
y /= yl
z /= zl
dummy.setColumn(0, x)
dummy.setColumn(1, y)
dummy.setColumn(2, z)
if dummy.determinant() < 0.0:
dummy.setColumn(0, -x)
scale.x = -scale.x
return (_vec3(self.mlist[3], self.mlist[7],
self.mlist[11]), dummy, scale)
def lookAt(pos, target, up=_vec3(0, 0, 1)):
"""Look from pos to target.
The resulting transformation moves the origin to pos and
rotates so that the z-axis points to target. The y-axis is
as close as possible to the up vector.
"""
pos = _vec3(pos)
target = _vec3(target)
up = _vec3(up)
dir = (target - pos).normalize()
up = up.normalize()
up -= (up * dir) * dir
try:
up = up.normalize()
except:
# We're looking along the up direction, so choose
# an arbitrary direction that is perpendicular to dir
# as new up.
up = dir.ortho()
right = up.cross(dir).normalize()
return mat4(right.x, up.x, dir.x, pos.x, right.y, up.y, dir.y, pos.y,
right.z, up.z, dir.z, pos.z, 0.0, 0.0, 0.0, 1.0)
def rotateVec(self, v):
"""Return the rotated vector v.
The quaternion must be a unit quaternion.
This operation is equivalent to turning v into a quat, computing
self*v*self.conjugate() and turning the result back into a vec3.
"""
v = _vec3(v)
ww = self.w * self.w
xx = self.x * self.x
yy = self.y * self.y
zz = self.z * self.z
wx = self.w * self.x
wy = self.w * self.y
wz = self.w * self.z
xy = self.x * self.y
xz = self.x * self.z
yz = self.y * self.z
return _vec3(
ww * v.x + xx * v.x - yy * v.x - zz * v.x + 2 * ((xy - wz) * v.y +
(xz + wy) * v.z),
ww * v.y - xx * v.y + yy * v.y - zz * v.y + 2 * ((xy + wz) * v.x +
(yz - wx) * v.z),
ww * v.z - xx * v.z - yy * v.z + zz * v.z + 2 * ((xz - wy) * v.x +
(yz + wx) * v.y))
def lookAt(pos, target, up=_vec3(0,0,1)):
"""Look from pos to target.
The resulting transformation moves the origin to pos and
rotates so that the z-axis points to target. The y-axis is
as close as possible to the up vector.
"""
pos = _vec3(pos)
target = _vec3(target)
up = _vec3(up)
dir = (target - pos).normalize()
up = up.normalize()
up -= (up * dir) * dir
try:
up = up.normalize()
except:
# We're looking along the up direction, so choose
# an arbitrary direction that is perpendicular to dir
# as new up.
up = dir.ortho()
right = up.cross(dir).normalize()
return mat4(right.x, up.x, dir.x, pos.x,
right.y, up.y, dir.y, pos.y,
right.z, up.z, dir.z, pos.z,
0.0, 0.0, 0.0, 1.0)
def rotateVec(self, v):
"""Return the rotated vector v.
The quaternion must be a unit quaternion.
This operation is equivalent to turning v into a quat, computing
self*v*self.conjugate() and turning the result back into a vec3.
"""
u = _vec3(v[:3])
ww = self.w*self.w
xx = self.x*self.x
yy = self.y*self.y
zz = self.z*self.z
wx = self.w*self.x
wy = self.w*self.y
wz = self.w*self.z
xy = self.x*self.y
xz = self.x*self.z
yz = self.y*self.z
u = (ww*u.x + xx*u.x - yy*u.x - zz*u.x + 2*((xy-wz)*u.y + (xz+wy)*u.z),
ww*u.y - xx*u.y + yy*u.y - zz*u.y + 2*((xy+wz)*u.x + (yz-wx)*u.z),
ww*u.z - xx*u.z - yy*u.z + zz*u.z + 2*((xz-wy)*u.x + (yz+wx)*u.y))
if isinstance(v, _vec4):
return _vec4(u)
return _vec3(u)
def decompose(self):
"""Decomposes the matrix into a translation, rotation and scaling part.
Returns a tuple (translation, rotation, scaling). The
translation and scaling parts are given as vec3's, the rotation
is still given as a mat4.
"""
dummy = self.ortho()
dummy.setRow(3,_vec4(0.0, 0.0, 0.0, 1.0))
dummy.setColumn(3,_vec4(0.0, 0.0, 0.0, 1.0))
x = dummy.getColumn(0)
y = dummy.getColumn(1)
z = dummy.getColumn(2)
xl = x.length()
yl = y.length()
zl = z.length()
scale = _vec3(xl,yl,zl)
x/=xl
y/=yl
z/=zl
dummy.setColumn(0,x)
dummy.setColumn(1,y)
dummy.setColumn(2,z)
if dummy.determinant()<0.0:
dummy.setColumn(0,-x)
scale.x=-scale.x
return (_vec3(self.mlist[3], self.mlist[7], self.mlist[11]),
dummy,
scale)
def ortho(self):
"""Return a matrix with orthogonal base vectors.
Makes the x-, y- and z-axis orthogonal.
The fourth column and row remain untouched.
"""
m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34,m41,m42,m43,m44 = self.mlist
x = _vec3(m11, m21, m31)
y = _vec3(m12, m22, m32)
z = _vec3(m13, m23, m33)
xl = x.length()
xl*=xl
y = y - ((x*y)/xl)*x
z = z - ((x*z)/xl)*x
yl = y.length()
yl*=yl
z = z - ((y*z)/yl)*y
return mat4( x.x, y.x, z.x, m14,
x.y, y.y, z.y, m24,
x.z, y.z, z.z, m34,
m41, m42, m43, m44)
def decompose(self):
"""Decomposes the matrix into a rotation and scaling part.
Returns a tuple (rotation, scaling). The scaling part is given
as a vec3, the rotation is still a mat3.
"""
try:
dummy = self.ortho()
except ZeroDivisionError:
return (mat3(1.0), _vec3(0))
x = dummy.getColumn(0)
y = dummy.getColumn(1)
z = dummy.getColumn(2)
xl = x.length()
yl = y.length()
zl = z.length()
scale = _vec3(xl,yl,zl)
x/=xl
y/=yl
z/=zl
dummy.setColumn(0,x)
dummy.setColumn(1,y)
dummy.setColumn(2,z)
if dummy.determinant()<0.0:
dummy.setColumn(0,-x)
scale.x=-scale.x
return (dummy, scale)
def getColumn(self, idx):
"""Return a column (as vec3)."""
if idx==0: return _vec3(self.mlist[0], self.mlist[3], self.mlist[6])
elif idx==1: return _vec3(self.mlist[1], self.mlist[4], self.mlist[7])
elif idx==2: return _vec3(self.mlist[2], self.mlist[5], self.mlist[8])
else:
raise IndexError,"index out of range"
def getColumn(self, idx):
"""Return a column (as vec3)."""
if idx == 0: return _vec3(self.mlist[0], self.mlist[3], self.mlist[6])
elif idx == 1:
return _vec3(self.mlist[1], self.mlist[4], self.mlist[7])
elif idx == 2:
return _vec3(self.mlist[2], self.mlist[5], self.mlist[8])
else:
raise IndexError, "index out of range"
def __getitem__(self, key):
"""Return a column or an individual element."""
if key==0: return _vec3(self.mlist[0],self.mlist[3],self.mlist[6])
elif key==1: return _vec3(self.mlist[1],self.mlist[4],self.mlist[7])
elif key==2: return _vec3(self.mlist[2],self.mlist[5],self.mlist[8])
elif type(key)==types.TupleType:
i,j=key
if i<0 or i>2 or j<0 or j>2:
raise IndexError, "index out of range"
return self.mlist[i*3+j]
else:
raise IndexError,"index out of range"
def getRow(self, index):
"""Return a row (as vec3)."""
if type(index)==int:
if index==0:
return _vec3(self.mlist[0], self.mlist[1], self.mlist[2])
elif index==1:
return _vec3(self.mlist[3], self.mlist[4], self.mlist[5])
elif index==2:
return _vec3(self.mlist[6], self.mlist[7], self.mlist[8])
else:
raise IndexError,"index out of range"
else:
raise TypeError,"index must be an integer"
def getColumn(self, index):
"""Return a column (as vec3)."""
if type(index)==int:
if index==0:
return _vec3(self.mlist[0], self.mlist[3], self.mlist[6])
elif index==1:
return _vec3(self.mlist[1], self.mlist[4], self.mlist[7])
elif index==2:
return _vec3(self.mlist[2], self.mlist[5], self.mlist[8])
else:
raise IndexError("index out of range")
else:
raise TypeError("index must be an integer")
def getColumn(self, index):
"""Return a column (as vec3)."""
if type(index)==int:
if index==0:
return _vec3(self.mlist[0], self.mlist[3], self.mlist[6])
elif index==1:
return _vec3(self.mlist[1], self.mlist[4], self.mlist[7])
elif index==2:
return _vec3(self.mlist[2], self.mlist[5], self.mlist[8])
else:
raise IndexError("index out of range")
else:
raise TypeError("index must be an integer")
def getRow(self, index):
"""Return a row (as vec3)."""
if type(index) == int:
if index == 0:
return _vec3(self.mlist[0], self.mlist[1], self.mlist[2])
elif index == 1:
return _vec3(self.mlist[3], self.mlist[4], self.mlist[5])
elif index == 2:
return _vec3(self.mlist[6], self.mlist[7], self.mlist[8])
else:
raise IndexError, "index out of range"
else:
raise TypeError, "index must be an integer"
def __getitem__(self, key):
"""Return a column or an individual element."""
if key == 0: return _vec3(self.mlist[0], self.mlist[3], self.mlist[6])
elif key == 1:
return _vec3(self.mlist[1], self.mlist[4], self.mlist[7])
elif key == 2:
return _vec3(self.mlist[2], self.mlist[5], self.mlist[8])
elif type(key) == types.TupleType:
i, j = key
if i < 0 or i > 2 or j < 0 or j > 2:
raise IndexError, "index out of range"
return self.mlist[i * 3 + j]
else:
raise IndexError, "index out of range"
def rotation(angle, axis):
"""Return rotation matrix.
angle must be given in radians. axis should be of type vec3.
"""
axis = _vec3(axis)
sqr_a = axis.x * axis.x
sqr_b = axis.y * axis.y
sqr_c = axis.z * axis.z
len2 = sqr_a + sqr_b + sqr_c
k2 = math.cos(angle)
k1 = (1.0 - k2) / len2
k3 = math.sin(angle) / math.sqrt(len2)
k1ab = k1 * axis.x * axis.y
k1ac = k1 * axis.x * axis.z
k1bc = k1 * axis.y * axis.z
k3a = k3 * axis.x
k3b = k3 * axis.y
k3c = k3 * axis.z
return mat4(k1 * sqr_a + k2, k1ab - k3c, k1ac + k3b, 0.0, k1ab + k3c,
k1 * sqr_b + k2, k1bc - k3a, 0.0, k1ac - k3b, k1bc + k3a,
k1 * sqr_c + k2, 0.0, 0.0, 0.0, 0.0, 1.0)
def __mul__(self, other):
T = type(other)
# mat3*scalar
if T == types.FloatType or T == types.IntType or T == types.LongType:
return mat3(map(lambda x, other=other: x * other, self.mlist))
# mat3*vec3
if isinstance(other, _vec3):
m11, m12, m13, m21, m22, m23, m31, m32, m33 = self.mlist
return _vec3(m11 * other.x + m12 * other.y + m13 * other.z,
m21 * other.x + m22 * other.y + m23 * other.z,
m31 * other.x + m32 * other.y + m33 * other.z)
# mat3*mat3
if isinstance(other, mat3):
m11, m12, m13, m21, m22, m23, m31, m32, m33 = self.mlist
n11, n12, n13, n21, n22, n23, n31, n32, n33 = other.mlist
return mat3(m11 * n11 + m12 * n21 + m13 * n31,
m11 * n12 + m12 * n22 + m13 * n32,
m11 * n13 + m12 * n23 + m13 * n33,
m21 * n11 + m22 * n21 + m23 * n31,
m21 * n12 + m22 * n22 + m23 * n32,
m21 * n13 + m22 * n23 + m23 * n33,
m31 * n11 + m32 * n21 + m33 * n31,
m31 * n12 + m32 * n22 + m33 * n32,
m31 * n13 + m32 * n23 + m33 * n33)
# unsupported
else:
raise TypeError, "unsupported operand type for *"
def __rmul__(self, other):
T = type(other)
# scalar*mat4
if T==types.FloatType or T==types.IntType or T==types.LongType:
return mat4(map(lambda x,other=other: other*x, self.mlist))
# vec4*mat4
if isinstance(other, _vec4):
m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34,m41,m42,m43,m44 = self.mlist
return _vec4(other.x*m11 + other.y*m21 + other.z*m31 + other.w*m41,
other.x*m12 + other.y*m22 + other.z*m32 + other.w*m42,
other.x*m13 + other.y*m23 + other.z*m33 + other.w*m43,
other.x*m14 + other.y*m24 + other.z*m34 + other.w*m44)
# vec3*mat4
if isinstance(other, _vec3):
m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34,m41,m42,m43,m44 = self.mlist
w = float(other.x*m14 + other.y*m24 + other.z*m34 + m44)
return _vec3(other.x*m11 + other.y*m21 + other.z*m31 + m41,
other.x*m12 + other.y*m22 + other.z*m32 + m42,
other.x*m13 + other.y*m23 + other.z*m33 + m43)/w
# mat4*mat4
if isinstance(other, mat4):
return self.__mul__(other)
# unsupported
else:
raise TypeError("unsupported operand type for *")
def __rmul__(self, other):
T = type(other)
# scalar*mat4
if T == types.FloatType or T == types.IntType or T == types.LongType:
return mat4(map(lambda x, other=other: other * x, self.mlist))
# vec4*mat4
if isinstance(other, _vec4):
m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44 = self.mlist
return _vec4(
other.x * m11 + other.y * m21 + other.z * m31 + other.w * m41,
other.x * m12 + other.y * m22 + other.z * m32 + other.w * m42,
other.x * m13 + other.y * m23 + other.z * m33 + other.w * m43,
other.x * m14 + other.y * m24 + other.z * m34 + other.w * m44)
# vec3*mat4
if isinstance(other, _vec3):
m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44 = self.mlist
w = float(other.x * m14 + other.y * m24 + other.z * m34 + m44)
return _vec3(
other.x * m11 + other.y * m21 + other.z * m31 + m41,
other.x * m12 + other.y * m22 + other.z * m32 + m42,
other.x * m13 + other.y * m23 + other.z * m33 + m43) / w
# mat4*mat4
if isinstance(other, mat4):
return self.__mul__(other)
# unsupported
else:
raise TypeError, "unsupported operand type for *"
def rotation(angle, axis):
"""Return rotation matrix.
angle must be given in radians. axis should be of type vec3.
"""
axis = _vec3(axis)
sqr_a = axis.x*axis.x
sqr_b = axis.y*axis.y
sqr_c = axis.z*axis.z
len2 = sqr_a+sqr_b+sqr_c
k2 = math.cos(angle)
k1 = (1.0-k2)/len2
k3 = math.sin(angle)/math.sqrt(len2)
k1ab = k1*axis.x*axis.y
k1ac = k1*axis.x*axis.z
k1bc = k1*axis.y*axis.z
k3a = k3*axis.x
k3b = k3*axis.y
k3c = k3*axis.z
return mat4( k1*sqr_a+k2, k1ab-k3c, k1ac+k3b, 0.0,
k1ab+k3c, k1*sqr_b+k2, k1bc-k3a, 0.0,
k1ac-k3b, k1bc+k3a, k1*sqr_c+k2, 0.0,
0.0, 0.0, 0.0, 1.0)
def __mul__(self, other):
T = type(other)
# mat3*scalar
if T==types.FloatType or T==types.IntType or T==types.LongType:
return mat3(map(lambda x,other=other: x*other, self.mlist))
# mat3*vec3
if isinstance(other, _vec3):
m11,m12,m13,m21,m22,m23,m31,m32,m33 = self.mlist
return _vec3(m11*other.x + m12*other.y + m13*other.z,
m21*other.x + m22*other.y + m23*other.z,
m31*other.x + m32*other.y + m33*other.z)
# mat3*mat3
if isinstance(other, mat3):
m11,m12,m13,m21,m22,m23,m31,m32,m33 = self.mlist
n11,n12,n13,n21,n22,n23,n31,n32,n33 = other.mlist
return mat3( m11*n11+m12*n21+m13*n31,
m11*n12+m12*n22+m13*n32,
m11*n13+m12*n23+m13*n33,
m21*n11+m22*n21+m23*n31,
m21*n12+m22*n22+m23*n32,
m21*n13+m22*n23+m23*n33,
m31*n11+m32*n21+m33*n31,
m31*n12+m32*n22+m33*n32,
m31*n13+m32*n23+m33*n33)
# unsupported
else:
raise TypeError, "unsupported operand type for *"
def ortho(self):
"""Return a matrix with orthogonal base vectors.
"""
m11, m12, m13, m21, m22, m23, m31, m32, m33 = self.mlist
x = _vec3(m11, m21, m31)
y = _vec3(m12, m22, m32)
z = _vec3(m13, m23, m33)
xl = x.length()
xl *= xl
y = y - ((x * y) / xl) * x
z = z - ((x * z) / xl) * x
yl = y.length()
yl *= yl
z = z - ((y * z) / yl) * y
return mat3(x.x, y.x, z.x, x.y, y.y, z.y, x.z, y.z, z.z)
def ortho(self):
"""Return a matrix with orthogonal base vectors.
"""
m11,m12,m13,m21,m22,m23,m31,m32,m33 = self.mlist
x = _vec3(m11, m21, m31)
y = _vec3(m12, m22, m32)
z = _vec3(m13, m23, m33)
xl = x.length()
xl*=xl
y = y - ((x*y)/xl)*x
z = z - ((x*z)/xl)*x
yl = y.length()
yl*=yl
z = z - ((y*z)/yl)*y
return mat3( x.x, y.x, z.x,
x.y, y.y, z.y,
x.z, y.z, z.z)
def toAngleAxis(self):
"""Return angle (in radians) and rotation axis.
>>> q=quat(0.9, 0.5, 0.2, 0.3)
>>> angle, axis = q.toAngleAxis()
>>> print round(angle,4)
1.2011
>>> print axis
(0.8111, 0.3244, 0.4867)
"""
nself = self.normalize()
# Clamp nself.w (since the quat has to be normalized it should
# be between -1 and 1 anyway, but it might be slightly off due
# to numerical inaccuracies)
w = max(min(nself.w,1.0),-1.0)
w = math.acos(w)
s = math.sin(w)
if s<1E-12:
return (0.0, _vec3(0.0,0.0,0.0))
return (2.0*w, _vec3(nself.x/s, nself.y/s, nself.z/s))
def toAngleAxis(self):
"""Return angle (in radians) and rotation axis.
>>> q=quat(0.9, 0.5, 0.2, 0.3)
>>> angle, axis = q.toAngleAxis()
>>> print round(angle,4)
1.2011
>>> print axis
(0.8111, 0.3244, 0.4867)
"""
nself = self.normalize()
# Clamp nself.w (since the quat has to be normalized it should
# be between -1 and 1 anyway, but it might be slightly off due
# to numerical inaccuracies)
w = max(min(nself.w, 1.0), -1.0)
w = math.acos(w)
s = math.sin(w)
if s < 1E-12:
return (0.0, _vec3(0.0, 0.0, 0.0))
return (2.0 * w, _vec3(nself.x / s, nself.y / s, nself.z / s))
def ortho(self):
"""Return a matrix with orthogonal base vectors.
Makes the x-, y- and z-axis orthogonal.
The fourth column and row remain untouched.
"""
m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44 = self.mlist
x = _vec3(m11, m21, m31)
y = _vec3(m12, m22, m32)
z = _vec3(m13, m23, m33)
xl = x.length()
xl *= xl
y = y - ((x * y) / xl) * x
z = z - ((x * z) / xl) * x
yl = y.length()
yl *= yl
z = z - ((y * z) / yl) * y
return mat4(x.x, y.x, z.x, m14, x.y, y.y, z.y, m24, x.z, y.z, z.z, m34,
m41, m42, m43, m44)
def __rmul__(self, other):
T = type(other)
# scalar*mat3
if T==types.FloatType or T==types.IntType or T==types.LongType:
return mat3(map(lambda x,other=other: other*x, self.mlist))
# vec3*mat3
if isinstance(other, _vec3):
m11,m12,m13,m21,m22,m23,m31,m32,m33 = self.mlist
return _vec3(other.x*m11 + other.y*m21 + other.z*m31,
other.x*m12 + other.y*m22 + other.z*m32,
other.x*m13 + other.y*m23 + other.z*m33)
# mat3*mat3
if isinstance(other, mat3):
return self.__mul__(other)
# unsupported
else:
raise TypeError, "unsupported operand type for *"
def __rmul__(self, other):
T = type(other)
# scalar*mat3
if T == types.FloatType or T == types.IntType or T == types.LongType:
return mat3(map(lambda x, other=other: other * x, self.mlist))
# vec3*mat3
if isinstance(other, _vec3):
m11, m12, m13, m21, m22, m23, m31, m32, m33 = self.mlist
return _vec3(other.x * m11 + other.y * m21 + other.z * m31,
other.x * m12 + other.y * m22 + other.z * m32,
other.x * m13 + other.y * m23 + other.z * m33)
# mat3*mat3
if isinstance(other, mat3):
return self.__mul__(other)
# unsupported
else:
raise TypeError, "unsupported operand type for *"
def rotation(angle, axis):
"""Return a rotation matrix."""
axis = _vec3(axis)
sqr_a = axis.x*axis.x
sqr_b = axis.y*axis.y
sqr_c = axis.z*axis.z
len2 = sqr_a+sqr_b+sqr_c
k2 = math.cos(angle)
k1 = (1.0-k2)/len2
k3 = math.sin(angle)/math.sqrt(len2)
k1ab = k1*axis.x*axis.y
k1ac = k1*axis.x*axis.z
k1bc = k1*axis.y*axis.z
k3a = k3*axis.x
k3b = k3*axis.y
k3c = k3*axis.z
return mat3( k1*sqr_a+k2, k1ab-k3c, k1ac+k3b,
k1ab+k3c, k1*sqr_b+k2, k1bc-k3a,
k1ac-k3b, k1bc+k3a, k1*sqr_c+k2)
def fromAngleAxis(self, angle, axis):
"""Initialize self from an angle (in radians) and an axis and returns self."""
if axis==_vec3(0):
self.w = 1.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
else:
angle/=2.0
self.w = math.cos(angle)
x, y, z = axis
s = math.sin(angle)/math.sqrt(x*x+y*y+z*z)
self.x = x*s
self.y = y*s
self.z = z*s
dummy = self.normalize()
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
return self
def fromAngleAxis(self, angle, axis):
"""Initialize self from an angle (in radians) and an axis and returns self."""
if axis == _vec3(0):
self.w = 1.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
else:
angle /= 2.0
self.w = math.cos(angle)
x, y, z = axis
s = math.sin(angle) / math.sqrt(x * x + y * y + z * z)
self.x = x * s
self.y = y * s
self.z = z * s
dummy = self.normalize()
self.w = dummy.w
self.x = dummy.x
self.y = dummy.y
self.z = dummy.z
return self
def rotation(angle, axis):
"""Return a rotation matrix."""
axis = _vec3(axis)
sqr_a = axis.x*axis.x
sqr_b = axis.y*axis.y
sqr_c = axis.z*axis.z
len2 = sqr_a+sqr_b+sqr_c
k2 = math.cos(angle)
k1 = (1.0-k2)/len2
k3 = math.sin(angle)/math.sqrt(len2)
k1ab = k1*axis.x*axis.y
k1ac = k1*axis.x*axis.z
k1bc = k1*axis.y*axis.z
k3a = k3*axis.x
k3b = k3*axis.y
k3c = k3*axis.z
return mat3( k1*sqr_a+k2, k1ab-k3c, k1ac+k3b,
k1ab+k3c, k1*sqr_b+k2, k1bc-k3a,
k1ac-k3b, k1bc+k3a, k1*sqr_c+k2)
def _fromMat(self, m):
"""Initialize self from either a mat3 or mat4 and returns self."""
global _epsilon
# Jonte: start out by fetching the rotation matrix' rotation vector.
angle = 0
cosa = (m[0, 0] + m[1, 1] + m[2, 2] - 1.0) * 0.5
try:
angle = math.acos(cosa)
except ValueError as e:
#print("Got an matrix-to-quaternion error:", e)
#print(m)
raise
#print("Angle is", angle)
v = _vec3(m[2, 1] - m[1, 2], m[0, 2] - m[2, 0], m[1, 0] - m[0, 1])
#print("Vector is", v)
if v.length() < _epsilon:
lEpsilonOne = 1.0 - _epsilon
if m[0, 0] >= lEpsilonOne:
v.x = 1.0
v.y = 0.0
v.z = 0.0
elif m[1, 1] >= lEpsilonOne:
v.x = 0.0
v.y = 1.0
v.z = 0.0
elif m[2, 2] >= lEpsilonOne:
v.x = 0.0
v.y = 0.0
v.z = 1.0
else:
raise Exception("Uh-uh! Bad matrix!")
# Now set the vector.
self.fromAngleAxis(angle, v)
## d1,d2,d3 = m[0,0],m[1,1],m[2,2]
## t = d1+d2+d3+1.0
## if t>_epsilon:
## #print("Probable OK1!")
## s = 0.5/math.sqrt(t)
## self.w = 0.25/s
## self.x = (m[2,1]-m[1,2])*s
## self.y = (m[0,2]-m[2,0])*s
## self.z = (m[1,0]-m[0,1])*s
## else:
## ad1 = d1
## ad2 = d2
## ad3 = d3
## if ad1>=ad2 and ad1>=ad3:
## print("Probable OK2!")
## s = math.sqrt(1.0+d1-d2-d3)*2.0
## self.x = 0.5/s
## self.y = (m[0,1]+m[1,0])/s
## self.z = (m[0,2]+m[2,0])/s
## self.w = (m[1,2]+m[2,1])/s
## elif ad2>=ad1 and ad2>=ad3:
## s = math.sqrt(1.0+d2-d1-d3)*2.0
## print("Probable failure!!! s is", s)
## self.x = (m[0,1]+m[1,0])/s
## self.y = 0.5/s
## self.z = (m[1,2]+m[2,1])/s
## self.w = (m[0,2]+m[2,0])/s
## else:
## print("Probable OK3!")
## s = math.sqrt(1.0+d3-d1-d2)*2.0
## self.x = (m[0,2]+m[2,0])/s
## self.y = (m[1,2]+m[2,1])/s
## self.z = 0.5/s
## self.w = (m[0,1]+m[1,0])/s
return self
def __mul__(self, other):
"""Multiplication.
>>> M=mat4(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16)
>>> print M*2.0
[ 2.0000, 4.0000, 6.0000, 8.0000]
[ 10.0000, 12.0000, 14.0000, 16.0000]
[ 18.0000, 20.0000, 22.0000, 24.0000]
[ 26.0000, 28.0000, 30.0000, 32.0000]
>>> print 2.0*M
[ 2.0000, 4.0000, 6.0000, 8.0000]
[ 10.0000, 12.0000, 14.0000, 16.0000]
[ 18.0000, 20.0000, 22.0000, 24.0000]
[ 26.0000, 28.0000, 30.0000, 32.0000]
>>> print M*M
[ 90.0000, 100.0000, 110.0000, 120.0000]
[ 202.0000, 228.0000, 254.0000, 280.0000]
[ 314.0000, 356.0000, 398.0000, 440.0000]
[ 426.0000, 484.0000, 542.0000, 600.0000]
>>> print M*_vec3(1,2,3)
(0.1765, 0.4510, 0.7255)
>>> print _vec3(1,2,3)*M
(0.7083, 0.8056, 0.9028)
"""
T = type(other)
# mat4*scalar
if T == types.FloatType or T == types.IntType or T == types.LongType:
return mat4(map(lambda x, other=other: x * other, self.mlist))
# mat4*vec3
if isinstance(other, _vec3):
m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44 = self.mlist
w = float(m41 * other.x + m42 * other.y + m43 * other.z + m44)
return _vec3(
m11 * other.x + m12 * other.y + m13 * other.z + m14,
m21 * other.x + m22 * other.y + m23 * other.z + m24,
m31 * other.x + m32 * other.y + m33 * other.z + m34) / w
# mat4*vec4
if isinstance(other, _vec4):
m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44 = self.mlist
return _vec4(
m11 * other.x + m12 * other.y + m13 * other.z + m14 * other.w,
m21 * other.x + m22 * other.y + m23 * other.z + m24 * other.w,
m31 * other.x + m32 * other.y + m33 * other.z + m34 * other.w,
m41 * other.x + m42 * other.y + m43 * other.z + m44 * other.w)
# mat4*mat4
if isinstance(other, mat4):
m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44 = self.mlist
n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44 = other.mlist
return mat4(m11 * n11 + m12 * n21 + m13 * n31 + m14 * n41,
m11 * n12 + m12 * n22 + m13 * n32 + m14 * n42,
m11 * n13 + m12 * n23 + m13 * n33 + m14 * n43,
m11 * n14 + m12 * n24 + m13 * n34 + m14 * n44,
m21 * n11 + m22 * n21 + m23 * n31 + m24 * n41,
m21 * n12 + m22 * n22 + m23 * n32 + m24 * n42,
m21 * n13 + m22 * n23 + m23 * n33 + m24 * n43,
m21 * n14 + m22 * n24 + m23 * n34 + m24 * n44,
m31 * n11 + m32 * n21 + m33 * n31 + m34 * n41,
m31 * n12 + m32 * n22 + m33 * n32 + m34 * n42,
m31 * n13 + m32 * n23 + m33 * n33 + m34 * n43,
m31 * n14 + m32 * n24 + m33 * n34 + m34 * n44,
m41 * n11 + m42 * n21 + m43 * n31 + m44 * n41,
m41 * n12 + m42 * n22 + m43 * n32 + m44 * n42,
m41 * n13 + m42 * n23 + m43 * n33 + m44 * n43,
m41 * n14 + m42 * n24 + m43 * n34 + m44 * n44)
# unsupported
else:
raise TypeError, "unsupported operand type for *"
def getDiag(self):
"""Return the diagonal."""
return _vec3(self.mlist[0], self.mlist[4], self.mlist[8])
def getDiag(self):
"""Return the diagonal."""
return _vec3(self.mlist[0], self.mlist[4], self.mlist[8])
def fromToRotation(_from, to):
"""Returns a rotation matrix that rotates one vector into another.
The generated rotation matrix will rotate the vector _from into
the vector to. _from and to must be unit vectors!
This method is based on the code from:
Tomas Möller, John Hughes
Efficiently Building a Matrix to Rotate One Vector to Another
Journal of Graphics Tools, 4(4):1-4, 1999
http://www.acm.org/jgt/papers/MollerHughes99/
"""
_from = _vec3(_from)
to = _vec3(to)
EPSILON = 0.000001
e = _from*to
f = abs(e)
if (f>1.0-EPSILON): # "from" and "to"-vector almost parallel
# vector most nearly orthogonal to "from"
fx = abs(_from.x)
fy = abs(_from.y)
fz = abs(_from.z)
if (fx<fy):
if (fx<fz):
x = _vec3(1.0, 0.0, 0.0)
else:
x = _vec3(0.0, 0.0, 1.0)
else:
if (fy<fz):
x = _vec3(0.0, 1.0, 0.0)
else:
x = _vec3(0.0, 0.0, 1.0)
u = x-_from
v = x-to
c1 = 2.0/(u*u)
c2 = 2.0/(v*v)
c3 = c1*c2*u*v
res = mat3()
for i in range(3):
for j in range(3):
res[i,j] = - c1*u[i]*u[j] - c2*v[i]*v[j] + c3*v[i]*u[j]
res[i,i] += 1.0
return res
else: # the most common case, unless "from"="to", or "from"=-"to"
v = _from.cross(to)
h = 1.0/(1.0 + e) # optimization by Gottfried Chen
hvx = h*v.x
hvz = h*v.z
hvxy = hvx*v.y
hvxz = hvx*v.z
hvyz = hvz*v.y
m11 = e + hvx*v.x
m12 = hvxy - v.z
m13 = hvxz + v.y
m21 = hvxy + v.z
m22 = e + h*v.y*v.y
m23 = hvyz - v.x
m31 = hvxz - v.y
m32 = hvyz + v.x
m33 = e + hvz*v.z
return mat3(m11,m12,m13,m21,m22,m23,m31,m32,m33)
def fromToRotation(_from, to):
"""Returns a rotation matrix that rotates one vector into another.
The generated rotation matrix will rotate the vector _from into
the vector to. _from and to must be unit vectors!
This method is based on the code from:
Tomas Möller, John Hughes
Efficiently Building a Matrix to Rotate One Vector to Another
Journal of Graphics Tools, 4(4):1-4, 1999
http://www.acm.org/jgt/papers/MollerHughes99/
"""
_from = _vec3(_from)
to = _vec3(to)
EPSILON = 0.000001
e = _from*to
f = abs(e)
if (f>1.0-EPSILON): # "from" and "to"-vector almost parallel
# vector most nearly orthogonal to "from"
fx = abs(_from.x)
fy = abs(_from.y)
fz = abs(_from.z)
if (fx<fy):
if (fx<fz):
x = _vec3(1.0, 0.0, 0.0)
else:
x = _vec3(0.0, 0.0, 1.0)
else:
if (fy<fz):
x = _vec3(0.0, 1.0, 0.0)
else:
x = _vec3(0.0, 0.0, 1.0)
u = x-_from
v = x-to
c1 = 2.0/(u*u)
c2 = 2.0/(v*v)
c3 = c1*c2*u*v
res = mat3()
for i in range(3):
for j in range(3):
res[i,j] = - c1*u[i]*u[j] - c2*v[i]*v[j] + c3*v[i]*u[j]
res[i,i] += 1.0
return res
else: # the most common case, unless "from"="to", or "from"=-"to"
v = _from.cross(to)
h = 1.0/(1.0 + e) # optimization by Gottfried Chen
hvx = h*v.x
hvz = h*v.z
hvxy = hvx*v.y
hvxz = hvx*v.z
hvyz = hvz*v.y
m11 = e + hvx*v.x
m12 = hvxy - v.z
m13 = hvxz + v.y
m21 = hvxy + v.z
m22 = e + h*v.y*v.y
m23 = hvyz - v.x
m31 = hvxz - v.y
m32 = hvyz + v.x
m33 = e + hvz*v.z
return mat3(m11,m12,m13,m21,m22,m23,m31,m32,m33)
def _fromMat(self, m):
"""Initialize self from either a mat3 or mat4 and returns self."""
global _epsilon
# Jonte: start out by fetching the rotation matrix' rotation vector.
angle = 0
cosa = (m[0,0] + m[1,1] + m[2,2] - 1.0) * 0.5
try:
angle = math.acos(cosa)
except ValueError as e:
#print("Got an matrix-to-quaternion error:", e)
#print(m)
raise
#print("Angle is", angle)
v = _vec3(m[2,1] - m[1,2],
m[0,2] - m[2,0],
m[1,0] - m[0,1])
#print("Vector is", v)
if v.length() < _epsilon:
lEpsilonOne = 1.0 - _epsilon
if m[0,0] >= lEpsilonOne:
v.x = 1.0
v.y = 0.0
v.z = 0.0
elif m[1,1] >= lEpsilonOne:
v.x = 0.0
v.y = 1.0
v.z = 0.0
elif m[2,2] >= lEpsilonOne:
v.x = 0.0
v.y = 0.0
v.z = 1.0
else:
raise Exception("Uh-uh! Bad matrix!")
# Now set the vector.
self.fromAngleAxis(angle, v)
## d1,d2,d3 = m[0,0],m[1,1],m[2,2]
## t = d1+d2+d3+1.0
## if t>_epsilon:
## #print("Probable OK1!")
## s = 0.5/math.sqrt(t)
## self.w = 0.25/s
## self.x = (m[2,1]-m[1,2])*s
## self.y = (m[0,2]-m[2,0])*s
## self.z = (m[1,0]-m[0,1])*s
## else:
## ad1 = d1
## ad2 = d2
## ad3 = d3
## if ad1>=ad2 and ad1>=ad3:
## print("Probable OK2!")
## s = math.sqrt(1.0+d1-d2-d3)*2.0
## self.x = 0.5/s
## self.y = (m[0,1]+m[1,0])/s
## self.z = (m[0,2]+m[2,0])/s
## self.w = (m[1,2]+m[2,1])/s
## elif ad2>=ad1 and ad2>=ad3:
## s = math.sqrt(1.0+d2-d1-d3)*2.0
## print("Probable failure!!! s is", s)
## self.x = (m[0,1]+m[1,0])/s
## self.y = 0.5/s
## self.z = (m[1,2]+m[2,1])/s
## self.w = (m[0,2]+m[2,0])/s
## else:
## print("Probable OK3!")
## s = math.sqrt(1.0+d3-d1-d2)*2.0
## self.x = (m[0,2]+m[2,0])/s
## self.y = (m[1,2]+m[2,1])/s
## self.z = 0.5/s
## self.w = (m[0,1]+m[1,0])/s
return self
def __mul__(self, other):
"""Multiplication.
>>> M=mat4(1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16)
>>> print M*2.0
[ 2.0000, 4.0000, 6.0000, 8.0000]
[ 10.0000, 12.0000, 14.0000, 16.0000]
[ 18.0000, 20.0000, 22.0000, 24.0000]
[ 26.0000, 28.0000, 30.0000, 32.0000]
>>> print 2.0*M
[ 2.0000, 4.0000, 6.0000, 8.0000]
[ 10.0000, 12.0000, 14.0000, 16.0000]
[ 18.0000, 20.0000, 22.0000, 24.0000]
[ 26.0000, 28.0000, 30.0000, 32.0000]
>>> print M*M
[ 90.0000, 100.0000, 110.0000, 120.0000]
[ 202.0000, 228.0000, 254.0000, 280.0000]
[ 314.0000, 356.0000, 398.0000, 440.0000]
[ 426.0000, 484.0000, 542.0000, 600.0000]
>>> print M*_vec3(1,2,3)
(0.1765, 0.4510, 0.7255)
>>> print _vec3(1,2,3)*M
(0.7083, 0.8056, 0.9028)
"""
T = type(other)
# mat4*scalar
if T==types.FloatType or T==types.IntType or T==types.LongType:
return mat4(map(lambda x,other=other: x*other, self.mlist))
# mat4*vec3
if isinstance(other, _vec3):
m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34,m41,m42,m43,m44 = self.mlist
w = float(m41*other.x + m42*other.y + m43*other.z + m44)
return _vec3(m11*other.x + m12*other.y + m13*other.z + m14,
m21*other.x + m22*other.y + m23*other.z + m24,
m31*other.x + m32*other.y + m33*other.z + m34)/w
# mat4*vec4
if isinstance(other, _vec4):
m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34,m41,m42,m43,m44 = self.mlist
return _vec4(m11*other.x + m12*other.y + m13*other.z + m14*other.w,
m21*other.x + m22*other.y + m23*other.z + m24*other.w,
m31*other.x + m32*other.y + m33*other.z + m34*other.w,
m41*other.x + m42*other.y + m43*other.z + m44*other.w)
# mat4*mat4
if isinstance(other, mat4):
m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34,m41,m42,m43,m44 = self.mlist
n11,n12,n13,n14,n21,n22,n23,n24,n31,n32,n33,n34,n41,n42,n43,n44 = other.mlist
return mat4( m11*n11+m12*n21+m13*n31+m14*n41,
m11*n12+m12*n22+m13*n32+m14*n42,
m11*n13+m12*n23+m13*n33+m14*n43,
m11*n14+m12*n24+m13*n34+m14*n44,
m21*n11+m22*n21+m23*n31+m24*n41,
m21*n12+m22*n22+m23*n32+m24*n42,
m21*n13+m22*n23+m23*n33+m24*n43,
m21*n14+m22*n24+m23*n34+m24*n44,
m31*n11+m32*n21+m33*n31+m34*n41,
m31*n12+m32*n22+m33*n32+m34*n42,
m31*n13+m32*n23+m33*n33+m34*n43,
m31*n14+m32*n24+m33*n34+m34*n44,
m41*n11+m42*n21+m43*n31+m44*n41,
m41*n12+m42*n22+m43*n32+m44*n42,
m41*n13+m42*n23+m43*n33+m44*n43,
m41*n14+m42*n24+m43*n34+m44*n44)
# unsupported
else:
raise TypeError("unsupported operand type for *")