/**
* equivalent to ``shell_angle_to_dist(angle_normalized_v3v3(a, b))``
*/
MINLINE float shell_v3v3_normalized_to_dist(const float a[3], const float b[3])
{
const float angle_cos = fabsf(dot_v3v3(a, b));
BLI_ASSERT_UNIT_V3(a);
BLI_ASSERT_UNIT_V3(b);
return (UNLIKELY(angle_cos < SMALL_NUMBER)) ? 1.0f : (1.0f / angle_cos);
}
/**
* equivalent to ``shell_angle_to_dist(angle_normalized_v3v3(a, b) / 2)``
*/
MINLINE float shell_v3v3_mid_normalized_to_dist(const float a[3], const float b[3])
{
float angle_cos;
float ab[3];
BLI_ASSERT_UNIT_V3(a);
BLI_ASSERT_UNIT_V3(b);
add_v3_v3v3(ab, a, b);
angle_cos = (normalize_v3(ab) != 0.0f) ? fabsf(dot_v3v3(a, ab)) : 0.0f;
return (UNLIKELY(angle_cos < SMALL_NUMBER)) ? 1.0f : (1.0f / angle_cos);
}
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);
/* 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);
}
}
/* adjust bone roll to align Z axis with vector
* vec is in local space and is normalized
*/
float ED_armature_ebone_roll_to_vector(const EditBone *bone, const float align_axis[3], const bool axis_only)
{
float mat[3][3], nor[3];
float vec[3], align_axis_proj[3], roll = 0.0f;
BLI_ASSERT_UNIT_V3(align_axis);
sub_v3_v3v3(nor, bone->tail, bone->head);
/* If tail == head or the bone is aligned with the axis... */
if (normalize_v3(nor) <= FLT_EPSILON || (fabsf(dot_v3v3(align_axis, nor)) >= (1.0f - FLT_EPSILON))) {
return roll;
}
vec_roll_to_mat3_normalized(nor, 0.0f, mat);
/* project the new_up_axis along the normal */
project_v3_v3v3_normalized(vec, align_axis, nor);
sub_v3_v3v3(align_axis_proj, align_axis, vec);
if (axis_only) {
if (angle_v3v3(align_axis_proj, mat[2]) > (float)(M_PI_2)) {
negate_v3(align_axis_proj);
}
}
roll = angle_v3v3(align_axis_proj, mat[2]);
cross_v3_v3v3(vec, mat[2], align_axis_proj);
if (dot_v3v3(vec, nor) < 0.0f) {
return -roll;
}
return roll;
}
/**
* Specialized function for calculating normals.
* fastpath for:
*
* \code{.c}
* add_v3_v3v3(r, a, b);
* normalize_v3(r)
* mul_v3_fl(r, angle_normalized_v3v3(a, b) / M_PI_2);
* \endcode
*
* We can use the length of (a + b) to calculate the angle.
*/
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;
/* 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);
}
/**
* Returns a reflection vector from a vector and a normal vector
* reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror)
*/
void reflect_v3_v3v3(float out[3], const float vec[3], const float normal[3])
{
const float dot2 = 2.0f * dot_v3v3(vec, normal);
BLI_ASSERT_UNIT_V3(normal);
out[0] = vec[0] - (dot2 * normal[0]);
out[1] = vec[1] - (dot2 * normal[1]);
out[2] = vec[2] - (dot2 * normal[2]);
}
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);
/* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
if (dot_v3v3(v1, v2) < 0.0f) {
float vec[3];
vec[0] = -v2[0];
vec[1] = -v2[1];
vec[2] = -v2[2];
return (float)M_PI - 2.0f * (float)saasin(len_v3v3(vec, v1) / 2.0f);
}
else
return 2.0f * (float)saasin(len_v3v3(v2, v1) / 2.0f);
}
/**
* 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;
}
/* Rotate a point p by angle theta around an arbitrary axis r
* http://local.wasp.uwa.edu.au/~pbourke/geometry/
*/
void rotate_normalized_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
{
const float costheta = cosf(angle);
const float sintheta = sinf(angle);
/* double check they are normalized */
BLI_ASSERT_UNIT_V3(axis);
r[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]);
r[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]);
r[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]);
}
