Пример #1
0
    public static DualQuaternion sclerp(DualQuaternion from, DualQuaternion to, float t)
    {
        float dot = from.dot(to);

        if (dot < 0)
        {
            to = to * -1.0f;
        }
        var diff = from.conjugate() * to;

        var vr = diff.real.getVectorPart();
        var vd = diff.dual.getVectorPart();

        float invr = 1.0f / Mathf.Sqrt(Vector3.Dot(vr, vr));

        float angle = 2.0f * Mathf.Acos(diff.real.w);
        float pitch = -2.0f * diff.dual.w * invr;

        Vector3 dir    = vr * invr;
        Vector3 moment = (vd - dir * pitch * diff.real.w * 0.5f) * invr;

        angle *= t;
        pitch *= t;

        float sinAngle = Mathf.Sin(0.5f * angle);
        float cosAngle = Mathf.Cos(0.5f * angle);

        var result = new DualQuaternion();

        result.real = new Quaternion(dir.x * sinAngle, dir.y * sinAngle, dir.z * sinAngle, cosAngle);
        var rdv = (sinAngle * moment + pitch * 0.5f * cosAngle * dir);

        result.dual = new Quaternion(rdv.x, rdv.y, rdv.z, -pitch * 0.5f * sinAngle);

        return(result);
    }