Math Lib
* Fix remaining issues before conversion. * Inline various vector functions, currently enabled for all platforms. I expect this to work in GCC/MSVC at least, if other platforms don't support it, #ifdef's can be added.
This commit is contained in:
Brecht Van Lommel
parent
d611dd3735
commit
385875632d
@ -43,7 +43,7 @@ float normal_quad_v3(float r[3], float a[3], float b[3], float c[3], float d[3])
|
||||
float area_tri_v2(float a[2], float b[2], float c[2]);
|
||||
float area_tri_v3(float a[3], float b[3], float c[3]);
|
||||
float area_quad_v3(float a[3], float b[3], float c[3], float d[3]);
|
||||
float area_poly_v3(int nr, float *verts, float normal[3]); // TODO float verts[][3]
|
||||
float area_poly_v3(int nr, float verts[][3], float normal[3]);
|
||||
|
||||
/********************************* Distance **********************************/
|
||||
|
||||
@ -56,7 +56,6 @@ void closest_to_line_segment_v3(float r[3], float p[3], float l1[3], float l2[3]
|
||||
|
||||
/******************************* Intersection ********************************/
|
||||
|
||||
/* TODO return values are not always first yet */
|
||||
/* TODO int return value consistency */
|
||||
|
||||
/* line-line */
|
||||
@ -65,8 +64,8 @@ void closest_to_line_segment_v3(float r[3], float p[3], float l1[3], float l2[3]
|
||||
#define ISECT_LINE_LINE_EXACT 1
|
||||
#define ISECT_LINE_LINE_CROSS 2
|
||||
|
||||
short isect_line_line_v2(float a1[2], float a2[2], float b1[2], float b2[2]); // TODO return int
|
||||
short isect_line_line_v2_short(short a1[2], short a2[2], short b1[2], short b2[2]); // TODO return int
|
||||
int isect_line_line_v2(float a1[2], float a2[2], float b1[2], float b2[2]);
|
||||
int isect_line_line_v2_short(short a1[2], short a2[2], short b1[2], short b2[2]);
|
||||
|
||||
/* Returns the number of point of interests
|
||||
* 0 - lines are colinear
|
||||
|
||||
@ -43,8 +43,8 @@ extern "C" {
|
||||
{ 0.0, 1.0, 0.0},\
|
||||
{ 0.0, 0.0, 1.0}}
|
||||
|
||||
void zero_m3(float *R); // TODO R[3][3]);
|
||||
void zero_m4(float *R); // TODO R[4][4]);
|
||||
void zero_m3(float R[3][3]);
|
||||
void zero_m4(float R[4][4]);
|
||||
|
||||
void unit_m3(float R[3][3]);
|
||||
void unit_m4(float R[4][4]);
|
||||
@ -62,8 +62,6 @@ void swap_m4m4(float A[4][4], float B[4][4]);
|
||||
void add_m3_m3m3(float R[3][3], float A[3][3], float B[3][3]);
|
||||
void add_m4_m4m4(float R[4][4], float A[4][4], float B[4][4]);
|
||||
|
||||
// TODO review mul order
|
||||
|
||||
void mul_m3_m3m3(float R[3][3], float A[3][3], float B[3][3]);
|
||||
void mul_m4_m4m4(float R[4][4], float A[4][4], float B[4][4]);
|
||||
void mul_m4_m3m4(float R[4][4], float A[3][3], float B[4][4]);
|
||||
@ -77,24 +75,24 @@ void mul_serie_m4(float R[4][4],
|
||||
float M1[4][4], float M2[4][4], float M3[4][4], float M4[4][4],
|
||||
float M5[4][4], float M6[4][4], float M7[4][4], float M8[4][4]);
|
||||
|
||||
void mul_m4_v3(float M[4][4], float r[3]); // TODO order
|
||||
void mul_m4_v3(float M[4][4], float r[3]);
|
||||
void mul_v3_m4v3(float r[3], float M[4][4], float v[3]);
|
||||
void mul_no_transl_m4v3(float M[4][4], float r[3]);
|
||||
void mul_m4_v4(float M[4][4], float r[3]); // TODO order
|
||||
void mul_project_m4_v4(float M[4][4], float r[3]); // TODO order
|
||||
void mul_mat3_m4_v3(float M[4][4], float r[3]);
|
||||
void mul_m4_v4(float M[4][4], float r[3]);
|
||||
void mul_project_m4_v4(float M[4][4], float r[3]);
|
||||
|
||||
void mul_m3_v3(float M[3][3], float r[3]); // TODO order
|
||||
void mul_transposed_m3_v3(float M[3][3], float r[3]); // TODO order
|
||||
void mul_m3_v3(float M[3][3], float r[3]);
|
||||
void mul_transposed_m3_v3(float M[3][3], float r[3]);
|
||||
void mul_m3_v3_double(float M[3][3], double r[3]);
|
||||
|
||||
void mul_m3_fl(float *R, float f); // TODO R[3][3], float f);
|
||||
void mul_m4_fl(float *R, float f); // TODO R[4][4], float f);
|
||||
void mul_no_transl_m4_fl(float *R, float f); // TODO R[4][4], float f);
|
||||
void mul_m3_fl(float R[3][3], float f);
|
||||
void mul_m4_fl(float R[4][4], float f);
|
||||
void mul_mat3_m4_fl(float R[4][4], float f);
|
||||
|
||||
void invert_m3(float R[3][3]);
|
||||
void invert_m3_m3(float R[3][3], float A[3][3]);
|
||||
void invert_m4(float R[4][4]); // TODO does not exist
|
||||
int invert_m4_m4(float R[4][4], float A[4][4]); // TODO return int
|
||||
int invert_m3(float R[3][3]);
|
||||
int invert_m3_m3(float R[3][3], float A[3][3]);
|
||||
int invert_m4(float R[4][4]);
|
||||
int invert_m4_m4(float R[4][4], float A[4][4]);
|
||||
|
||||
/****************************** Linear Algebra *******************************/
|
||||
|
||||
@ -113,9 +111,6 @@ int is_orthogonal_m4(float mat[4][4]);
|
||||
void adjoint_m3_m3(float R[3][3], float A[3][3]);
|
||||
void adjoint_m4_m4(float R[4][4], float A[4][4]);
|
||||
|
||||
//float determinant_m2(float A[2][2]); // TODO params
|
||||
//float determinant_m3(float A[3][3]); // TODO params
|
||||
|
||||
float determinant_m2(
|
||||
float a, float b,
|
||||
float c, float d);
|
||||
|
||||
@ -44,7 +44,7 @@ void copy_qt_qt(float q[4], float a[4]);
|
||||
|
||||
/* arithmetic */
|
||||
void mul_qt_qtqt(float q[4], float a[4], float b[4]);
|
||||
void mul_qt_v3(float q[4], float r[3]); // TODO order
|
||||
void mul_qt_v3(float q[4], float r[3]);
|
||||
void mul_qt_fl(float q[4], float f);
|
||||
void mul_fac_qt_fl(float q[4], float f);
|
||||
|
||||
@ -60,7 +60,7 @@ int is_zero_qt(float q[4]);
|
||||
|
||||
/* interpolation */
|
||||
void interp_qt_qtqt(float q[4], float a[4], float b[4], float t);
|
||||
void add_qt_qtqt(float q[4], float a[4], float b[4], float t); // TODO name
|
||||
void add_qt_qtqt(float q[4], float a[4], float b[4], float t);
|
||||
|
||||
/* conversion */
|
||||
void quat_to_mat3(float mat[3][3], float q[4]);
|
||||
@ -72,8 +72,6 @@ void tri_to_quat(float q[4], float a[3], float b[3], float c[3]);
|
||||
void vec_to_quat(float q[4], float vec[3], short axis, short upflag);
|
||||
void rotation_between_vecs_to_quat(float q[4], float v1[3], float v2[3]);
|
||||
|
||||
void Mat3ToQuat_is_ok(float wmat[][3], float *q); // TODO what is this?
|
||||
|
||||
/* other */
|
||||
void print_qt(char *str, float q[4]);
|
||||
|
||||
@ -96,7 +94,7 @@ void mat4_to_axis_angle(float axis[3], float *angle, float M[4][4]);
|
||||
void mat3_to_vec_rot(float vec[3], float *phi, float mat[3][3]);
|
||||
void mat4_to_vec_rot(float vec[3], float *phi, float mat[4][4]);
|
||||
|
||||
void vec_rot_to_quat(float quat[4], float vec[3], float phi); // TODO
|
||||
void vec_rot_to_quat(float quat[4], float vec[3], float phi);
|
||||
void vec_rot_to_mat3(float mat[3][3], float vec[3], float phi);
|
||||
void vec_rot_to_mat4(float mat[4][4], float vec[3], float phi);
|
||||
|
||||
|
||||
@ -32,64 +32,73 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define MINLINE
|
||||
/* add platform/compiler checks here if it is not supported */
|
||||
#define BLI_MATH_INLINE
|
||||
|
||||
//#define static inline
|
||||
//#include "intern/math_vector_inline.h"
|
||||
#ifdef BLI_MATH_INLINE
|
||||
#ifdef _MSC_VER
|
||||
#define MINLINE static __inline
|
||||
#else
|
||||
#define MINLINE static inline
|
||||
#endif
|
||||
#include "intern/math_vector_inline.c"
|
||||
#else
|
||||
#define MINLINE
|
||||
#endif
|
||||
|
||||
/************************************* Init ***********************************/
|
||||
|
||||
void zero_v2(float r[2]);
|
||||
void zero_v3(float r[3]);
|
||||
MINLINE void zero_v2(float r[2]);
|
||||
MINLINE void zero_v3(float r[3]);
|
||||
|
||||
void copy_v2_v2(float r[2], float a[2]);
|
||||
void copy_v3_v3(float r[3], float a[3]);
|
||||
MINLINE void copy_v2_v2(float r[2], float a[2]);
|
||||
MINLINE void copy_v3_v3(float r[3], float a[3]);
|
||||
|
||||
/********************************* Arithmetic ********************************/
|
||||
|
||||
void add_v2_v2(float r[2], float a[2]);
|
||||
void add_v2_v2v2(float r[2], float a[2], float b[2]);
|
||||
void add_v3_v3(float r[3], float a[3]);
|
||||
void add_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
MINLINE void add_v2_v2(float r[2], float a[2]);
|
||||
MINLINE void add_v2_v2v2(float r[2], float a[2], float b[2]);
|
||||
MINLINE void add_v3_v3(float r[3], float a[3]);
|
||||
MINLINE void add_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
|
||||
void sub_v2_v2(float r[2], float a[2]);
|
||||
void sub_v2_v2v2(float r[2], float a[2], float b[2]);
|
||||
void sub_v3_v3(float r[3], float a[3]);
|
||||
void sub_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
MINLINE void sub_v2_v2(float r[2], float a[2]);
|
||||
MINLINE void sub_v2_v2v2(float r[2], float a[2], float b[2]);
|
||||
MINLINE void sub_v3_v3(float r[3], float a[3]);
|
||||
MINLINE void sub_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
|
||||
void mul_v2_fl(float r[2], float f);
|
||||
void mul_v3_fl(float r[3], float f);
|
||||
void mul_v3_v3fl(float r[3], float a[3], float f);
|
||||
void mul_v3_v3(float r[3], float a[3]);
|
||||
void mul_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
MINLINE void mul_v2_fl(float r[2], float f);
|
||||
MINLINE void mul_v3_fl(float r[3], float f);
|
||||
MINLINE void mul_v3_v3fl(float r[3], float a[3], float f);
|
||||
MINLINE void mul_v3_v3(float r[3], float a[3]);
|
||||
MINLINE void mul_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
|
||||
void negate_v3(float r[3]);
|
||||
void negate_v3_v3(float r[3], float a[3]);
|
||||
MINLINE void negate_v3(float r[3]);
|
||||
MINLINE void negate_v3_v3(float r[3], float a[3]);
|
||||
|
||||
float dot_v2v2(float a[2], float b[2]);
|
||||
float dot_v3v3(float a[3], float b[3]);
|
||||
MINLINE float dot_v2v2(float a[2], float b[2]);
|
||||
MINLINE float dot_v3v3(float a[3], float b[3]);
|
||||
|
||||
float cross_v2v2(float a[2], float b[2]);
|
||||
void cross_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
MINLINE float cross_v2v2(float a[2], float b[2]);
|
||||
MINLINE void cross_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
|
||||
void star_m3_v3(float R[3][3],float a[3]);
|
||||
MINLINE void star_m3_v3(float R[3][3],float a[3]);
|
||||
|
||||
/*********************************** Length **********************************/
|
||||
|
||||
float len_v2(float a[2]);
|
||||
float len_v2v2(float a[2], float b[2]);
|
||||
float len_v3(float a[3]);
|
||||
float len_v3v3(float a[3], float b[3]);
|
||||
MINLINE float len_v2(float a[2]);
|
||||
MINLINE float len_v2v2(float a[2], float b[2]);
|
||||
MINLINE float len_v3(float a[3]);
|
||||
MINLINE float len_v3v3(float a[3], float b[3]);
|
||||
|
||||
float normalize_v2(float r[2]);
|
||||
float normalize_v3(float r[3]);
|
||||
MINLINE float normalize_v2(float r[2]);
|
||||
MINLINE float normalize_v3(float r[3]);
|
||||
|
||||
/******************************* Interpolation *******************************/
|
||||
|
||||
void interp_v2_v2v2(float r[2], const float a[2], const float b[2], const float t); // TODO const
|
||||
void interp_v2_v2v2v2(float r[2], const float a[2], const float b[2], const float c[3], const float t[3]); // TODO const
|
||||
void interp_v3_v3v3(float r[3], const float a[3], const float b[3], const float t); // TODO const
|
||||
void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3]); // TODO const
|
||||
void interp_v2_v2v2(float r[2], float a[2], float b[2], float t);
|
||||
void interp_v2_v2v2v2(float r[2], float a[2], float b[2], float c[3], float t[3]);
|
||||
void interp_v3_v3v3(float r[3], float a[3], float b[3], float t);
|
||||
void interp_v3_v3v3v3(float p[3], float v1[3], float v2[3], float v3[3], float w[3]);
|
||||
|
||||
void mid_v3_v3v3(float r[3], float a[3], float b[3]);
|
||||
|
||||
|
||||
@ -124,7 +124,7 @@ float area_tri_v3(float *v1, float *v2, float *v3) /* Triangles */
|
||||
#define MAX3(x,y,z) MAX2(MAX2((x),(y)) , (z))
|
||||
|
||||
|
||||
float area_poly_v3(int nr, float *verts, float *normal)
|
||||
float area_poly_v3(int nr, float verts[][3], float *normal)
|
||||
{
|
||||
float x, y, z, area, max;
|
||||
float *cur, *prev;
|
||||
@ -142,13 +142,13 @@ float area_poly_v3(int nr, float *verts, float *normal)
|
||||
}
|
||||
|
||||
/* The Trapezium Area Rule */
|
||||
prev= verts+3*(nr-1);
|
||||
cur= verts;
|
||||
prev= verts[nr-1];
|
||||
cur= verts[0];
|
||||
area= 0;
|
||||
for(a=0; a<nr; a++) {
|
||||
area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
|
||||
prev= cur;
|
||||
cur+=3;
|
||||
prev= verts[a];
|
||||
cur= verts[a+1];
|
||||
}
|
||||
|
||||
return (float)fabs(0.5*area/max);
|
||||
@ -232,7 +232,7 @@ float dist_to_line_segment_v3(float *v1, float *v2, float *v3)
|
||||
/******************************* Intersection ********************************/
|
||||
|
||||
/* intersect Line-Line, shorts */
|
||||
short isect_line_line_v2_short(short *v1, short *v2, short *v3, short *v4)
|
||||
int isect_line_line_v2_short(short *v1, short *v2, short *v3, short *v4)
|
||||
{
|
||||
/* return:
|
||||
-1: colliniar
|
||||
@ -257,7 +257,7 @@ short isect_line_line_v2_short(short *v1, short *v2, short *v3, short *v4)
|
||||
}
|
||||
|
||||
/* intersect Line-Line, floats */
|
||||
short isect_line_line_v2(float *v1, float *v2, float *v3, float *v4)
|
||||
int isect_line_line_v2(float *v1, float *v2, float *v3, float *v4)
|
||||
{
|
||||
/* return:
|
||||
-1: colliniar
|
||||
|
||||
@ -34,12 +34,12 @@
|
||||
|
||||
/********************************* Init **************************************/
|
||||
|
||||
void zero_m3(float *m)
|
||||
void zero_m3(float m[3][3])
|
||||
{
|
||||
memset(m, 0, 3*3*sizeof(float));
|
||||
}
|
||||
|
||||
void zero_m4(float *m)
|
||||
void zero_m4(float m[4][4])
|
||||
{
|
||||
memset(m, 0, 4*4*sizeof(float));
|
||||
}
|
||||
@ -183,6 +183,7 @@ void mul_m4_m4m3(float (*m1)[4], float (*m3)[4], float (*m2)[3])
|
||||
m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
|
||||
m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
|
||||
}
|
||||
|
||||
/* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
|
||||
void mul_m3_m3m4(float m1[][3], float m2[][3], float m3[][4])
|
||||
{
|
||||
@ -200,8 +201,6 @@ void mul_m3_m3m4(float m1[][3], float m2[][3], float m3[][4])
|
||||
m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
|
||||
{
|
||||
m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
|
||||
@ -223,7 +222,6 @@ void mul_serie_m3(float answ[][3],
|
||||
float temp[3][3];
|
||||
|
||||
if(m1==0 || m2==0) return;
|
||||
|
||||
|
||||
mul_m3_m3m3(answ, m2, m1);
|
||||
if(m3) {
|
||||
@ -304,7 +302,7 @@ void mul_v3_m4v3(float *in, float mat[][4], float *vec)
|
||||
in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
|
||||
}
|
||||
|
||||
void mul_no_transl_m4v3(float mat[][4], float *vec)
|
||||
void mul_mat3_m4_v3(float mat[][4], float *vec)
|
||||
{
|
||||
float x,y;
|
||||
|
||||
@ -362,30 +360,31 @@ void mul_transposed_m3_v3(float mat[][3], float *vec)
|
||||
vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
|
||||
}
|
||||
|
||||
void mul_m3_fl(float *m, float f)
|
||||
void mul_m3_fl(float m[3][3], float f)
|
||||
{
|
||||
int i;
|
||||
int i, j;
|
||||
|
||||
for(i=0;i<9;i++) m[i]*=f;
|
||||
for(i=0;i<3;i++)
|
||||
for(j=0;j<3;j++)
|
||||
m[i][j] *= f;
|
||||
}
|
||||
|
||||
void mul_m4_fl(float *m, float f)
|
||||
void mul_m4_fl(float m[4][4], float f)
|
||||
{
|
||||
int i;
|
||||
int i, j;
|
||||
|
||||
for(i=0;i<16;i++) m[i]*=f; /* count to 12: without vector component */
|
||||
for(i=0;i<4;i++)
|
||||
for(j=0;j<4;j++)
|
||||
m[i][j] *= f;
|
||||
}
|
||||
|
||||
void mul_no_transl_m4_fl(float *m, float f) /* only scale component */
|
||||
void mul_mat3_m4_fl(float m[4][4], float f)
|
||||
{
|
||||
int i,j;
|
||||
int i, j;
|
||||
|
||||
for(i=0; i<3; i++) {
|
||||
for(j=0; j<3; j++) {
|
||||
|
||||
m[4*i+j] *= f;
|
||||
}
|
||||
}
|
||||
for(i=0; i<3; i++)
|
||||
for(j=0; j<3; j++)
|
||||
m[i][j] *= f;
|
||||
}
|
||||
|
||||
void mul_m3_v3_double(float mat[][3], double *vec)
|
||||
@ -417,10 +416,21 @@ void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
|
||||
m1[i][j]= m2[i][j] + m3[i][j];
|
||||
}
|
||||
|
||||
void invert_m3_m3(float m1[][3], float m2[][3])
|
||||
int invert_m3(float m[3][3])
|
||||
{
|
||||
float tmp[3][3];
|
||||
int success;
|
||||
|
||||
success= invert_m3_m3(tmp, m);
|
||||
copy_m3_m3(m, tmp);
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int invert_m3_m3(float m1[3][3], float m2[3][3])
|
||||
{
|
||||
short a,b;
|
||||
float det;
|
||||
int a, b, success;
|
||||
|
||||
/* calc adjoint */
|
||||
adjoint_m3_m3(m1,m2);
|
||||
@ -430,6 +440,8 @@ void invert_m3_m3(float m1[][3], float m2[][3])
|
||||
-m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
|
||||
+m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
|
||||
|
||||
success= (det != 0);
|
||||
|
||||
if(det==0) det=1;
|
||||
det= 1/det;
|
||||
for(a=0;a<3;a++) {
|
||||
@ -437,6 +449,19 @@ void invert_m3_m3(float m1[][3], float m2[][3])
|
||||
m1[a][b]*=det;
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int invert_m4(float m[4][4])
|
||||
{
|
||||
float tmp[4][4];
|
||||
int success;
|
||||
|
||||
success= invert_m4_m4(tmp, m);
|
||||
copy_m4_m4(m, tmp);
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
/*
|
||||
@ -448,7 +473,7 @@ void invert_m3_m3(float m1[][3], float m2[][3])
|
||||
* Mark Segal - 1992
|
||||
*/
|
||||
|
||||
int invert_m4_m4(float inverse[][4], float mat[][4])
|
||||
int invert_m4_m4(float inverse[4][4], float mat[4][4])
|
||||
{
|
||||
int i, j, k;
|
||||
double temp;
|
||||
|
||||
@ -209,7 +209,7 @@ void quat_to_mat4(float m[][4], float *q)
|
||||
m[3][3]= 1.0f;
|
||||
}
|
||||
|
||||
void mat3_to_quat(float *q,float wmat[][3])
|
||||
void mat3_to_quat(float *q, float wmat[][3])
|
||||
{
|
||||
double tr, s;
|
||||
float mat[3][3];
|
||||
@ -257,74 +257,27 @@ void mat3_to_quat(float *q,float wmat[][3])
|
||||
q[2]= (float)((mat[2][1] + mat[1][2])*s);
|
||||
}
|
||||
}
|
||||
|
||||
normalize_qt(q);
|
||||
}
|
||||
|
||||
#if 0
|
||||
void Mat3ToQuat_is_ok(float wmat[][3], float *q)
|
||||
{
|
||||
float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
|
||||
|
||||
/* work on a copy */
|
||||
copy_m3_m3(mat, wmat);
|
||||
normalize_m3(mat);
|
||||
|
||||
/* rotate z-axis of matrix to z-axis */
|
||||
|
||||
nor[0] = mat[2][1]; /* cross product with (0,0,1) */
|
||||
nor[1] = -mat[2][0];
|
||||
nor[2] = 0.0;
|
||||
normalize_v3(nor);
|
||||
|
||||
co= mat[2][2];
|
||||
angle= 0.5f*saacos(co);
|
||||
|
||||
co= (float)cos(angle);
|
||||
si= (float)sin(angle);
|
||||
q1[0]= co;
|
||||
q1[1]= -nor[0]*si; /* negative here, but why? */
|
||||
q1[2]= -nor[1]*si;
|
||||
q1[3]= -nor[2]*si;
|
||||
|
||||
/* rotate back x-axis from mat, using inverse q1 */
|
||||
quat_to_mat3(matr,q1);
|
||||
invert_m3_m3(matn, matr);
|
||||
mul_m3_v3(matn, mat[0]);
|
||||
|
||||
/* and align x-axes */
|
||||
angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
|
||||
|
||||
co= (float)cos(angle);
|
||||
si= (float)sin(angle);
|
||||
q2[0]= co;
|
||||
q2[1]= 0.0f;
|
||||
q2[2]= 0.0f;
|
||||
q2[3]= si;
|
||||
|
||||
mul_qt_qtqt(q, q1, q2);
|
||||
}
|
||||
#endif
|
||||
|
||||
void mat4_to_quat(float *q, float m[][4])
|
||||
{
|
||||
float mat[3][3];
|
||||
|
||||
copy_m3_m4(mat, m);
|
||||
mat3_to_quat(q,mat);
|
||||
|
||||
}
|
||||
|
||||
void normalize_qt(float *q)
|
||||
{
|
||||
float len;
|
||||
|
||||
len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
|
||||
len= (float)sqrt(dot_qtqt(q, q));
|
||||
if(len!=0.0) {
|
||||
q[0]/= len;
|
||||
q[1]/= len;
|
||||
q[2]/= len;
|
||||
q[3]/= len;
|
||||
} else {
|
||||
mul_qt_fl(q, 1.0f/len);
|
||||
}
|
||||
else {
|
||||
q[1]= 1.0f;
|
||||
q[0]= q[2]= q[3]= 0.0f;
|
||||
}
|
||||
@ -1422,7 +1375,7 @@ void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight)
|
||||
weight= -weight;
|
||||
|
||||
copy_m4_m4(wmat, dq->scale);
|
||||
mul_m4_fl((float*)wmat, weight);
|
||||
mul_m4_fl(wmat, weight);
|
||||
add_m4_m4m4(dqsum->scale, dqsum->scale, wmat);
|
||||
dqsum->scale_weight += weight;
|
||||
}
|
||||
@ -1445,7 +1398,7 @@ void normalize_dq(DualQuat *dq, float totweight)
|
||||
dq->scale[3][3] += addweight;
|
||||
}
|
||||
|
||||
mul_m4_fl((float*)dq->scale, scale);
|
||||
mul_m4_fl(dq->scale, scale);
|
||||
dq->scale_weight= 1.0f;
|
||||
}
|
||||
}
|
||||
@ -1496,7 +1449,7 @@ void mul_v3m3_dq(float *co, float mat[][3],DualQuat *dq)
|
||||
}
|
||||
else
|
||||
copy_m3_m3(mat, M);
|
||||
mul_m3_fl((float*)mat, len2);
|
||||
mul_m3_fl(mat, len2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -32,234 +32,11 @@
|
||||
|
||||
#include "BLI_math.h"
|
||||
|
||||
/********************************** Init *************************************/
|
||||
//******************************* Interpolation *******************************/
|
||||
|
||||
void zero_v2(float r[2])
|
||||
void interp_v2_v2v2(float *target, float *a, float *b, float t)
|
||||
{
|
||||
r[0]= 0.0f;
|
||||
r[1]= 0.0f;
|
||||
}
|
||||
|
||||
void zero_v3(float r[3])
|
||||
{
|
||||
r[0]= 0.0f;
|
||||
r[1]= 0.0f;
|
||||
r[2]= 0.0f;
|
||||
}
|
||||
|
||||
void copy_v2_v2(float r[2], float a[2])
|
||||
{
|
||||
r[0]= a[0];
|
||||
r[1]= a[1];
|
||||
}
|
||||
|
||||
void copy_v3_v3(float r[3], float a[3])
|
||||
{
|
||||
r[0]= a[0];
|
||||
r[1]= a[1];
|
||||
r[2]= a[2];
|
||||
}
|
||||
|
||||
/********************************* Arithmetic ********************************/
|
||||
|
||||
void add_v2_v2(float *r, float *a)
|
||||
{
|
||||
r[0] += a[0];
|
||||
r[1] += a[1];
|
||||
}
|
||||
|
||||
void add_v2_v2v2(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] + b[0];
|
||||
r[1]= a[1] + b[1];
|
||||
}
|
||||
|
||||
void add_v3_v3(float *r, float *a)
|
||||
{
|
||||
r[0] += a[0];
|
||||
r[1] += a[1];
|
||||
r[1] += a[1];
|
||||
}
|
||||
|
||||
void add_v3_v3v3(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] + b[0];
|
||||
r[1]= a[1] + b[1];
|
||||
r[2]= a[2] + b[2];
|
||||
}
|
||||
|
||||
void sub_v2_v2(float *r, float *a)
|
||||
{
|
||||
r[0] -= a[0];
|
||||
r[1] -= a[1];
|
||||
}
|
||||
|
||||
void sub_v2_v2v2(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] - b[0];
|
||||
r[1]= a[1] - b[1];
|
||||
}
|
||||
|
||||
void sub_v3_v3(float *r, float *a)
|
||||
{
|
||||
r[0] -= a[0];
|
||||
r[1] -= a[1];
|
||||
r[1] -= a[1];
|
||||
}
|
||||
|
||||
void sub_v3_v3v3(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] - b[0];
|
||||
r[1]= a[1] - b[1];
|
||||
r[2]= a[2] - b[2];
|
||||
}
|
||||
|
||||
void mul_v2_fl(float *v1, float f)
|
||||
{
|
||||
v1[0]*= f;
|
||||
v1[1]*= f;
|
||||
}
|
||||
|
||||
void mul_v3_fl(float r[3], float f)
|
||||
{
|
||||
r[0] *= f;
|
||||
r[1] *= f;
|
||||
r[2] *= f;
|
||||
}
|
||||
|
||||
void mul_v3_v3fl(float r[3], float a[3], float f)
|
||||
{
|
||||
r[0]= a[0]*f;
|
||||
r[1]= a[1]*f;
|
||||
r[2]= a[2]*f;
|
||||
}
|
||||
|
||||
void mul_v3_v3(float r[3], float a[3])
|
||||
{
|
||||
r[0] *= a[0];
|
||||
r[1] *= a[1];
|
||||
r[2] *= a[2];
|
||||
}
|
||||
|
||||
void mul_v3_v3v3(float *v, float *v1, float *v2)
|
||||
{
|
||||
v[0] = v1[0] * v2[0];
|
||||
v[1] = v1[1] * v2[1];
|
||||
v[2] = v1[2] * v2[2];
|
||||
}
|
||||
|
||||
void negate_v3(float r[3])
|
||||
{
|
||||
r[0]= -r[0];
|
||||
r[1]= -r[1];
|
||||
r[2]= -r[2];
|
||||
}
|
||||
|
||||
void negate_v3_v3(float r[3], float a[3])
|
||||
{
|
||||
r[0]= -a[0];
|
||||
r[1]= -a[1];
|
||||
r[2]= -a[2];
|
||||
}
|
||||
|
||||
float dot_v2v2(float *a, float *b)
|
||||
{
|
||||
return a[0]*b[0] + a[1]*b[1];
|
||||
}
|
||||
|
||||
float dot_v3v3(float a[3], float b[3])
|
||||
{
|
||||
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
|
||||
}
|
||||
|
||||
void cross_v3_v3v3(float r[3], float a[3], float b[3])
|
||||
{
|
||||
r[0]= a[1]*b[2] - a[2]*b[1];
|
||||
r[1]= a[2]*b[0] - a[0]*b[2];
|
||||
r[2]= a[0]*b[1] - a[1]*b[0];
|
||||
}
|
||||
|
||||
void star_m3_v3(float mat[][3], float *vec)
|
||||
{
|
||||
mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
|
||||
mat[0][1]= -vec[2];
|
||||
mat[0][2]= vec[1];
|
||||
mat[1][0]= vec[2];
|
||||
mat[1][2]= -vec[0];
|
||||
mat[2][0]= -vec[1];
|
||||
mat[2][1]= vec[0];
|
||||
|
||||
}
|
||||
|
||||
/*********************************** Length **********************************/
|
||||
|
||||
float len_v2(float *v)
|
||||
{
|
||||
return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
|
||||
}
|
||||
|
||||
float len_v2v2(float *v1, float *v2)
|
||||
{
|
||||
float x, y;
|
||||
|
||||
x = v1[0]-v2[0];
|
||||
y = v1[1]-v2[1];
|
||||
return (float)sqrt(x*x+y*y);
|
||||
}
|
||||
|
||||
float len_v3(float a[3])
|
||||
{
|
||||
return sqrtf(dot_v3v3(a, a));
|
||||
}
|
||||
|
||||
float len_v3v3(float a[3], float b[3])
|
||||
{
|
||||
float d[3];
|
||||
|
||||
sub_v3_v3v3(d, b, a);
|
||||
return len_v3(d);
|
||||
}
|
||||
|
||||
float normalize_v2(float n[2])
|
||||
{
|
||||
float d;
|
||||
|
||||
d= n[0]*n[0]+n[1]*n[1];
|
||||
|
||||
if(d>1.0e-35f) {
|
||||
d= (float)sqrt(d);
|
||||
n[0]/=d;
|
||||
n[1]/=d;
|
||||
} else {
|
||||
n[0]=n[1]= 0.0f;
|
||||
d= 0.0f;
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
float normalize_v3(float n[3])
|
||||
{
|
||||
float d= dot_v3v3(n, n);
|
||||
|
||||
/* a larger value causes normalize errors in a
|
||||
scaled down models with camera xtreme close */
|
||||
if(d > 1.0e-35f) {
|
||||
d= sqrtf(d);
|
||||
mul_v3_fl(n, 1.0f/d);
|
||||
}
|
||||
else {
|
||||
zero_v3(n);
|
||||
d= 0.0f;
|
||||
}
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
/******************************* Interpolation *******************************/
|
||||
|
||||
void interp_v2_v2v2(float *target, const float *a, const float *b, const float t)
|
||||
{
|
||||
const 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];
|
||||
@ -267,17 +44,15 @@ void interp_v2_v2v2(float *target, const float *a, const float *b, const float t
|
||||
|
||||
/* 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], float v1[2], float v2[2], float v3[2], 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, const float *a, const float *b, const float t)
|
||||
void interp_v3_v3v3(float *target, float *a, float *b, float t)
|
||||
{
|
||||
const 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];
|
||||
@ -286,7 +61,7 @@ void interp_v3_v3v3(float *target, const float *a, const float *b, const float t
|
||||
|
||||
/* 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], float v1[3], float v2[3], float v3[3], 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];
|
||||
@ -295,9 +70,9 @@ void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const fl
|
||||
|
||||
void mid_v3_v3v3(float *v, float *v1, float *v2)
|
||||
{
|
||||
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]);
|
||||
}
|
||||
|
||||
/********************************* Comparison ********************************/
|
||||
|
||||
258
source/blender/blenlib/intern/math_vector_inline.c
Normal file
258
source/blender/blenlib/intern/math_vector_inline.c
Normal file
@ -0,0 +1,258 @@
|
||||
/**
|
||||
* $Id$
|
||||
*
|
||||
* ***** 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, 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 *****
|
||||
* */
|
||||
|
||||
#include "BLI_math.h"
|
||||
|
||||
#ifndef BLI_MATH_VECTOR_INLINE
|
||||
#define BLI_MATH_VECTOR_INLINE
|
||||
|
||||
/********************************** Init *************************************/
|
||||
|
||||
MINLINE void zero_v2(float r[2])
|
||||
{
|
||||
r[0]= 0.0f;
|
||||
r[1]= 0.0f;
|
||||
}
|
||||
|
||||
MINLINE void zero_v3(float r[3])
|
||||
{
|
||||
r[0]= 0.0f;
|
||||
r[1]= 0.0f;
|
||||
r[2]= 0.0f;
|
||||
}
|
||||
|
||||
MINLINE void copy_v2_v2(float r[2], float a[2])
|
||||
{
|
||||
r[0]= a[0];
|
||||
r[1]= a[1];
|
||||
}
|
||||
|
||||
MINLINE void copy_v3_v3(float r[3], float a[3])
|
||||
{
|
||||
r[0]= a[0];
|
||||
r[1]= a[1];
|
||||
r[2]= a[2];
|
||||
}
|
||||
|
||||
/********************************* Arithmetic ********************************/
|
||||
|
||||
MINLINE void add_v2_v2(float *r, float *a)
|
||||
{
|
||||
r[0] += a[0];
|
||||
r[1] += a[1];
|
||||
}
|
||||
|
||||
MINLINE void add_v2_v2v2(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] + b[0];
|
||||
r[1]= a[1] + b[1];
|
||||
}
|
||||
|
||||
MINLINE void add_v3_v3(float *r, float *a)
|
||||
{
|
||||
r[0] += a[0];
|
||||
r[1] += a[1];
|
||||
r[1] += a[1];
|
||||
}
|
||||
|
||||
MINLINE void add_v3_v3v3(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] + b[0];
|
||||
r[1]= a[1] + b[1];
|
||||
r[2]= a[2] + b[2];
|
||||
}
|
||||
|
||||
MINLINE void sub_v2_v2(float *r, float *a)
|
||||
{
|
||||
r[0] -= a[0];
|
||||
r[1] -= a[1];
|
||||
}
|
||||
|
||||
MINLINE void sub_v2_v2v2(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] - b[0];
|
||||
r[1]= a[1] - b[1];
|
||||
}
|
||||
|
||||
MINLINE void sub_v3_v3(float *r, float *a)
|
||||
{
|
||||
r[0] -= a[0];
|
||||
r[1] -= a[1];
|
||||
r[1] -= a[1];
|
||||
}
|
||||
|
||||
MINLINE void sub_v3_v3v3(float *r, float *a, float *b)
|
||||
{
|
||||
r[0]= a[0] - b[0];
|
||||
r[1]= a[1] - b[1];
|
||||
r[2]= a[2] - b[2];
|
||||
}
|
||||
|
||||
MINLINE void mul_v2_fl(float *v1, float f)
|
||||
{
|
||||
v1[0]*= f;
|
||||
v1[1]*= f;
|
||||
}
|
||||
|
||||
MINLINE void mul_v3_fl(float r[3], float f)
|
||||
{
|
||||
r[0] *= f;
|
||||
r[1] *= f;
|
||||
r[2] *= f;
|
||||
}
|
||||
|
||||
MINLINE void mul_v3_v3fl(float r[3], float a[3], float f)
|
||||
{
|
||||
r[0]= a[0]*f;
|
||||
r[1]= a[1]*f;
|
||||
r[2]= a[2]*f;
|
||||
}
|
||||
|
||||
MINLINE void mul_v3_v3(float r[3], float a[3])
|
||||
{
|
||||
r[0] *= a[0];
|
||||
r[1] *= a[1];
|
||||
r[2] *= a[2];
|
||||
}
|
||||
|
||||
MINLINE void mul_v3_v3v3(float *v, float *v1, float *v2)
|
||||
{
|
||||
v[0] = v1[0] * v2[0];
|
||||
v[1] = v1[1] * v2[1];
|
||||
v[2] = v1[2] * v2[2];
|
||||
}
|
||||
|
||||
MINLINE void negate_v3(float r[3])
|
||||
{
|
||||
r[0]= -r[0];
|
||||
r[1]= -r[1];
|
||||
r[2]= -r[2];
|
||||
}
|
||||
|
||||
MINLINE void negate_v3_v3(float r[3], float a[3])
|
||||
{
|
||||
r[0]= -a[0];
|
||||
r[1]= -a[1];
|
||||
r[2]= -a[2];
|
||||
}
|
||||
|
||||
MINLINE float dot_v2v2(float *a, float *b)
|
||||
{
|
||||
return a[0]*b[0] + a[1]*b[1];
|
||||
}
|
||||
|
||||
MINLINE float dot_v3v3(float a[3], float b[3])
|
||||
{
|
||||
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
|
||||
}
|
||||
|
||||
MINLINE float cross_v2v2(float a[2], float b[2])
|
||||
{
|
||||
return a[0]*b[1] - a[1]*b[0];
|
||||
}
|
||||
|
||||
MINLINE void cross_v3_v3v3(float r[3], float a[3], float b[3])
|
||||
{
|
||||
r[0]= a[1]*b[2] - a[2]*b[1];
|
||||
r[1]= a[2]*b[0] - a[0]*b[2];
|
||||
r[2]= a[0]*b[1] - a[1]*b[0];
|
||||
}
|
||||
|
||||
MINLINE void star_m3_v3(float mat[][3], float *vec)
|
||||
{
|
||||
mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
|
||||
mat[0][1]= -vec[2];
|
||||
mat[0][2]= vec[1];
|
||||
mat[1][0]= vec[2];
|
||||
mat[1][2]= -vec[0];
|
||||
mat[2][0]= -vec[1];
|
||||
mat[2][1]= vec[0];
|
||||
}
|
||||
|
||||
/*********************************** Length **********************************/
|
||||
|
||||
MINLINE float len_v2(float *v)
|
||||
{
|
||||
return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
|
||||
}
|
||||
|
||||
MINLINE float len_v2v2(float *v1, float *v2)
|
||||
{
|
||||
float x, y;
|
||||
|
||||
x = v1[0]-v2[0];
|
||||
y = v1[1]-v2[1];
|
||||
return (float)sqrt(x*x+y*y);
|
||||
}
|
||||
|
||||
MINLINE float len_v3(float a[3])
|
||||
{
|
||||
return sqrtf(dot_v3v3(a, a));
|
||||
}
|
||||
|
||||
MINLINE float len_v3v3(float a[3], float b[3])
|
||||
{
|
||||
float d[3];
|
||||
|
||||
sub_v3_v3v3(d, b, a);
|
||||
return len_v3(d);
|
||||
}
|
||||
|
||||
MINLINE float normalize_v2(float n[2])
|
||||
{
|
||||
float d= dot_v2v2(n, n);
|
||||
|
||||
if(d > 1.0e-35f) {
|
||||
d= sqrtf(d);
|
||||
mul_v2_fl(n, 1.0f/d);
|
||||
} else {
|
||||
zero_v2(n);
|
||||
d= 0.0f;
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
MINLINE float normalize_v3(float n[3])
|
||||
{
|
||||
float d= dot_v3v3(n, n);
|
||||
|
||||
/* a larger value causes normalize errors in a
|
||||
scaled down models with camera xtreme close */
|
||||
if(d > 1.0e-35f) {
|
||||
d= sqrtf(d);
|
||||
mul_v3_fl(n, 1.0f/d);
|
||||
}
|
||||
else {
|
||||
zero_v3(n);
|
||||
d= 0.0f;
|
||||
}
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
#endif /* BLI_MATH_VECTOR_INLINE */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user