diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h index eb4579887f7..25c9b5e468f 100644 --- a/source/blender/blenlib/BLI_math_rotation.h +++ b/source/blender/blenlib/BLI_math_rotation.h @@ -69,6 +69,7 @@ float normalize_qt_qt(float q1[4], const float q2[4]); bool is_zero_qt(const float q[4]); /* interpolation */ +void interp_dot_slerp(const float t, const float cosom, float w[2]); void interp_qt_qtqt(float q[4], const float a[4], const float b[4], const float t); void add_qt_qtqt(float q[4], const float a[4], const float b[4], const float t); diff --git a/source/blender/blenlib/BLI_math_vector.h b/source/blender/blenlib/BLI_math_vector.h index 9ef2dc781de..1f4ccf880fb 100644 --- a/source/blender/blenlib/BLI_math_vector.h +++ b/source/blender/blenlib/BLI_math_vector.h @@ -190,6 +190,12 @@ void interp_v4_v4v4v4(float p[4], const float v1[4], const float v2[4], const fl void interp_v4_v4v4v4v4(float p[4], const float v1[4], const float v2[4], const float v3[4], const float v4[4], const float w[4]); void interp_v3_v3v3v3_uv(float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2]); +bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t) ATTR_WARN_UNUSED_RESULT; +bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], const float t) ATTR_WARN_UNUSED_RESULT; + +void interp_v3_v3v3_slerp_safe(float target[3], const float a[3], const float b[3], const float t); +void interp_v2_v2v2_slerp_safe(float target[2], const float a[2], const float b[2], const float t); + void interp_v3_v3v3_char(char target[3], const char a[3], const char b[3], const float t); void interp_v3_v3v3_uchar(unsigned char target[3], const unsigned char a[3], const unsigned char b[3], const float t); void interp_v4_v4v4_char(char target[4], const char a[4], const char b[4], const float t); @@ -255,6 +261,7 @@ void project_v3_plane(float v[3], const float n[3], const float p[3]); void reflect_v3_v3v3(float r[3], const float v[3], const float n[3]); void ortho_basis_v3v3_v3(float r1[3], float r2[3], const float a[3]); void ortho_v3_v3(float p[3], const float v[3]); +void ortho_v2_v2(float p[3], const float v[3]); void bisect_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3]); void rotate_v3_v3v3fl(float v[3], const float p[3], const float axis[3], const float angle); void rotate_normalized_v3_v3v3fl(float v[3], const float p[3], const float axis[3], const float angle); diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index 1136020a4f7..64016de1718 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.c @@ -623,9 +623,42 @@ void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t) } #endif +/** + * Generic function for implementing slerp + * (quaternions and spherical vector coords). + * + * \param t: factor in [0..1] + * \param cosom: dot product from normalized vectors/quats. + * \param r_w: calculated weights. + */ +void interp_dot_slerp(const float t, const float cosom, float r_w[2]) +{ + const float eps = 0.0001f; + + BLI_assert(IN_RANGE_INCL(cosom, -1.0f, 1.0f)); + + /* within [-1..1] range, avoid aligned axis */ + if (LIKELY(fabsf(cosom) < (1.0f - eps))) { + float omega, sinom; + + omega = acosf(cosom); + sinom = sinf(omega); + r_w[0] = sinf((1.0f - t) * omega) / sinom; + r_w[1] = sinf(t * omega) / sinom; + } + else { + /* fallback to lerp */ + r_w[0] = 1.0f - t; + r_w[1] = t; + } +} + void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t) { - float quat[4], omega, cosom, sinom, sc1, sc2; + float quat[4], cosom, w[2]; + + BLI_ASSERT_UNIT_QUAT(quat1); + BLI_ASSERT_UNIT_QUAT(quat2); cosom = dot_qtqt(quat1, quat2); @@ -638,21 +671,12 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], copy_qt_qt(quat, quat1); } - if ((1.0f - cosom) > 0.0001f) { - omega = acosf(cosom); - sinom = sinf(omega); - sc1 = sinf((1.0f - t) * omega) / sinom; - sc2 = sinf(t * omega) / sinom; - } - else { - sc1 = 1.0f - t; - sc2 = t; - } + interp_dot_slerp(t, cosom, w); - result[0] = sc1 * quat[0] + sc2 * quat2[0]; - result[1] = sc1 * quat[1] + sc2 * quat2[1]; - result[2] = sc1 * quat[2] + sc2 * quat2[2]; - result[3] = sc1 * quat[3] + sc2 * quat2[3]; + result[0] = w[0] * quat[0] + w[1] * quat2[0]; + result[1] = w[0] * quat[1] + w[1] * quat2[1]; + result[2] = w[0] * quat[2] + w[1] * quat2[2]; + result[3] = w[0] * quat[3] + w[1] * quat2[3]; } void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t) diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c index 3fbddacaea2..8455bf7550f 100644 --- a/source/blender/blenlib/intern/math_vector.c +++ b/source/blender/blenlib/intern/math_vector.c @@ -68,6 +68,103 @@ void interp_v4_v4v4(float target[4], const float a[4], const float b[4], const f target[3] = s * a[3] + t * b[3]; } +/** + * slerp, treat vectors as spherical coordinates + * \see #interp_qt_qtqt + * + * \return success + */ +bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t) +{ + float cosom, w[2]; + + BLI_ASSERT_UNIT_V3(a); + BLI_ASSERT_UNIT_V3(b); + + cosom = dot_v3v3(a, b); + + /* direct opposites */ + if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) { + return false; + } + + interp_dot_slerp(t, cosom, w); + + target[0] = w[0] * a[0] + w[1] * b[0]; + target[1] = w[0] * a[1] + w[1] * b[1]; + target[2] = w[0] * a[2] + w[1] * b[2]; + + return true; +} +bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], const float t) +{ + float cosom, w[2]; + + BLI_ASSERT_UNIT_V2(a); + BLI_ASSERT_UNIT_V2(b); + + cosom = dot_v2v2(a, b); + + /* direct opposites */ + if (UNLIKELY(cosom < (1.0f + FLT_EPSILON))) { + return false; + } + + interp_dot_slerp(t, cosom, w); + + target[0] = w[0] * a[0] + w[1] * b[0]; + target[1] = w[0] * a[1] + w[1] * b[1]; + + return true; +} + +/** + * Same as #interp_v3_v3v3_slerp buy uses fallback values + * for opposite vectors. + */ +void interp_v3_v3v3_slerp_safe(float target[3], const float a[3], const float b[3], const float t) +{ + if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, b, t))) { + /* axis are aligned so any otho vector is acceptable */ + float ab_ortho[3]; + ortho_v3_v3(ab_ortho, a); + normalize_v3(ab_ortho); + if (t < 0.5f) { + if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, ab_ortho, t * 2.0f))) { + BLI_assert(0); + copy_v3_v3(target, a); + } + } + else { + if (UNLIKELY(!interp_v3_v3v3_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) { + BLI_assert(0); + copy_v3_v3(target, b); + } + } + } +} +void interp_v2_v2v2_slerp_safe(float target[2], const float a[2], const float b[2], const float t) +{ + if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, b, t))) { + /* axis are aligned so any otho vector is acceptable */ + float ab_ortho[2]; + ortho_v2_v2(ab_ortho, a); + // normalize_v2(ab_ortho); + if (t < 0.5f) { + if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, ab_ortho, t * 2.0f))) { + BLI_assert(0); + copy_v2_v2(target, a); + } + } + else { + if (UNLIKELY(!interp_v2_v2v2_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) { + BLI_assert(0); + copy_v2_v2(target, b); + } + } + } +} + /* weight 3 vectors, * 'w' must be unit length but is not a vector, just 3 weights */ void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3]) @@ -538,6 +635,17 @@ void ortho_v3_v3(float p[3], const float v[3]) } } +/** + * no brainer compared to v3, just have for consistency. + */ +void ortho_v2_v2(float p[3], const float v[3]) +{ + BLI_assert(p != v); + + p[0] = -v[1]; + p[1] = v[0]; +} + /* Rotate a point p by angle theta around an arbitrary axis r * http://local.wasp.uwa.edu.au/~pbourke/geometry/ */ diff --git a/source/blender/python/mathutils/mathutils_Vector.c b/source/blender/python/mathutils/mathutils_Vector.c index 96e68317e2d..7f191369cd7 100644 --- a/source/blender/python/mathutils/mathutils_Vector.c +++ b/source/blender/python/mathutils/mathutils_Vector.c @@ -945,13 +945,13 @@ static PyObject *Vector_dot(VectorObject *self, PyObject *value) } PyDoc_STRVAR(Vector_angle_doc, -".. function:: angle(other, fallback)\n" +".. function:: angle(other, fallback=None)\n" "\n" " Return the angle between two vectors.\n" "\n" " :arg other: another vector to compare the angle with\n" " :type other: :class:`Vector`\n" -" :arg fallback: return this value when the angle cant be calculated\n" +" :arg fallback: return this value when the angle can't be calculated\n" " (zero length vector)\n" " :type fallback: any\n" " :return: angle in radians or fallback when given\n" @@ -1015,7 +1015,7 @@ PyDoc_STRVAR(Vector_angle_signed_doc, "\n" " :arg other: another vector to compare the angle with\n" " :type other: :class:`Vector`\n" -" :arg fallback: return this value when the angle cant be calculated\n" +" :arg fallback: return this value when the angle can't be calculated\n" " (zero length vector)\n" " :type fallback: any\n" " :return: angle in radians or fallback when given\n" @@ -1198,6 +1198,107 @@ static PyObject *Vector_lerp(VectorObject *self, PyObject *args) return Vector_CreatePyObject_alloc(vec, size, Py_TYPE(self)); } +PyDoc_STRVAR(Vector_slerp_doc, +".. function:: slerp(other, factor, fallback=None)\n" +"\n" +" Returns the interpolation of two unit vectors (spherical coordinates).\n" +"\n" +" :arg other: value to interpolate with.\n" +" :type other: :class:`Vector`\n" +" :arg factor: The interpolation value in [0.0, 1.0].\n" +" :type factor: float\n" +" :arg fallback: return this value when the vector can't be calculated\n" +" (zero length vector or direct opposites)\n" +" :type fallback: any\n" +" :return: The interpolated vector.\n" +" :rtype: :class:`Vector`\n" +); +static PyObject *Vector_slerp(VectorObject *self, PyObject *args) +{ + const int size = self->size; + PyObject *value = NULL; + float fac, cosom, w[2]; + float tvec[3], vec[3]; + double self_len_sq, other_len_sq; + int x; + PyObject *fallback = NULL; + + if (!PyArg_ParseTuple(args, "Of|O:slerp", &value, &fac, &fallback)) + return NULL; + + if (BaseMath_ReadCallback(self) == -1) { + return NULL; + } + + if (self->size > 3) { + PyErr_SetString(PyExc_ValueError, + "Vector must be 2D or 3D"); + return NULL; + } + + if (mathutils_array_parse(tvec, size, size, value, "Vector.slerp(other), invalid 'other' arg") == -1) { + return NULL; + } + + self_len_sq = len_squared_vn(self->vec, size); + other_len_sq = len_squared_vn(tvec, size); + + /* use fallbacks for zero length vectors */ + if (UNLIKELY((self_len_sq < (double)FLT_EPSILON) || + (other_len_sq < (double)FLT_EPSILON))) + { + /* avoid exception */ + if (fallback) { + Py_INCREF(fallback); + return fallback; + } + else { + PyErr_SetString(PyExc_ValueError, + "Vector.slerp(): " + "zero length vectors unsupported"); + return NULL; + } + } + + /* no attempt made to normalize, no fallback */ + if (UNLIKELY((fabs(self_len_sq - 1.0) > (double)FLT_EPSILON) || + (fabs(other_len_sq - 1.0) > (double)FLT_EPSILON))) + { + PyErr_SetString(PyExc_ValueError, + "Vector.slerp(): " + "both vectors must be unit length"); + return NULL; + } + + /* We have sane state, execute slerp */ + cosom = (float)dot_vn_vn(self->vec, tvec, size); + + /* direct opposite, can't slerp */ + if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) { + /* avoid exception */ + if (fallback) { + Py_INCREF(fallback); + return fallback; + } + else { + PyErr_SetString(PyExc_ValueError, + "Vector.slerp(): " + "opposite vectors unsupported"); + return NULL; + } + } + + interp_dot_slerp(fac, cosom, w); + + for (x = 0; x < size; x++) { + vec[x] = (w[0] * self->vec[x]) + (w[1] * tvec[x]); + } + + interp_v3_v3v3_slerp_safe(vec, self->vec, tvec, fac); + + return Vector_CreatePyObject(vec, size, Py_NEW, Py_TYPE(self)); +} + PyDoc_STRVAR(Vector_rotate_doc, ".. function:: rotate(other)\n" "\n" @@ -2798,6 +2899,7 @@ static struct PyMethodDef Vector_methods[] = { {"rotation_difference", (PyCFunction) Vector_rotation_difference, METH_O, Vector_rotation_difference_doc}, {"project", (PyCFunction) Vector_project, METH_O, Vector_project_doc}, {"lerp", (PyCFunction) Vector_lerp, METH_VARARGS, Vector_lerp_doc}, + {"slerp", (PyCFunction) Vector_slerp, METH_VARARGS, Vector_slerp_doc}, {"rotate", (PyCFunction) Vector_rotate, METH_O, Vector_rotate_doc}, {"copy", (PyCFunction) Vector_copy, METH_NOARGS, Vector_copy_doc},