forked from bartvdbraak/blender
Py API: Vector.slerp(). also added interp_v3_v3v3_slerp(_safe) functions
This commit is contained in:
Campbell Barton
parent
6aa75d3b2c
commit
55f83e36cc
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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/
|
||||
*/
|
||||
|
||||
@ -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},
|
||||
|
||||
Loading…
Reference in New Issue
Block a user