diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_vector.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_vector.c | 321 |
1 files changed, 179 insertions, 142 deletions
diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c index 63094d9cfd0..65600a31042 100644 --- a/source/blender/blenlib/intern/math_vector.c +++ b/source/blender/blenlib/intern/math_vector.c @@ -17,7 +17,7 @@ * * 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 ***** @@ -35,78 +35,78 @@ void interp_v2_v2v2(float target[2], const float a[2], const float b[2], const float t) { - float s = 1.0f-t; + 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]) { - 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) { - float s = 1.0f-t; + 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) { - float s = 1.0f-t; + 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]; } /* 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]; + 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]; + 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]; + 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]; + 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 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]); } /********************************** Angles ***********************************/ @@ -145,12 +145,12 @@ 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]; - + 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); @@ -185,15 +185,15 @@ float angle_normalized_v3v3(const float v1[3], const float v2[3]) /* 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); + + 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); + return 2.0f * (float)saasin(len_v3v3(v2, v1) / 2.0f); } float angle_normalized_v2v2(const float v1[2], const float v2[2]) @@ -201,14 +201,14 @@ float angle_normalized_v2v2(const float v1[2], const float v2[2]) /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */ if (dot_v2v2(v1, v2) < 0.0f) { float vec[2]; - - vec[0]= -v2[0]; - vec[1]= -v2[1]; - - return (float)M_PI - 2.0f*saasin(len_v2v2(vec, v1)/2.0f); + + vec[0] = -v2[0]; + vec[1] = -v2[1]; + + return (float)M_PI - 2.0f * saasin(len_v2v2(vec, v1) / 2.0f); } else - return 2.0f*(float)saasin(len_v2v2(v2, v1)/2.0f); + return 2.0f * (float)saasin(len_v2v2(v2, v1) / 2.0f); } void angle_tri_v3(float angles[3], const float v1[3], const float v2[3], const float v3[3]) @@ -223,10 +223,10 @@ void angle_tri_v3(float angles[3], const float v1[3], const float v2[3], const f 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); + 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[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]) @@ -243,10 +243,10 @@ void angle_quad_v3(float angles[4], const float v1[3], const float v2[3], const 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) @@ -254,12 +254,12 @@ 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]); + 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[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]); } } @@ -280,12 +280,26 @@ void project_v3_v3v3(float c[3], const float v1[3], const float v2[3]) { float mul; mul = dot_v3v3(v1, v2) / dot_v3v3(v2, v2); - + c[0] = mul * v2[0]; c[1] = mul * v2[1]; c[2] = mul * v2[2]; } +/* project a vector on a plane defined by normal and a plane point p */ +void project_v3_plane(float v[3], const float n[3], const float p[3]) +{ + float vector[3]; + float mul; + + sub_v3_v3v3(vector, v, p); + mul = dot_v3v3(vector, n) / len_squared_v3(n); + + mul_v3_v3fl(vector, n, mul); + + sub_v3_v3(v, 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]) { @@ -321,7 +335,7 @@ void reflect_v3_v3v3(float out[3], const float v1[3], const float v2[3]) void ortho_basis_v3v3_v3(float v1[3], float v2[3], const float v[3]) { - const float f = (float)sqrt(v[0]*v[0] + v[1]*v[1]); + const float f = (float)sqrt(v[0] * v[0] + v[1] * v[1]); if (f < 1e-35f) { // degenerate case @@ -329,15 +343,15 @@ void ortho_basis_v3v3_v3(float v1[3], float v2[3], const float v[3]) v1[1] = v1[2] = v2[0] = v2[2] = 0.0f; v2[1] = 1.0f; } - else { - const float d= 1.0f/f; + else { + const float d = 1.0f / f; - v1[0] = v[1]*d; - v1[1] = -v[0]*d; + v1[0] = v[1] * d; + v1[1] = -v[0] * d; v1[2] = 0.0f; - v2[0] = -v[2]*v1[1]; - v2[1] = v[2]*v1[0]; - v2[2] = v[0]*v1[1] - v[1]*v1[0]; + v2[0] = -v[2] * v1[1]; + v2[1] = v[2] * v1[0]; + v2[2] = v[0] * v1[1] - v[1] * v1[0]; } } @@ -346,20 +360,20 @@ void ortho_basis_v3v3_v3(float v1[3], float v2[3], const float v[3]) */ void rotate_normalized_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle) { - const float costheta= cos(angle); - const float sintheta= sin(angle); + const float costheta = cos(angle); + const float sintheta = sin(angle); - 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[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[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]); + 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]); } void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle) @@ -390,39 +404,40 @@ void print_v4(const char *str, const float v[4]) 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 (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 (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]; } - /***************************** Array Functions *******************************/ 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 += *(array_pt_a--) * *(array_pt_b--); } + 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 += *(array_pt_a--) * *(array_pt_b--); + } return d; } float normalize_vn_vn(float *array_tar, const float *array_src, const int size) { - double d= dot_vn_vn(array_tar, array_src, size); + double d = dot_vn_vn(array_tar, 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); + d_sqrt = (float)sqrt(d); + mul_vn_vn_fl(array_tar, array_src, size, 1.0f / d_sqrt); } else { fill_vn_fl(array_tar, size, 0.0f); - d_sqrt= 0.0f; + d_sqrt = 0.0f; } return d_sqrt; } @@ -434,16 +449,18 @@ float normalize_vn(float *array_tar, const int 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_fl(float *array_tar, const int size, const float start, const float step) { - float *array_pt= array_tar + (size-1); - int i= size; + float *array_pt = array_tar + (size - 1); + int i = size; while (i--) { *(array_pt--) = start + step * (float)(i); } @@ -451,78 +468,98 @@ void range_vn_fl(float *array_tar, const int size, const float start, const floa 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_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) { - 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 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) { - 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 fill_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 fill_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; + } } |