/** * 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]); }