diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_vector.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_vector.c | 1398 |
1 files changed, 741 insertions, 657 deletions
diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c index 0aacfd6cde1..1977558ac88 100644 --- a/source/blender/blenlib/intern/math_vector.c +++ b/source/blender/blenlib/intern/math_vector.c @@ -32,37 +32,38 @@ void interp_v2_v2v2(float target[2], const float a[2], const float b[2], const float t) { - const float s = 1.0f - t; + const float s = 1.0f - t; - target[0] = s * a[0] + t * b[0]; - target[1] = s * a[1] + t * b[1]; + target[0] = s * a[0] + t * b[0]; + target[1] = s * a[1] + t * b[1]; } /* weight 3 2D vectors, * 'w' must be unit length but is not a vector, just 3 weights */ -void interp_v2_v2v2v2(float p[2], const float v1[2], const float v2[2], const float v3[2], const float w[3]) +void interp_v2_v2v2v2( + float p[2], const float v1[2], const float v2[2], const float v3[2], const float w[3]) { - p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2]; - p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2]; + p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2]; + p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2]; } void interp_v3_v3v3(float target[3], const float a[3], const float b[3], const float t) { - const float s = 1.0f - t; + const float s = 1.0f - t; - target[0] = s * a[0] + t * b[0]; - target[1] = s * a[1] + t * b[1]; - target[2] = s * a[2] + t * b[2]; + target[0] = s * a[0] + t * b[0]; + target[1] = s * a[1] + t * b[1]; + target[2] = s * a[2] + t * b[2]; } void interp_v4_v4v4(float target[4], const float a[4], const float b[4], const float t) { - const float s = 1.0f - t; + const float s = 1.0f - t; - target[0] = s * a[0] + t * b[0]; - target[1] = s * a[1] + t * b[1]; - target[2] = s * a[2] + t * b[2]; - target[3] = s * a[3] + t * b[3]; + target[0] = s * a[0] + t * b[0]; + target[1] = s * a[1] + t * b[1]; + target[2] = s * a[2] + t * b[2]; + target[3] = s * a[3] + t * b[3]; } /** @@ -73,46 +74,46 @@ void interp_v4_v4v4(float target[4], const float a[4], const float b[4], const f */ bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t) { - float cosom, w[2]; + float cosom, w[2]; - BLI_ASSERT_UNIT_V3(a); - BLI_ASSERT_UNIT_V3(b); + BLI_ASSERT_UNIT_V3(a); + BLI_ASSERT_UNIT_V3(b); - cosom = dot_v3v3(a, b); + cosom = dot_v3v3(a, b); - /* direct opposites */ - if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) { - return false; - } + /* direct opposites */ + if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) { + return false; + } - interp_dot_slerp(t, cosom, w); + 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]; + 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; + 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]; + float cosom, w[2]; - BLI_ASSERT_UNIT_V2(a); - BLI_ASSERT_UNIT_V2(b); + BLI_ASSERT_UNIT_V2(a); + BLI_ASSERT_UNIT_V2(b); - cosom = dot_v2v2(a, b); + cosom = dot_v2v2(a, b); - /* direct opposites */ - if (UNLIKELY(cosom < (1.0f + FLT_EPSILON))) { - return false; - } + /* direct opposites */ + if (UNLIKELY(cosom < (1.0f + FLT_EPSILON))) { + return false; + } - interp_dot_slerp(t, cosom, w); + 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[0] = w[0] * a[0] + w[1] * b[0]; + target[1] = w[0] * a[1] + w[1] * b[1]; - return true; + return true; } /** @@ -120,171 +121,196 @@ bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], c */ 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); - } - } - } + 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); - } - } - } + 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); + } + } + } } /** \name Cubic curve interpolation (bezier spline). * \{ */ -void interp_v2_v2v2v2v2_cubic( - float p[2], const float v1[2], const float v2[2], const float v3[2], const float v4[2], - const float u) +void interp_v2_v2v2v2v2_cubic(float p[2], + const float v1[2], + const float v2[2], + const float v3[2], + const float v4[2], + const float u) { - float q0[2], q1[2], q2[2], r0[2], r1[2]; + float q0[2], q1[2], q2[2], r0[2], r1[2]; - interp_v2_v2v2(q0, v1, v2, u); - interp_v2_v2v2(q1, v2, v3, u); - interp_v2_v2v2(q2, v3, v4, u); + interp_v2_v2v2(q0, v1, v2, u); + interp_v2_v2v2(q1, v2, v3, u); + interp_v2_v2v2(q2, v3, v4, u); - interp_v2_v2v2(r0, q0, q1, u); - interp_v2_v2v2(r1, q1, q2, u); + interp_v2_v2v2(r0, q0, q1, u); + interp_v2_v2v2(r1, q1, q2, u); - interp_v2_v2v2(p, r0, r1, u); + interp_v2_v2v2(p, r0, r1, u); } /** \} */ /* 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]) +void interp_v3_v3v3v3( + float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3]) { - p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2]; - p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2]; - p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2]; + p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2]; + p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2]; + p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2]; } /* weight 3 vectors, * 'w' must be unit length but is not a vector, just 4 weights */ -void interp_v3_v3v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3], const float w[4]) +void interp_v3_v3v3v3v3(float p[3], + const float v1[3], + const float v2[3], + const float v3[3], + const float v4[3], + const float w[4]) { - p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3]; - p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3]; - p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3]; + p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3]; + p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3]; + p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3]; } -void interp_v4_v4v4v4(float p[4], const float v1[4], const float v2[4], const float v3[4], const float w[3]) +void interp_v4_v4v4v4( + float p[4], const float v1[4], const float v2[4], const float v3[4], const float w[3]) { - p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2]; - p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2]; - p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2]; - p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2]; + p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2]; + p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2]; + p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2]; + p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2]; } -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_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]) { - p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3]; - p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3]; - p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3]; - p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2] + v4[3] * w[3]; + p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3]; + p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3]; + p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3]; + p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2] + v4[3] * w[3]; } -void interp_v3_v3v3v3_uv(float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2]) +void interp_v3_v3v3v3_uv( + float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2]) { - p[0] = v1[0] + ((v2[0] - v1[0]) * uv[0]) + ((v3[0] - v1[0]) * uv[1]); - p[1] = v1[1] + ((v2[1] - v1[1]) * uv[0]) + ((v3[1] - v1[1]) * uv[1]); - p[2] = v1[2] + ((v2[2] - v1[2]) * uv[0]) + ((v3[2] - v1[2]) * uv[1]); + p[0] = v1[0] + ((v2[0] - v1[0]) * uv[0]) + ((v3[0] - v1[0]) * uv[1]); + p[1] = v1[1] + ((v2[1] - v1[1]) * uv[0]) + ((v3[1] - v1[1]) * uv[1]); + p[2] = v1[2] + ((v2[2] - v1[2]) * uv[0]) + ((v3[2] - v1[2]) * uv[1]); } -void interp_v3_v3v3_uchar(char unsigned target[3], const unsigned char a[3], const unsigned char b[3], const float t) +void interp_v3_v3v3_uchar(char unsigned target[3], + const unsigned char a[3], + const unsigned char b[3], + const float t) { - const float s = 1.0f - t; + const float s = 1.0f - t; - target[0] = (char)floorf(s * a[0] + t * b[0]); - target[1] = (char)floorf(s * a[1] + t * b[1]); - target[2] = (char)floorf(s * a[2] + t * b[2]); + target[0] = (char)floorf(s * a[0] + t * b[0]); + target[1] = (char)floorf(s * a[1] + t * b[1]); + target[2] = (char)floorf(s * a[2] + t * b[2]); } void interp_v3_v3v3_char(char target[3], const char a[3], const char b[3], const float t) { - interp_v3_v3v3_uchar((unsigned char *)target, (const unsigned char *)a, (const unsigned char *)b, t); + interp_v3_v3v3_uchar( + (unsigned char *)target, (const unsigned char *)a, (const unsigned char *)b, t); } -void interp_v4_v4v4_uchar(char unsigned target[4], const unsigned char a[4], const unsigned char b[4], const float t) +void interp_v4_v4v4_uchar(char unsigned target[4], + const unsigned char a[4], + const unsigned char b[4], + const float t) { - const float s = 1.0f - t; + const float s = 1.0f - t; - target[0] = (char)floorf(s * a[0] + t * b[0]); - target[1] = (char)floorf(s * a[1] + t * b[1]); - target[2] = (char)floorf(s * a[2] + t * b[2]); - target[3] = (char)floorf(s * a[3] + t * b[3]); + target[0] = (char)floorf(s * a[0] + t * b[0]); + target[1] = (char)floorf(s * a[1] + t * b[1]); + target[2] = (char)floorf(s * a[2] + t * b[2]); + target[3] = (char)floorf(s * a[3] + t * b[3]); } void interp_v4_v4v4_char(char target[4], const char a[4], const char b[4], const float t) { - interp_v4_v4v4_uchar((unsigned char *)target, (const unsigned char *)a, (const unsigned char *)b, t); + interp_v4_v4v4_uchar( + (unsigned char *)target, (const unsigned char *)a, (const unsigned char *)b, t); } void mid_v3_v3v3(float v[3], const float v1[3], const float v2[3]) { - v[0] = 0.5f * (v1[0] + v2[0]); - v[1] = 0.5f * (v1[1] + v2[1]); - v[2] = 0.5f * (v1[2] + v2[2]); + v[0] = 0.5f * (v1[0] + v2[0]); + v[1] = 0.5f * (v1[1] + v2[1]); + v[2] = 0.5f * (v1[2] + v2[2]); } void mid_v2_v2v2(float v[2], const float v1[2], const float v2[2]) { - v[0] = 0.5f * (v1[0] + v2[0]); - v[1] = 0.5f * (v1[1] + v2[1]); + v[0] = 0.5f * (v1[0] + v2[0]); + v[1] = 0.5f * (v1[1] + v2[1]); } void mid_v3_v3v3v3(float v[3], const float v1[3], const float v2[3], const float v3[3]) { - v[0] = (v1[0] + v2[0] + v3[0]) / 3.0f; - v[1] = (v1[1] + v2[1] + v3[1]) / 3.0f; - v[2] = (v1[2] + v2[2] + v3[2]) / 3.0f; + v[0] = (v1[0] + v2[0] + v3[0]) / 3.0f; + v[1] = (v1[1] + v2[1] + v3[1]) / 3.0f; + v[2] = (v1[2] + v2[2] + v3[2]) / 3.0f; } -void mid_v3_v3v3v3v3(float v[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3]) +void mid_v3_v3v3v3v3( + float v[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3]) { - v[0] = (v1[0] + v2[0] + v3[0] + v4[0]) / 4.0f; - v[1] = (v1[1] + v2[1] + v3[1] + v4[1]) / 4.0f; - v[2] = (v1[2] + v2[2] + v3[2] + v4[2]) / 4.0f; + v[0] = (v1[0] + v2[0] + v3[0] + v4[0]) / 4.0f; + v[1] = (v1[1] + v2[1] + v3[1] + v4[1]) / 4.0f; + v[2] = (v1[2] + v2[2] + v3[2] + v4[2]) / 4.0f; } void mid_v3_v3_array(float r[3], const float (*vec_arr)[3], const unsigned int nbr) { - const float factor = 1.0f / (float)nbr; - zero_v3(r); + const float factor = 1.0f / (float)nbr; + zero_v3(r); - for (unsigned int i = 0; i < nbr; i++) { - madd_v3_v3fl(r, vec_arr[i], factor); - } + for (unsigned int i = 0; i < nbr; i++) { + madd_v3_v3fl(r, vec_arr[i], factor); + } } /** @@ -301,21 +327,21 @@ void mid_v3_v3_array(float r[3], const float (*vec_arr)[3], const unsigned int n */ void mid_v3_v3v3_angle_weighted(float r[3], const float a[3], const float b[3]) { - /* trick, we want the middle of 2 normals as well as the angle between them - * avoid multiple calculations by */ - float angle; + /* trick, we want the middle of 2 normals as well as the angle between them + * avoid multiple calculations by */ + float angle; - /* double check they are normalized */ - BLI_ASSERT_UNIT_V3(a); - BLI_ASSERT_UNIT_V3(b); + /* double check they are normalized */ + BLI_ASSERT_UNIT_V3(a); + BLI_ASSERT_UNIT_V3(b); - add_v3_v3v3(r, a, b); - angle = ((float)(1.0 / (M_PI / 2.0)) * - /* normally we would only multiply by 2, - * but instead of an angle make this 0-1 factor */ - 2.0f) * - acosf(normalize_v3(r) / 2.0f); - mul_v3_fl(r, angle); + add_v3_v3v3(r, a, b); + angle = ((float)(1.0 / (M_PI / 2.0)) * + /* normally we would only multiply by 2, + * but instead of an angle make this 0-1 factor */ + 2.0f) * + acosf(normalize_v3(r) / 2.0f); + mul_v3_fl(r, angle); } /** * Same as mid_v3_v3v3_angle_weighted @@ -323,19 +349,19 @@ void mid_v3_v3v3_angle_weighted(float r[3], const float a[3], const float b[3]) */ void mid_v3_angle_weighted(float r[3]) { - /* trick, we want the middle of 2 normals as well as the angle between them - * avoid multiple calculations by */ - float angle; + /* trick, we want the middle of 2 normals as well as the angle between them + * avoid multiple calculations by */ + float angle; - /* double check they are normalized */ - BLI_assert(len_squared_v3(r) <= 1.0f + FLT_EPSILON); + /* double check they are normalized */ + BLI_assert(len_squared_v3(r) <= 1.0f + FLT_EPSILON); - angle = ((float)(1.0 / (M_PI / 2.0)) * - /* normally we would only multiply by 2, - * but instead of an angle make this 0-1 factor */ - 2.0f) * - acosf(normalize_v3(r)); - mul_v3_fl(r, angle); + angle = ((float)(1.0 / (M_PI / 2.0)) * + /* normally we would only multiply by 2, + * but instead of an angle make this 0-1 factor */ + 2.0f) * + acosf(normalize_v3(r)); + mul_v3_fl(r, angle); } /** @@ -345,44 +371,42 @@ void mid_v3_angle_weighted(float r[3]) void flip_v4_v4v4(float v[4], const float v1[4], const float v2[4]) { - v[0] = v1[0] + (v1[0] - v2[0]); - v[1] = v1[1] + (v1[1] - v2[1]); - v[2] = v1[2] + (v1[2] - v2[2]); - v[3] = v1[3] + (v1[3] - v2[3]); + v[0] = v1[0] + (v1[0] - v2[0]); + v[1] = v1[1] + (v1[1] - v2[1]); + v[2] = v1[2] + (v1[2] - v2[2]); + v[3] = v1[3] + (v1[3] - v2[3]); } void flip_v3_v3v3(float v[3], const float v1[3], const float v2[3]) { - v[0] = v1[0] + (v1[0] - v2[0]); - v[1] = v1[1] + (v1[1] - v2[1]); - v[2] = v1[2] + (v1[2] - v2[2]); + v[0] = v1[0] + (v1[0] - v2[0]); + v[1] = v1[1] + (v1[1] - v2[1]); + v[2] = v1[2] + (v1[2] - v2[2]); } void flip_v2_v2v2(float v[2], const float v1[2], const float v2[2]) { - v[0] = v1[0] + (v1[0] - v2[0]); - v[1] = v1[1] + (v1[1] - v2[1]); + v[0] = v1[0] + (v1[0] - v2[0]); + v[1] = v1[1] + (v1[1] - v2[1]); } - /********************************* Comparison ********************************/ bool is_finite_v2(const float v[2]) { - return (isfinite(v[0]) && isfinite(v[1])); + return (isfinite(v[0]) && isfinite(v[1])); } bool is_finite_v3(const float v[3]) { - return (isfinite(v[0]) && isfinite(v[1]) && isfinite(v[2])); + return (isfinite(v[0]) && isfinite(v[1]) && isfinite(v[2])); } bool is_finite_v4(const float v[4]) { - return (isfinite(v[0]) && isfinite(v[1]) && isfinite(v[2]) && isfinite(v[3])); + return (isfinite(v[0]) && isfinite(v[1]) && isfinite(v[2]) && isfinite(v[3])); } - /********************************** Angles ***********************************/ /* Return the angle in radians between vecs 1-2 and 2-3 in radians @@ -394,124 +418,124 @@ bool is_finite_v4(const float v[4]) */ float angle_v3v3v3(const float v1[3], const float v2[3], const float v3[3]) { - float vec1[3], vec2[3]; + float vec1[3], vec2[3]; - sub_v3_v3v3(vec1, v2, v1); - sub_v3_v3v3(vec2, v2, v3); - normalize_v3(vec1); - normalize_v3(vec2); + sub_v3_v3v3(vec1, v2, v1); + sub_v3_v3v3(vec2, v2, v3); + normalize_v3(vec1); + normalize_v3(vec2); - return angle_normalized_v3v3(vec1, vec2); + return angle_normalized_v3v3(vec1, vec2); } /* Quicker than full angle computation */ float cos_v3v3v3(const float p1[3], const float p2[3], const float p3[3]) { - float vec1[3], vec2[3]; + float vec1[3], vec2[3]; - sub_v3_v3v3(vec1, p2, p1); - sub_v3_v3v3(vec2, p2, p3); - normalize_v3(vec1); - normalize_v3(vec2); + sub_v3_v3v3(vec1, p2, p1); + sub_v3_v3v3(vec2, p2, p3); + normalize_v3(vec1); + normalize_v3(vec2); - return dot_v3v3(vec1, vec2); + return dot_v3v3(vec1, vec2); } /* Return the shortest angle in radians between the 2 vectors */ float angle_v3v3(const float v1[3], const float v2[3]) { - float vec1[3], vec2[3]; + float vec1[3], vec2[3]; - normalize_v3_v3(vec1, v1); - normalize_v3_v3(vec2, v2); + normalize_v3_v3(vec1, v1); + normalize_v3_v3(vec2, v2); - return angle_normalized_v3v3(vec1, vec2); + return angle_normalized_v3v3(vec1, vec2); } float angle_v2v2v2(const float v1[2], const float v2[2], const float v3[2]) { - float vec1[2], vec2[2]; + float vec1[2], vec2[2]; - vec1[0] = v2[0] - v1[0]; - vec1[1] = v2[1] - v1[1]; + vec1[0] = v2[0] - v1[0]; + vec1[1] = v2[1] - v1[1]; - vec2[0] = v2[0] - v3[0]; - vec2[1] = v2[1] - v3[1]; + vec2[0] = v2[0] - v3[0]; + vec2[1] = v2[1] - v3[1]; - normalize_v2(vec1); - normalize_v2(vec2); + normalize_v2(vec1); + normalize_v2(vec2); - return angle_normalized_v2v2(vec1, vec2); + return angle_normalized_v2v2(vec1, vec2); } /* Quicker than full angle computation */ float cos_v2v2v2(const float p1[2], const float p2[2], const float p3[2]) { - float vec1[2], vec2[2]; + float vec1[2], vec2[2]; - sub_v2_v2v2(vec1, p2, p1); - sub_v2_v2v2(vec2, p2, p3); - normalize_v2(vec1); - normalize_v2(vec2); + sub_v2_v2v2(vec1, p2, p1); + sub_v2_v2v2(vec2, p2, p3); + normalize_v2(vec1); + normalize_v2(vec2); - return dot_v2v2(vec1, vec2); + return dot_v2v2(vec1, vec2); } /* Return the shortest angle in radians between the 2 vectors */ float angle_v2v2(const float v1[2], const float v2[2]) { - float vec1[2], vec2[2]; + float vec1[2], vec2[2]; - vec1[0] = v1[0]; - vec1[1] = v1[1]; + vec1[0] = v1[0]; + vec1[1] = v1[1]; - vec2[0] = v2[0]; - vec2[1] = v2[1]; + vec2[0] = v2[0]; + vec2[1] = v2[1]; - normalize_v2(vec1); - normalize_v2(vec2); + normalize_v2(vec1); + normalize_v2(vec2); - return angle_normalized_v2v2(vec1, vec2); + return angle_normalized_v2v2(vec1, vec2); } float angle_signed_v2v2(const float v1[2], const float v2[2]) { - const float perp_dot = (v1[1] * v2[0]) - (v1[0] * v2[1]); - return atan2f(perp_dot, dot_v2v2(v1, v2)); + const float perp_dot = (v1[1] * v2[0]) - (v1[0] * v2[1]); + return atan2f(perp_dot, dot_v2v2(v1, v2)); } float angle_normalized_v3v3(const float v1[3], const float v2[3]) { - /* double check they are normalized */ - BLI_ASSERT_UNIT_V3(v1); - BLI_ASSERT_UNIT_V3(v2); + /* double check they are normalized */ + BLI_ASSERT_UNIT_V3(v1); + BLI_ASSERT_UNIT_V3(v2); - /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */ - if (dot_v3v3(v1, v2) >= 0.0f) { - return 2.0f * saasin(len_v3v3(v1, v2) / 2.0f); - } - else { - float v2_n[3]; - negate_v3_v3(v2_n, v2); - return (float)M_PI - 2.0f * saasin(len_v3v3(v1, v2_n) / 2.0f); - } + /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */ + if (dot_v3v3(v1, v2) >= 0.0f) { + return 2.0f * saasin(len_v3v3(v1, v2) / 2.0f); + } + else { + float v2_n[3]; + negate_v3_v3(v2_n, v2); + return (float)M_PI - 2.0f * saasin(len_v3v3(v1, v2_n) / 2.0f); + } } float angle_normalized_v2v2(const float v1[2], const float v2[2]) { - /* double check they are normalized */ - BLI_ASSERT_UNIT_V2(v1); - BLI_ASSERT_UNIT_V2(v2); + /* double check they are normalized */ + BLI_ASSERT_UNIT_V2(v1); + BLI_ASSERT_UNIT_V2(v2); - /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */ - if (dot_v2v2(v1, v2) >= 0.0f) { - return 2.0f * saasin(len_v2v2(v1, v2) / 2.0f); - } - else { - float v2_n[2]; - negate_v2_v2(v2_n, v2); - return (float)M_PI - 2.0f * saasin(len_v2v2(v1, v2_n) / 2.0f); - } + /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */ + if (dot_v2v2(v1, v2) >= 0.0f) { + return 2.0f * saasin(len_v2v2(v1, v2) / 2.0f); + } + else { + float v2_n[2]; + negate_v2_v2(v2_n, v2); + return (float)M_PI - 2.0f * saasin(len_v2v2(v1, v2_n) / 2.0f); + } } /** @@ -519,108 +543,115 @@ float angle_normalized_v2v2(const float v1[2], const float v2[2]) */ float angle_on_axis_v3v3_v3(const float v1[3], const float v2[3], const float axis[3]) { - float v1_proj[3], v2_proj[3]; + float v1_proj[3], v2_proj[3]; - /* project the vectors onto the axis */ - project_plane_normalized_v3_v3v3(v1_proj, v1, axis); - project_plane_normalized_v3_v3v3(v2_proj, v2, axis); + /* project the vectors onto the axis */ + project_plane_normalized_v3_v3v3(v1_proj, v1, axis); + project_plane_normalized_v3_v3v3(v2_proj, v2, axis); - return angle_v3v3(v1_proj, v2_proj); + return angle_v3v3(v1_proj, v2_proj); } float angle_signed_on_axis_v3v3_v3(const float v1[3], const float v2[3], const float axis[3]) { - float v1_proj[3], v2_proj[3], tproj[3]; - float angle; + float v1_proj[3], v2_proj[3], tproj[3]; + float angle; - /* project the vectors onto the axis */ - project_plane_normalized_v3_v3v3(v1_proj, v1, axis); - project_plane_normalized_v3_v3v3(v2_proj, v2, axis); + /* project the vectors onto the axis */ + project_plane_normalized_v3_v3v3(v1_proj, v1, axis); + project_plane_normalized_v3_v3v3(v2_proj, v2, axis); - angle = angle_v3v3(v1_proj, v2_proj); + angle = angle_v3v3(v1_proj, v2_proj); - /* calculate the sign (reuse 'tproj') */ - cross_v3_v3v3(tproj, v2_proj, v1_proj); - if (dot_v3v3(tproj, axis) < 0.0f) { - angle = ((float)(M_PI * 2.0)) - angle; - } + /* calculate the sign (reuse 'tproj') */ + cross_v3_v3v3(tproj, v2_proj, v1_proj); + if (dot_v3v3(tproj, axis) < 0.0f) { + angle = ((float)(M_PI * 2.0)) - angle; + } - return angle; + return angle; } /** * Angle between 2 vectors defined by 3 coords, about an axis (axis can be considered a plane). */ -float angle_on_axis_v3v3v3_v3(const float v1[3], const float v2[3], const float v3[3], const float axis[3]) +float angle_on_axis_v3v3v3_v3(const float v1[3], + const float v2[3], + const float v3[3], + const float axis[3]) { - float vec1[3], vec2[3]; + float vec1[3], vec2[3]; - sub_v3_v3v3(vec1, v1, v2); - sub_v3_v3v3(vec2, v3, v2); + sub_v3_v3v3(vec1, v1, v2); + sub_v3_v3v3(vec2, v3, v2); - return angle_on_axis_v3v3_v3(vec1, vec2, axis); + return angle_on_axis_v3v3_v3(vec1, vec2, axis); } -float angle_signed_on_axis_v3v3v3_v3(const float v1[3], const float v2[3], const float v3[3], const float axis[3]) +float angle_signed_on_axis_v3v3v3_v3(const float v1[3], + const float v2[3], + const float v3[3], + const float axis[3]) { - float vec1[3], vec2[3]; + float vec1[3], vec2[3]; - sub_v3_v3v3(vec1, v1, v2); - sub_v3_v3v3(vec2, v3, v2); + sub_v3_v3v3(vec1, v1, v2); + sub_v3_v3v3(vec2, v3, v2); - return angle_signed_on_axis_v3v3_v3(vec1, vec2, axis); + return angle_signed_on_axis_v3v3_v3(vec1, vec2, axis); } void angle_tri_v3(float angles[3], const float v1[3], const float v2[3], const float v3[3]) { - float ed1[3], ed2[3], ed3[3]; + float ed1[3], ed2[3], ed3[3]; - sub_v3_v3v3(ed1, v3, v1); - sub_v3_v3v3(ed2, v1, v2); - sub_v3_v3v3(ed3, v2, v3); + sub_v3_v3v3(ed1, v3, v1); + sub_v3_v3v3(ed2, v1, v2); + sub_v3_v3v3(ed3, v2, v3); - normalize_v3(ed1); - normalize_v3(ed2); - normalize_v3(ed3); + normalize_v3(ed1); + normalize_v3(ed2); + normalize_v3(ed3); - angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2); - angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3); - // face_angles[2] = M_PI - angle_normalized_v3v3(ed3, ed1); - angles[2] = (float)M_PI - (angles[0] + angles[1]); + angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2); + angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3); + // face_angles[2] = M_PI - angle_normalized_v3v3(ed3, ed1); + angles[2] = (float)M_PI - (angles[0] + angles[1]); } -void angle_quad_v3(float angles[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3]) +void angle_quad_v3( + float angles[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3]) { - float ed1[3], ed2[3], ed3[3], ed4[3]; + float ed1[3], ed2[3], ed3[3], ed4[3]; - sub_v3_v3v3(ed1, v4, v1); - sub_v3_v3v3(ed2, v1, v2); - sub_v3_v3v3(ed3, v2, v3); - sub_v3_v3v3(ed4, v3, v4); + sub_v3_v3v3(ed1, v4, v1); + sub_v3_v3v3(ed2, v1, v2); + sub_v3_v3v3(ed3, v2, v3); + sub_v3_v3v3(ed4, v3, v4); - normalize_v3(ed1); - normalize_v3(ed2); - normalize_v3(ed3); - normalize_v3(ed4); + normalize_v3(ed1); + normalize_v3(ed2); + normalize_v3(ed3); + normalize_v3(ed4); - angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2); - angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3); - angles[2] = (float)M_PI - angle_normalized_v3v3(ed3, ed4); - angles[3] = (float)M_PI - angle_normalized_v3v3(ed4, ed1); + angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2); + angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3); + angles[2] = (float)M_PI - angle_normalized_v3v3(ed3, ed4); + angles[3] = (float)M_PI - angle_normalized_v3v3(ed4, ed1); } void angle_poly_v3(float *angles, const float *verts[3], int len) { - int i; - float vec[3][3]; + int i; + float vec[3][3]; - sub_v3_v3v3(vec[2], verts[len - 1], verts[0]); - normalize_v3(vec[2]); - for (i = 0; i < len; i++) { - sub_v3_v3v3(vec[i % 3], verts[i % len], verts[(i + 1) % len]); - normalize_v3(vec[i % 3]); - angles[i] = (float)M_PI - angle_normalized_v3v3(vec[(i + 2) % 3], vec[i % 3]); - } + sub_v3_v3v3(vec[2], verts[len - 1], verts[0]); + normalize_v3(vec[2]); + for (i = 0; i < len; i++) { + sub_v3_v3v3(vec[i % 3], verts[i % len], verts[(i + 1) % len]); + normalize_v3(vec[i % 3]); + angles[i] = (float)M_PI - angle_normalized_v3v3(vec[(i + 2) % 3], vec[i % 3]); + } } /********************************* Geometry **********************************/ @@ -630,10 +661,10 @@ void angle_poly_v3(float *angles, const float *verts[3], int len) */ void project_v2_v2v2(float out[2], const float p[2], const float v_proj[2]) { - const float mul = dot_v2v2(p, v_proj) / dot_v2v2(v_proj, v_proj); + const float mul = dot_v2v2(p, v_proj) / dot_v2v2(v_proj, v_proj); - out[0] = mul * v_proj[0]; - out[1] = mul * v_proj[1]; + out[0] = mul * v_proj[0]; + out[1] = mul * v_proj[1]; } /** @@ -641,11 +672,11 @@ void project_v2_v2v2(float out[2], const float p[2], const float v_proj[2]) */ void project_v3_v3v3(float out[3], const float p[3], const float v_proj[3]) { - const float mul = dot_v3v3(p, v_proj) / dot_v3v3(v_proj, v_proj); + const float mul = dot_v3v3(p, v_proj) / dot_v3v3(v_proj, v_proj); - out[0] = mul * v_proj[0]; - out[1] = mul * v_proj[1]; - out[2] = mul * v_proj[2]; + out[0] = mul * v_proj[0]; + out[1] = mul * v_proj[1]; + out[2] = mul * v_proj[2]; } /** @@ -653,11 +684,11 @@ void project_v3_v3v3(float out[3], const float p[3], const float v_proj[3]) */ void project_v2_v2v2_normalized(float out[2], const float p[2], const float v_proj[2]) { - BLI_ASSERT_UNIT_V2(v_proj); - const float mul = dot_v2v2(p, v_proj); + BLI_ASSERT_UNIT_V2(v_proj); + const float mul = dot_v2v2(p, v_proj); - out[0] = mul * v_proj[0]; - out[1] = mul * v_proj[1]; + out[0] = mul * v_proj[0]; + out[1] = mul * v_proj[1]; } /** @@ -665,12 +696,12 @@ void project_v2_v2v2_normalized(float out[2], const float p[2], const float v_pr */ void project_v3_v3v3_normalized(float out[3], const float p[3], const float v_proj[3]) { - BLI_ASSERT_UNIT_V3(v_proj); - const float mul = dot_v3v3(p, v_proj); + BLI_ASSERT_UNIT_V3(v_proj); + const float mul = dot_v3v3(p, v_proj); - out[0] = mul * v_proj[0]; - out[1] = mul * v_proj[1]; - out[2] = mul * v_proj[2]; + out[0] = mul * v_proj[0]; + out[1] = mul * v_proj[1]; + out[2] = mul * v_proj[2]; } /** @@ -688,64 +719,64 @@ void project_v3_v3v3_normalized(float out[3], const float p[3], const float v_pr */ void project_plane_v3_v3v3(float out[3], const float p[3], const float v_plane[3]) { - const float mul = dot_v3v3(p, v_plane) / dot_v3v3(v_plane, v_plane); + const float mul = dot_v3v3(p, v_plane) / dot_v3v3(v_plane, v_plane); - out[0] = p[0] - (mul * v_plane[0]); - out[1] = p[1] - (mul * v_plane[1]); - out[2] = p[2] - (mul * v_plane[2]); + out[0] = p[0] - (mul * v_plane[0]); + out[1] = p[1] - (mul * v_plane[1]); + out[2] = p[2] - (mul * v_plane[2]); } void project_plane_v2_v2v2(float out[2], const float p[2], const float v_plane[2]) { - const float mul = dot_v2v2(p, v_plane) / dot_v2v2(v_plane, v_plane); + const float mul = dot_v2v2(p, v_plane) / dot_v2v2(v_plane, v_plane); - out[0] = p[0] - (mul * v_plane[0]); - out[1] = p[1] - (mul * v_plane[1]); + out[0] = p[0] - (mul * v_plane[0]); + out[1] = p[1] - (mul * v_plane[1]); } void project_plane_normalized_v3_v3v3(float out[3], const float p[3], const float v_plane[3]) { - BLI_ASSERT_UNIT_V3(v_plane); - const float mul = dot_v3v3(p, v_plane); + BLI_ASSERT_UNIT_V3(v_plane); + const float mul = dot_v3v3(p, v_plane); - out[0] = p[0] - (mul * v_plane[0]); - out[1] = p[1] - (mul * v_plane[1]); - out[2] = p[2] - (mul * v_plane[2]); + out[0] = p[0] - (mul * v_plane[0]); + out[1] = p[1] - (mul * v_plane[1]); + out[2] = p[2] - (mul * v_plane[2]); } void project_plane_normalized_v2_v2v2(float out[2], const float p[2], const float v_plane[2]) { - BLI_ASSERT_UNIT_V2(v_plane); - const float mul = dot_v2v2(p, v_plane); + BLI_ASSERT_UNIT_V2(v_plane); + const float mul = dot_v2v2(p, v_plane); - out[0] = p[0] - (mul * v_plane[0]); - out[1] = p[1] - (mul * v_plane[1]); + out[0] = p[0] - (mul * v_plane[0]); + out[1] = p[1] - (mul * v_plane[1]); } /* project a vector on a plane defined by normal and a plane point p */ void project_v3_plane(float out[3], const float plane_no[3], const float plane_co[3]) { - float vector[3]; - float mul; + float vector[3]; + float mul; - sub_v3_v3v3(vector, out, plane_co); - mul = dot_v3v3(vector, plane_no) / len_squared_v3(plane_no); + sub_v3_v3v3(vector, out, plane_co); + mul = dot_v3v3(vector, plane_no) / len_squared_v3(plane_no); - mul_v3_v3fl(vector, plane_no, mul); + mul_v3_v3fl(vector, plane_no, mul); - sub_v3_v3(out, vector); + sub_v3_v3(out, vector); } /* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */ void bisect_v3_v3v3v3(float out[3], const float v1[3], const float v2[3], const float v3[3]) { - float d_12[3], d_23[3]; - sub_v3_v3v3(d_12, v2, v1); - sub_v3_v3v3(d_23, v3, v2); - normalize_v3(d_12); - normalize_v3(d_23); - add_v3_v3v3(out, d_12, d_23); - normalize_v3(out); + float d_12[3], d_23[3]; + sub_v3_v3v3(d_12, v2, v1); + sub_v3_v3v3(d_23, v3, v2); + normalize_v3(d_12); + normalize_v3(d_23); + add_v3_v3v3(out, d_12, d_23); + normalize_v3(out); } /** @@ -766,13 +797,13 @@ void bisect_v3_v3v3v3(float out[3], const float v1[3], const float v2[3], const */ void reflect_v3_v3v3(float out[3], const float v[3], const float normal[3]) { - const float dot2 = 2.0f * dot_v3v3(v, normal); + const float dot2 = 2.0f * dot_v3v3(v, normal); - BLI_ASSERT_UNIT_V3(normal); + BLI_ASSERT_UNIT_V3(normal); - out[0] = v[0] - (dot2 * normal[0]); - out[1] = v[1] - (dot2 * normal[1]); - out[2] = v[2] - (dot2 * normal[2]); + out[0] = v[0] - (dot2 * normal[0]); + out[1] = v[1] - (dot2 * normal[1]); + out[2] = v[2] - (dot2 * normal[2]); } /** @@ -782,27 +813,27 @@ void reflect_v3_v3v3(float out[3], const float v[3], const float normal[3]) */ void ortho_basis_v3v3_v3(float r_n1[3], float r_n2[3], const float n[3]) { - const float eps = FLT_EPSILON; - const float f = len_squared_v2(n); + const float eps = FLT_EPSILON; + const float f = len_squared_v2(n); - if (f > eps) { - const float d = 1.0f / sqrtf(f); + if (f > eps) { + const float d = 1.0f / sqrtf(f); - BLI_assert(isfinite(d)); + BLI_assert(isfinite(d)); - r_n1[0] = n[1] * d; - r_n1[1] = -n[0] * d; - r_n1[2] = 0.0f; - r_n2[0] = -n[2] * r_n1[1]; - r_n2[1] = n[2] * r_n1[0]; - r_n2[2] = n[0] * r_n1[1] - n[1] * r_n1[0]; - } - else { - /* degenerate case */ - r_n1[0] = (n[2] < 0.0f) ? -1.0f : 1.0f; - r_n1[1] = r_n1[2] = r_n2[0] = r_n2[2] = 0.0f; - r_n2[1] = 1.0f; - } + r_n1[0] = n[1] * d; + r_n1[1] = -n[0] * d; + r_n1[2] = 0.0f; + r_n2[0] = -n[2] * r_n1[1]; + r_n2[1] = n[2] * r_n1[0]; + r_n2[2] = n[0] * r_n1[1] - n[1] * r_n1[0]; + } + else { + /* degenerate case */ + r_n1[0] = (n[2] < 0.0f) ? -1.0f : 1.0f; + r_n1[1] = r_n1[2] = r_n2[0] = r_n2[2] = 0.0f; + r_n2[1] = 1.0f; + } } /** @@ -812,27 +843,27 @@ void ortho_basis_v3v3_v3(float r_n1[3], float r_n2[3], const float n[3]) */ void ortho_v3_v3(float out[3], const float v[3]) { - const int axis = axis_dominant_v3_single(v); - - BLI_assert(out != v); - - switch (axis) { - case 0: - out[0] = -v[1] - v[2]; - out[1] = v[0]; - out[2] = v[0]; - break; - case 1: - out[0] = v[1]; - out[1] = -v[0] - v[2]; - out[2] = v[1]; - break; - case 2: - out[0] = v[2]; - out[1] = v[2]; - out[2] = -v[0] - v[1]; - break; - } + const int axis = axis_dominant_v3_single(v); + + BLI_assert(out != v); + + switch (axis) { + case 0: + out[0] = -v[1] - v[2]; + out[1] = v[0]; + out[2] = v[0]; + break; + case 1: + out[0] = v[1]; + out[1] = -v[0] - v[2]; + out[2] = v[1]; + break; + case 2: + out[0] = v[2]; + out[1] = v[2]; + out[2] = -v[0] - v[1]; + break; + } } /** @@ -840,10 +871,10 @@ void ortho_v3_v3(float out[3], const float v[3]) */ void ortho_v2_v2(float out[2], const float v[2]) { - BLI_assert(out != v); + BLI_assert(out != v); - out[0] = -v[1]; - out[1] = v[0]; + out[0] = -v[1]; + out[1] = v[0]; } /** @@ -851,146 +882,179 @@ void ortho_v2_v2(float out[2], const float v[2]) */ void rotate_v2_v2fl(float r[2], const float p[2], const float angle) { - const float co = cosf(angle); - const float si = sinf(angle); + const float co = cosf(angle); + const float si = sinf(angle); - BLI_assert(r != p); + BLI_assert(r != p); - r[0] = co * p[0] - si * p[1]; - r[1] = si * p[0] + co * p[1]; + r[0] = co * p[0] - si * p[1]; + r[1] = si * p[0] + co * p[1]; } /** * Rotate a point \a p by \a angle around an arbitrary unit length \a axis. * http://local.wasp.uwa.edu.au/~pbourke/geometry/ */ -void rotate_normalized_v3_v3v3fl(float out[3], const float p[3], const float axis[3], const float angle) +void rotate_normalized_v3_v3v3fl(float out[3], + const float p[3], + const float axis[3], + const float angle) { - const float costheta = cosf(angle); - const float sintheta = sinf(angle); + const float costheta = cosf(angle); + const float sintheta = sinf(angle); - /* double check they are normalized */ - BLI_ASSERT_UNIT_V3(axis); + /* double check they are normalized */ + BLI_ASSERT_UNIT_V3(axis); - out[0] = ((costheta + (1 - costheta) * axis[0] * axis[0]) * p[0]) + - (((1 - costheta) * axis[0] * axis[1] - axis[2] * sintheta) * p[1]) + - (((1 - costheta) * axis[0] * axis[2] + axis[1] * sintheta) * p[2]); + out[0] = ((costheta + (1 - costheta) * axis[0] * axis[0]) * p[0]) + + (((1 - costheta) * axis[0] * axis[1] - axis[2] * sintheta) * p[1]) + + (((1 - costheta) * axis[0] * axis[2] + axis[1] * sintheta) * p[2]); - out[1] = (((1 - costheta) * axis[0] * axis[1] + axis[2] * sintheta) * p[0]) + - ((costheta + (1 - costheta) * axis[1] * axis[1]) * p[1]) + - (((1 - costheta) * axis[1] * axis[2] - axis[0] * sintheta) * p[2]); + out[1] = (((1 - costheta) * axis[0] * axis[1] + axis[2] * sintheta) * p[0]) + + ((costheta + (1 - costheta) * axis[1] * axis[1]) * p[1]) + + (((1 - costheta) * axis[1] * axis[2] - axis[0] * sintheta) * p[2]); - out[2] = (((1 - costheta) * axis[0] * axis[2] - axis[1] * sintheta) * p[0]) + - (((1 - costheta) * axis[1] * axis[2] + axis[0] * sintheta) * p[1]) + - ((costheta + (1 - costheta) * axis[2] * axis[2]) * p[2]); + out[2] = (((1 - costheta) * axis[0] * axis[2] - axis[1] * sintheta) * p[0]) + + (((1 - costheta) * axis[1] * axis[2] + axis[0] * sintheta) * p[1]) + + ((costheta + (1 - costheta) * axis[2] * axis[2]) * p[2]); } void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle) { - BLI_assert(r != p); + BLI_assert(r != p); - float axis_n[3]; + float axis_n[3]; - normalize_v3_v3(axis_n, axis); + normalize_v3_v3(axis_n, axis); - rotate_normalized_v3_v3v3fl(r, p, axis_n, angle); + rotate_normalized_v3_v3v3fl(r, p, axis_n, angle); } /*********************************** Other ***********************************/ void print_v2(const char *str, const float v[2]) { - printf("%s: %.8f %.8f\n", str, v[0], v[1]); + printf("%s: %.8f %.8f\n", str, v[0], v[1]); } void print_v3(const char *str, const float v[3]) { - printf("%s: %.8f %.8f %.8f\n", str, v[0], v[1], v[2]); + printf("%s: %.8f %.8f %.8f\n", str, v[0], v[1], v[2]); } void print_v4(const char *str, const float v[4]) { - printf("%s: %.8f %.8f %.8f %.8f\n", str, v[0], v[1], v[2], v[3]); + printf("%s: %.8f %.8f %.8f %.8f\n", str, v[0], v[1], v[2], v[3]); } void print_vn(const char *str, const float v[], const int n) { - int i = 0; - printf("%s[%d]:", str, n); - while (i < n) { - printf(" %.8f", v[i++]); - } - printf("\n"); + int i = 0; + printf("%s[%d]:", str, n); + while (i < n) { + printf(" %.8f", v[i++]); + } + printf("\n"); } void minmax_v3v3_v3(float min[3], float max[3], const float vec[3]) { - if (min[0] > vec[0]) { min[0] = vec[0]; } - if (min[1] > vec[1]) { min[1] = vec[1]; } - if (min[2] > vec[2]) { min[2] = vec[2]; } - - if (max[0] < vec[0]) { max[0] = vec[0]; } - if (max[1] < vec[1]) { max[1] = vec[1]; } - if (max[2] < vec[2]) { max[2] = vec[2]; } + if (min[0] > vec[0]) { + min[0] = vec[0]; + } + if (min[1] > vec[1]) { + min[1] = vec[1]; + } + if (min[2] > vec[2]) { + min[2] = vec[2]; + } + + if (max[0] < vec[0]) { + max[0] = vec[0]; + } + if (max[1] < vec[1]) { + max[1] = vec[1]; + } + if (max[2] < vec[2]) { + max[2] = vec[2]; + } } void minmax_v2v2_v2(float min[2], float max[2], const float vec[2]) { - if (min[0] > vec[0]) { min[0] = vec[0]; } - if (min[1] > vec[1]) { min[1] = vec[1]; } + if (min[0] > vec[0]) { + min[0] = vec[0]; + } + if (min[1] > vec[1]) { + min[1] = vec[1]; + } - if (max[0] < vec[0]) { max[0] = vec[0]; } - if (max[1] < vec[1]) { max[1] = vec[1]; } + if (max[0] < vec[0]) { + max[0] = vec[0]; + } + if (max[1] < vec[1]) { + max[1] = vec[1]; + } } void minmax_v3v3_v3_array(float r_min[3], float r_max[3], const float (*vec_arr)[3], int nbr) { - while (nbr--) { - minmax_v3v3_v3(r_min, r_max, *vec_arr++); - } + while (nbr--) { + minmax_v3v3_v3(r_min, r_max, *vec_arr++); + } } /** ensure \a v1 is \a dist from \a v2 */ void dist_ensure_v3_v3fl(float v1[3], const float v2[3], const float dist) { - if (!equals_v3v3(v2, v1)) { - float nor[3]; + if (!equals_v3v3(v2, v1)) { + float nor[3]; - sub_v3_v3v3(nor, v1, v2); - normalize_v3(nor); - madd_v3_v3v3fl(v1, v2, nor, dist); - } + sub_v3_v3v3(nor, v1, v2); + normalize_v3(nor); + madd_v3_v3v3fl(v1, v2, nor, dist); + } } void dist_ensure_v2_v2fl(float v1[2], const float v2[2], const float dist) { - if (!equals_v2v2(v2, v1)) { - float nor[2]; + if (!equals_v2v2(v2, v1)) { + float nor[2]; - sub_v2_v2v2(nor, v1, v2); - normalize_v2(nor); - madd_v2_v2v2fl(v1, v2, nor, dist); - } + sub_v2_v2v2(nor, v1, v2); + normalize_v2(nor); + madd_v2_v2v2fl(v1, v2, nor, dist); + } } void axis_sort_v3(const float axis_values[3], int r_axis_order[3]) { - float v[3]; - copy_v3_v3(v, axis_values); - -#define SWAP_AXIS(a, b) { \ - SWAP(float, v[a], v[b]); \ - SWAP(int, r_axis_order[a], r_axis_order[b]); \ -} (void)0 - - if (v[0] < v[1]) { - if (v[2] < v[0]) { SWAP_AXIS(0, 2); } - } - else { - if (v[1] < v[2]) { SWAP_AXIS(0, 1); } - else { SWAP_AXIS(0, 2); } - } - if (v[2] < v[1]) { SWAP_AXIS(1, 2); } + float v[3]; + copy_v3_v3(v, axis_values); + +#define SWAP_AXIS(a, b) \ + { \ + SWAP(float, v[a], v[b]); \ + SWAP(int, r_axis_order[a], r_axis_order[b]); \ + } \ + (void)0 + + if (v[0] < v[1]) { + if (v[2] < v[0]) { + SWAP_AXIS(0, 2); + } + } + else { + if (v[1] < v[2]) { + SWAP_AXIS(0, 1); + } + else { + SWAP_AXIS(0, 2); + } + } + if (v[2] < v[1]) { + SWAP_AXIS(1, 2); + } #undef SWAP_AXIS } @@ -999,280 +1063,297 @@ void axis_sort_v3(const float axis_values[3], int r_axis_order[3]) MINLINE double sqr_db(double f) { - return f * f; + return f * f; } double dot_vn_vn(const float *array_src_a, const float *array_src_b, const int size) { - double d = 0.0f; - const float *array_pt_a = array_src_a + (size - 1); - const float *array_pt_b = array_src_b + (size - 1); - int i = size; - while (i--) { - d += (double)(*(array_pt_a--) * *(array_pt_b--)); - } - return d; + double d = 0.0f; + const float *array_pt_a = array_src_a + (size - 1); + const float *array_pt_b = array_src_b + (size - 1); + int i = size; + while (i--) { + d += (double)(*(array_pt_a--) * *(array_pt_b--)); + } + return d; } double len_squared_vn(const float *array, const int size) { - double d = 0.0f; - const float *array_pt = array + (size - 1); - int i = size; - while (i--) { - d += sqr_db((double)(*(array_pt--))); - } - return d; + double d = 0.0f; + const float *array_pt = array + (size - 1); + int i = size; + while (i--) { + d += sqr_db((double)(*(array_pt--))); + } + return d; } float normalize_vn_vn(float *array_tar, const float *array_src, const int size) { - const double d = len_squared_vn(array_src, size); - float d_sqrt; - if (d > 1.0e-35) { - d_sqrt = (float)sqrt(d); - mul_vn_vn_fl(array_tar, array_src, size, 1.0f / d_sqrt); - } - else { - copy_vn_fl(array_tar, size, 0.0f); - d_sqrt = 0.0f; - } - return d_sqrt; + const double d = len_squared_vn(array_src, size); + float d_sqrt; + if (d > 1.0e-35) { + d_sqrt = (float)sqrt(d); + mul_vn_vn_fl(array_tar, array_src, size, 1.0f / d_sqrt); + } + else { + copy_vn_fl(array_tar, size, 0.0f); + d_sqrt = 0.0f; + } + return d_sqrt; } float normalize_vn(float *array_tar, const int size) { - return normalize_vn_vn(array_tar, array_tar, size); + return normalize_vn_vn(array_tar, array_tar, size); } void range_vn_i(int *array_tar, const int size, const int start) { - int *array_pt = array_tar + (size - 1); - int j = start + (size - 1); - int i = size; - while (i--) { - *(array_pt--) = j--; - } + int *array_pt = array_tar + (size - 1); + int j = start + (size - 1); + int i = size; + while (i--) { + *(array_pt--) = j--; + } } void range_vn_u(unsigned int *array_tar, const int size, const unsigned int start) { - unsigned int *array_pt = array_tar + (size - 1); - unsigned int j = start + (unsigned int)(size - 1); - int i = size; - while (i--) { - *(array_pt--) = j--; - } + unsigned int *array_pt = array_tar + (size - 1); + unsigned int j = start + (unsigned int)(size - 1); + int i = size; + while (i--) { + *(array_pt--) = j--; + } } void range_vn_fl(float *array_tar, const int size, const float start, const float step) { - float *array_pt = array_tar + (size - 1); - int i = size; - while (i--) { - *(array_pt--) = start + step * (float)(i); - } + float *array_pt = array_tar + (size - 1); + int i = size; + while (i--) { + *(array_pt--) = start + step * (float)(i); + } } void negate_vn(float *array_tar, const int size) { - float *array_pt = array_tar + (size - 1); - int i = size; - while (i--) { - *(array_pt--) *= -1.0f; - } + float *array_pt = array_tar + (size - 1); + int i = size; + while (i--) { + *(array_pt--) *= -1.0f; + } } void negate_vn_vn(float *array_tar, const float *array_src, const int size) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) = -*(src--); - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) = -*(src--); + } } void mul_vn_vn(float *array_tar, const float *array_src, const int size) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) *= *(src--); - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) *= *(src--); + } } -void mul_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size) +void mul_vn_vnvn(float *array_tar, + const float *array_src_a, + const float *array_src_b, + const int size) { - float *tar = array_tar + (size - 1); - const float *src_a = array_src_a + (size - 1); - const float *src_b = array_src_b + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src_a--) * *(src_b--); - } + float *tar = array_tar + (size - 1); + const float *src_a = array_src_a + (size - 1); + const float *src_b = array_src_b + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src_a--) * *(src_b--); + } } void mul_vn_fl(float *array_tar, const int size, const float f) { - float *array_pt = array_tar + (size - 1); - int i = size; - while (i--) { - *(array_pt--) *= f; - } + float *array_pt = array_tar + (size - 1); + int i = size; + while (i--) { + *(array_pt--) *= f; + } } void mul_vn_vn_fl(float *array_tar, const float *array_src, const int size, const float f) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src--) * f; - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src--) * f; + } } void add_vn_vn(float *array_tar, const float *array_src, const int size) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) += *(src--); - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) += *(src--); + } } -void add_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size) +void add_vn_vnvn(float *array_tar, + const float *array_src_a, + const float *array_src_b, + const int size) { - float *tar = array_tar + (size - 1); - const float *src_a = array_src_a + (size - 1); - const float *src_b = array_src_b + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src_a--) + *(src_b--); - } + float *tar = array_tar + (size - 1); + const float *src_a = array_src_a + (size - 1); + const float *src_b = array_src_b + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src_a--) + *(src_b--); + } } void madd_vn_vn(float *array_tar, const float *array_src, const float f, const int size) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) += *(src--) * f; - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) += *(src--) * f; + } } -void madd_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const float f, const int size) +void madd_vn_vnvn(float *array_tar, + const float *array_src_a, + const float *array_src_b, + const float f, + const int size) { - float *tar = array_tar + (size - 1); - const float *src_a = array_src_a + (size - 1); - const float *src_b = array_src_b + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src_a--) + (*(src_b--) * f); - } + float *tar = array_tar + (size - 1); + const float *src_a = array_src_a + (size - 1); + const float *src_b = array_src_b + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src_a--) + (*(src_b--) * f); + } } void sub_vn_vn(float *array_tar, const float *array_src, const int size) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) -= *(src--); - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) -= *(src--); + } } -void sub_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size) +void sub_vn_vnvn(float *array_tar, + const float *array_src_a, + const float *array_src_b, + const int size) { - float *tar = array_tar + (size - 1); - const float *src_a = array_src_a + (size - 1); - const float *src_b = array_src_b + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src_a--) - *(src_b--); - } + float *tar = array_tar + (size - 1); + const float *src_a = array_src_a + (size - 1); + const float *src_b = array_src_b + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src_a--) - *(src_b--); + } } void msub_vn_vn(float *array_tar, const float *array_src, const float f, const int size) { - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) -= *(src--) * f; - } + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) -= *(src--) * f; + } } -void msub_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const float f, const int size) +void msub_vn_vnvn(float *array_tar, + const float *array_src_a, + const float *array_src_b, + const float f, + const int size) { - float *tar = array_tar + (size - 1); - const float *src_a = array_src_a + (size - 1); - const float *src_b = array_src_b + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src_a--) - (*(src_b--) * f); - } + float *tar = array_tar + (size - 1); + const float *src_a = array_src_a + (size - 1); + const float *src_b = array_src_b + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src_a--) - (*(src_b--) * f); + } } void interp_vn_vn(float *array_tar, const float *array_src, const float t, const int size) { - const float s = 1.0f - t; - float *tar = array_tar + (size - 1); - const float *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar) = (s * *(tar)) + (t * *(src)); - tar--; - src--; - } + const float s = 1.0f - t; + float *tar = array_tar + (size - 1); + const float *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar) = (s * *(tar)) + (t * *(src)); + tar--; + src--; + } } void copy_vn_i(int *array_tar, const int size, const int val) { - int *tar = array_tar + (size - 1); - int i = size; - while (i--) { - *(tar--) = val; - } + int *tar = array_tar + (size - 1); + int i = size; + while (i--) { + *(tar--) = val; + } } void copy_vn_short(short *array_tar, const int size, const short val) { - short *tar = array_tar + (size - 1); - int i = size; - while (i--) { - *(tar--) = val; - } + short *tar = array_tar + (size - 1); + int i = size; + while (i--) { + *(tar--) = val; + } } void copy_vn_ushort(unsigned short *array_tar, const int size, const unsigned short val) { - unsigned short *tar = array_tar + (size - 1); - int i = size; - while (i--) { - *(tar--) = val; - } + unsigned short *tar = array_tar + (size - 1); + int i = size; + while (i--) { + *(tar--) = val; + } } void copy_vn_uchar(unsigned char *array_tar, const int size, const unsigned char val) { - unsigned char *tar = array_tar + (size - 1); - int i = size; - while (i--) { - *(tar--) = val; - } + unsigned char *tar = array_tar + (size - 1); + int i = size; + while (i--) { + *(tar--) = val; + } } void copy_vn_fl(float *array_tar, const int size, const float val) { - float *tar = array_tar + (size - 1); - int i = size; - while (i--) { - *(tar--) = val; - } + float *tar = array_tar + (size - 1); + int i = size; + while (i--) { + *(tar--) = val; + } } /** \name Double precision versions 'db'. @@ -1280,32 +1361,35 @@ void copy_vn_fl(float *array_tar, const int size, const float val) void add_vn_vn_d(double *array_tar, const double *array_src, const int size) { - double *tar = array_tar + (size - 1); - const double *src = array_src + (size - 1); - int i = size; - while (i--) { - *(tar--) += *(src--); - } + double *tar = array_tar + (size - 1); + const double *src = array_src + (size - 1); + int i = size; + while (i--) { + *(tar--) += *(src--); + } } -void add_vn_vnvn_d(double *array_tar, const double *array_src_a, const double *array_src_b, const int size) +void add_vn_vnvn_d(double *array_tar, + const double *array_src_a, + const double *array_src_b, + const int size) { - double *tar = array_tar + (size - 1); - const double *src_a = array_src_a + (size - 1); - const double *src_b = array_src_b + (size - 1); - int i = size; - while (i--) { - *(tar--) = *(src_a--) + *(src_b--); - } + double *tar = array_tar + (size - 1); + const double *src_a = array_src_a + (size - 1); + const double *src_b = array_src_b + (size - 1); + int i = size; + while (i--) { + *(tar--) = *(src_a--) + *(src_b--); + } } void mul_vn_db(double *array_tar, const int size, const double f) { - double *array_pt = array_tar + (size - 1); - int i = size; - while (i--) { - *(array_pt--) *= f; - } + double *array_pt = array_tar + (size - 1); + int i = size; + while (i--) { + *(array_pt--) *= f; + } } /** \} */ |