archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity
This repository has been archived on 2023-10-09. You can view files and clone it, but cannot push or open issues or pull requests.
Files
ccdacf1c9b31b15e188aa9e9adb044ffd0ca0da4
blender-archive/source/blender/blenlib/intern/math_vector.c
Sergey Sharybin bb3efe6127 Blenlib: Assert when attempting to rotate point around vector and store result in the point 
This isn't supported since there are subsequent reads to all point coordinates
after modification started.

Probably we need to create a temp copy of point, but that's like extra CPU
ticks.
2018-02-15 11:53:57 +01:00

1294 lines
32 KiB
C
Raw Blame History

/*
* ***** BEGIN GPL LICENSE BLOCK *****
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: some of this file.
*
* ***** END GPL LICENSE BLOCK *****
* */
/** \file blender/blenlib/intern/math_vector.c
* \ingroup bli
*/
#include "BLI_math.h"
#include "BLI_strict_flags.h"
//******************************* Interpolation *******************************/
void interp_v2_v2v2(float target[2], const float a[2], const float b[2], const float t)
{
const float s = 1.0f - t;
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])
{
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;
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;
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];
}
/**
* 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 but 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);
}
}
}
}
/** \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)
{
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(r0, q0, q1, u);
interp_v2_v2v2(r1, q1, q2, 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])
{
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])
{
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])
{
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])
{
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])
{
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)
{
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]);
}
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);
}
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;
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);
}
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]);
}
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]);
}
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;
}
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;
}
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);
for (unsigned int i = 0; i < nbr; i++) {
madd_v3_v3fl(r, vec_arr[i], factor);
}
}
/**
* 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);
}
/**
* Same as mid_v3_v3v3_angle_weighted
* but \a r is assumed to be accumulated normals, divided by their total.
*/
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;
/* 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);
}
/**
* Equivalent to:
* interp_v3_v3v3(v, v1, v2, -1.0f);
*/
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]);
}
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]);
}
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]);
}
/********************************* Comparison ********************************/
bool is_finite_v2(const float v[2])
{
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]));
}
bool is_finite_v4(const float v[4])
{
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
* If v1 is a shoulder, v2 is the elbow and v3 is the hand,
* this would return the angle at the elbow.
*
* note that when v1/v2/v3 represent 3 points along a straight line
* that the angle returned will be pi (180deg), rather then 0.0
*/
float angle_v3v3v3(const float v1[3], const float v2[3], const float v3[3])
{
float vec1[3], vec2[3];
sub_v3_v3v3(vec1, v2, v1);
sub_v3_v3v3(vec2, v2, v3);
normalize_v3(vec1);
normalize_v3(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];
sub_v3_v3v3(vec1, p2, p1);
sub_v3_v3v3(vec2, p2, p3);
normalize_v3(vec1);
normalize_v3(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];
normalize_v3_v3(vec1, v1);
normalize_v3_v3(vec2, v2);
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];
vec1[0] = v2[0] - v1[0];
vec1[1] = v2[1] - v1[1];
vec2[0] = v2[0] - v3[0];
vec2[1] = v2[1] - v3[1];
normalize_v2(vec1);
normalize_v2(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];
sub_v2_v2v2(vec1, p2, p1);
sub_v2_v2v2(vec2, p2, p3);
normalize_v2(vec1);
normalize_v2(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];
vec1[0] = v1[0];
vec1[1] = v1[1];
vec2[0] = v2[0];
vec2[1] = v2[1];
normalize_v2(vec1);
normalize_v2(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));
}
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);
}
}
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);
/* 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);
}
}
/**
* Angle between 2 vectors, about an axis (axis can be considered a plane).
*/
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];
/* 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);
}
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;
/* 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);
/* 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;
}
/**
* 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 vec1[3], vec2[3];
sub_v3_v3v3(vec1, v1, v2);
sub_v3_v3v3(vec2, v3, v2);
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 vec1[3], vec2[3];
sub_v3_v3v3(vec1, v1, v2);
sub_v3_v3v3(vec2, v3, v2);
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];
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);
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])
{
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);
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);
}
void angle_poly_v3(float *angles, const float *verts[3], int len)
{
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]);
}
}
/********************************* Geometry **********************************/
/**
* Project \a p onto \a v_proj
*/
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);
out[0] = mul * v_proj[0];
out[1] = mul * v_proj[1];
}
/**
* Project \a p onto \a v_proj
*/
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);
out[0] = mul * v_proj[0];
out[1] = mul * v_proj[1];
out[2] = mul * v_proj[2];
}
/**
* Project \a p onto a unit length \a v_proj
*/
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);
out[0] = mul * v_proj[0];
out[1] = mul * v_proj[1];
}
/**
* Project \a p onto a unit length \a v_proj
*/
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);
out[0] = mul * v_proj[0];
out[1] = mul * v_proj[1];
out[2] = mul * v_proj[2];
}
/**
* In this case plane is a 3D vector only (no 4th component).
*
* Projecting will make \a c a copy of \a v orthogonal to \a v_plane.
*
* \note If \a v is exactly perpendicular to \a v_plane, \a c will just be a copy of \a v.
*
* \note This function is a convenience to call:
* \code{.c}
* project_v3_v3v3(c, v, v_plane);
* sub_v3_v3v3(c, v, c);
* \endcode
*/
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);
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);
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);
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);
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;
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);
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);
}
/**
* Returns a reflection vector from a vector and a normal vector
* reflect = vec - ((2 * dot(vec, mirror)) * mirror).
*
* <pre>
* v
* + ^
* \ |
* \|
* + normal: axis of reflection
* /
* /
* +
* out: result (negate for a 'bounce').
* </pre>
*/
void reflect_v3_v3v3(float out[3], const float v[3], const float normal[3])
{
const float dot2 = 2.0f * dot_v3v3(v, 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]);
}
/**
* Takes a vector and computes 2 orthogonal directions.
*
* \note if \a n is n unit length, computed values will be too.
*/
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);
if (f > eps) {
const float d = 1.0f / sqrtf(f);
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;
}
}
/**
* Calculates \a p - a perpendicular vector to \a v
*
* \note return vector won't maintain same length.
*/
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;
}
}
/**
* no brainer compared to v3, just have for consistency.
*/
void ortho_v2_v2(float out[2], const float v[2])
{
BLI_assert(out != v);
out[0] = -v[1];
out[1] = v[0];
}
/**
* Rotate a point \a p by \a angle around origin (0, 0)
*/
void rotate_v2_v2fl(float r[2], const float p[2], const float angle)
{
const float co = cosf(angle);
const float si = sinf(angle);
BLI_assert(r != p);
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)
{
const float costheta = cosf(angle);
const float sintheta = sinf(angle);
/* 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[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]);
}
void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
{
BLI_assert(r != p);
float axis_n[3];
normalize_v3_v3(axis_n, axis);
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]);
}
void print_v3(const char *str, const float v[3])
{
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]);
}
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");
}
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];
}
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 (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++);
}
}
/** 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];
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];
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); }
#undef SWAP_AXIS
}
/***************************** Array Functions *******************************/
MINLINE double sqr_db(double 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 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;
}
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;
}
float normalize_vn(float *array_tar, const int 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--;
}
}
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--;
}
}
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);
}
}
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;
}
}
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--);
}
}
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;
}
}
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;
}
}
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--);
}
}
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--);
}
}
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;
}
}
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);
}
}
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--);
}
}
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--);
}
}
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;
}
}
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);
}
}
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--;
}
}
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;
}
}
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;
}
}
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;
}
}
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;
}
}
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;
}
}
/** \name Double precision versions 'db'.
* \{ */
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--);
}
}
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--);
}
}
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;
}
}
/** \} */
View Git Blame Copy Permalink