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