def quaternionSlerp(q1, q2, blend):
"""
Get an interpolate quaternion based in slerp function.
Args:
q1 (quaternion): Input quaternion 1.
q2 (quaternion): Input quaternion 2.
blend (float): Blending value.
Returns:
quaternion: The interpolated quaternion.
Example:
.. code-block:: python
q = quaternionSlerp(dt.Quaternion(t1.getRotationQuaternion()),
dt.Quaternion(t2.getRotationQuaternion()), blend)
"""
dot = quaternionDotProd(q1, q2)
if dot < 0.0:
dot = quaternionDotProd(q1, q2.negateIt())
arcos = math.acos(round(dot, 10))
sin = math.sin(arcos)
if sin > 0.001:
w1 = math.sin((1.0 - blend) * arcos) / sin
w2 = math.sin(blend * arcos) / sin
else:
w1 = 1.0 - blend
w2 = blend
result = dt.Quaternion(q1).scaleIt(w1) + dt.Quaternion(q2).scaleIt(w2)
return result
def getInterpolateTransformMatrix(t1, t2, blend=.5):
"""Interpolate 2 matrix.
Arguments:
t1 (matrix): Input matrix 1.
t2 (matrix): Input matrix 2.
blend (float): The blending value. Default 0.5
Returns:
matrix: The newly interpolated transformation matrix.
>>> t = tra.getInterpolateTransformMatrix(self.fk_ctl[0],
self.tws1A_npo,
.3333)
"""
# check if the input transforms are transformMatrix
t1 = convert2TransformMatrix(t1)
t2 = convert2TransformMatrix(t2)
if (blend == 1.0):
return t2
elif (blend == 0.0):
return t1
# translate
pos = vector.linearlyInterpolate(t1.getTranslation(space="world"),
t2.getTranslation(space="world"),
blend)
# scale
scaleA = datatypes.Vector(*t1.getScale(space="world"))
scaleB = datatypes.Vector(*t2.getScale(space="world"))
vs = vector.linearlyInterpolate(scaleA, scaleB, blend)
# rotate
q = quaternionSlerp(datatypes.Quaternion(t1.getRotationQuaternion()),
datatypes.Quaternion(t2.getRotationQuaternion()),
blend)
# out
result = datatypes.TransformationMatrix()
result.setTranslation(pos, space="world")
result.setRotationQuaternion(q.x, q.y, q.z, q.w)
result.setScale([vs.x, vs.y, vs.z], space="world")
return result
def MTransform(item, target, attrs):
Ipwm = item.getAttr('worldMatrix')
if target.getParent():
Tpim = target.getParent().getAttr('worldMatrix').inverse()
else:
Tpim = target.getAttr('parentInverseMatrix')
prod = Ipwm * Tpim
tMat = dt.TransformationMatrix(prod)
translate = tMat.getTranslation('transform')
quat_rotate = tMat.getRotationQuaternion()
quat = dt.Quaternion(quat_rotate)
euler = quat.asEulerRotation()
rotate = map(math.degrees, euler)
scale = tMat.getScale('transform')
data = {}
if 't' in attrs:
data["translate"] = translate
if 'r' in attrs:
data["rotate"] = rotate
if 'jo' in attrs:
data["jointOrient"] = rotate
if 's' in attrs:
data["scale"] = scale
for DItem in data.items():
setKwargs(target, DItem[0], DItem[1])
def slerp(v0, v1, t):
qm = dt.Quaternion()
cosHalfTheta = (v0.w * v1.w) + (v0.x * v1.x) + (v0.y * v1.y) + (v0.z *
v1.z)
if abs(cosHalfTheta) >= 1.0:
qm.w = v0.w
qm.x = v0.x
qm.y = v0.y
qm.z = v0.z
return qm
halfTheta = dt.acos(cosHalfTheta)
sinHalfTheta = dt.sqrt(1.0 - cosHalfTheta * cosHalfTheta)
if dt.fabs(sinHalfTheta) < 0.001:
qm.w = (v0.w * 0.5 + v1.w * 0.5)
qm.x = (v0.x * 0.5 + v1.x * 0.5)
qm.y = (v0.y * 0.5 + v1.y * 0.5)
qm.z = (v0.z * 0.5 + v1.z * 0.5)
return qm
ratioA = dt.sin((1 - t) * halfTheta) / sinHalfTheta
ratioB = dt.sin(t * halfTheta) / sinHalfTheta
qm.w = (v0.w * ratioA + v1.w * ratioB)
qm.x = (v0.x * ratioA + v1.x * ratioB)
qm.y = (v0.y * ratioA + v1.y * ratioB)
qm.z = (v0.z * ratioA + v1.z * ratioB)
return qm